From 6d6f185870f768167b52aff309119cea2e925713 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 9 Apr 2018 21:41:34 +0200 Subject: [PATCH] feedback --- core/include/moveit/task_constructor/task.h | 4 ++-- core/src/stages/compute_ik.cpp | 8 ++++++-- core/src/stages/move_relative.cpp | 4 ++-- core/src/stages/move_to.cpp | 5 +++-- 4 files changed, 13 insertions(+), 8 deletions(-) diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 6ac3fced..db2bb8af 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -64,7 +64,7 @@ MOVEIT_CLASS_FORWARD(Task) */ class Task : protected WrapperBase { public: - // TODO: move into MoveIt! core + // +1 TODO: move into MoveIt! core static planning_pipeline::PlanningPipelinePtr createPlanner(const moveit::core::RobotModelConstPtr &model, const std::string &ns = "move_group", const std::string &planning_plugin_param_name = "planning_plugin", @@ -113,7 +113,7 @@ public: /// publish all top-level solutions void publishAllSolutions(bool wait = true); - // TODO: convenient access to arbitrary stage by name + // +1 TODO: convenient access to arbitrary stage by name. traverse hierarchy using / separator? /// access stage tree ContainerBase *stages(); const ContainerBase *stages() const; diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 7bfea6c6..bbd2ff81 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -116,7 +116,8 @@ typedef std::vector> IKSolutions; namespace { -// TODO: provide callback methods in PlanningScene class / probably not very useful here though... +// ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though... +// move into MoveIt! core bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, Eigen::Affine3d pose, const robot_model::LinkModel* link) { @@ -216,7 +217,8 @@ void ComputeIK::onNewSolution(const SolutionBase &s) assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator planning_scene::PlanningScenePtr sandbox_scene = s.start()->scene()->diff(); - // TODO: this should not be necessary in my opinion + // -1 TODO: this should not be necessary in my opinion: Why do you think so? + // It is, because the properties on the interface might change from call to call... // enforced initialization from interface ensures that new target_pose is read properties().performInitFrom(INTERFACE, s.start()->properties(), true); const auto& props = properties(); @@ -379,6 +381,8 @@ void ComputeIK::onNewSolution(const SolutionBase &s) } // TODO: magic constant should be a property instead ("current_seed_only", or equivalent) + // Yeah, you are right, these are two different semantic concepts: + // One could also have multiple IK solutions derived from the same seed if (!succeeded && max_ik_solutions == 1) break; // first and only attempt failed } diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 239b3ac4..efe1d5ce 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -241,12 +241,12 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning // add an arrow marker visualization_msgs::Marker m; - // TODO: make "marker" a common property of all stages + // +1 TODO: make "marker" a common property of all stages. However, I would stick with "marker_ns" m.ns = props.get("marker_ns"); if (!m.ns.empty()) { m.header.frame_id = scene->getPlanningFrame(); if (linear_norm > 1e-3) { - // TODO: arrow could be split into "valid" and "invalid" part + // +1 TODO: arrow could be split into "valid" and "invalid" part (as red cylinder) rviz_marker_tools::setColor(m.color, success ? rviz_marker_tools::LIME_GREEN : rviz_marker_tools::RED); rviz_marker_tools::makeArrow(m, linear_norm); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 78978caa..7b916f6f 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -48,7 +48,7 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan , planner_(planner) { auto& p = properties(); - p.declare("timeout", 10.0, "planning timeout"); // TODO: make this private in Stage + p.declare("timeout", 10.0, "planning timeout"); // TODO: make this a common property in Stage p.declare("group", "name of planning group"); p.declare("link", "", "link to move (default is tip of jmg)"); @@ -184,7 +184,8 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP return success; } -// TODO: move these as default implementation to PropagateEitherWay? +// -1 TODO: move these as default implementation to PropagateEitherWay? +// Essentially, here compute() is a class-specific worker function bool MoveTo::computeForward(const InterfaceState &from){ planning_scene::PlanningScenePtr to; SubTrajectory trajectory;