From ade42456b86b08593340862de0d88c3d6949f272 Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 23 Feb 2018 17:36:16 +0100 Subject: [PATCH] list of random todos --- core/include/moveit/task_constructor/task.h | 3 +++ core/src/stages/compute_ik.cpp | 3 +++ core/src/stages/move_relative.cpp | 2 ++ core/src/stages/move_to.cpp | 3 ++- 4 files changed, 10 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 04e754dc..6ac3fced 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -64,6 +64,7 @@ MOVEIT_CLASS_FORWARD(Task) */ class Task : protected WrapperBase { public: + // 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", @@ -81,6 +82,7 @@ public: /// load robot model from given parameter void loadRobotModel(const std::string& robot_description = "robot_description"); + // TODO: use Stage::insert as well? void add(Stage::pointer &&stage); void clear() override; @@ -111,6 +113,7 @@ public: /// publish all top-level solutions void publishAllSolutions(bool wait = true); + // TODO: convenient access to arbitrary stage by name /// 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 1281675e..7bfea6c6 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -116,6 +116,7 @@ typedef std::vector> IKSolutions; namespace { +// TODO: provide callback methods in PlanningScene class / probably not very useful here though... bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, Eigen::Affine3d pose, const robot_model::LinkModel* link) { @@ -215,6 +216,7 @@ 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 // enforced initialization from interface ensures that new target_pose is read properties().performInitFrom(INTERFACE, s.start()->properties(), true); const auto& props = properties(); @@ -376,6 +378,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s) spawn(InterfaceState(scene), std::move(solution)); } + // TODO: magic constant should be a property instead ("current_seed_only", or equivalent) 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 0f83286d..239b3ac4 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -241,10 +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 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 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 2c6b66d3..78978caa 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"); + p.declare("timeout", 10.0, "planning timeout"); // TODO: make this private in Stage p.declare("group", "name of planning group"); p.declare("link", "", "link to move (default is tip of jmg)"); @@ -184,6 +184,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP return success; } +// TODO: move these as default implementation to PropagateEitherWay? bool MoveTo::computeForward(const InterfaceState &from){ planning_scene::PlanningScenePtr to; SubTrajectory trajectory;