This commit is contained in:
Robert Haschke 2018-04-09 21:41:34 +02:00
parent ade42456b8
commit 6d6f185870
4 changed files with 13 additions and 8 deletions

View File

@ -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;

View File

@ -116,7 +116,8 @@ typedef std::vector<std::vector<double>> 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
}

View File

@ -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<std::string>("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);

View File

@ -48,7 +48,7 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
, planner_(planner)
{
auto& p = properties();
p.declare<double>("timeout", 10.0, "planning timeout"); // TODO: make this private in Stage
p.declare<double>("timeout", 10.0, "planning timeout"); // TODO: make this a common property in Stage
p.declare<std::string>("group", "name of planning group");
p.declare<std::string>("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;