Add no-op stage

[captain-yoshi]

Generic passthrough propagator for storing user defined properties. Useful for post-planning purposes.

We can already setup custom hooks, via properties, in stages for pre/post behavior after planning a task. In my opinion, it is better to see the custom stage in the task tree instead of the previous/next stage properties field. Furthermore, it simplifies the task construction and the post-planning process.

[codecov[bot]]

Codecov Report

All modified and coverable lines are covered by tests :white_check_mark:

Project coverage is 59.09%. Comparing base (5720b83) to head (39e9022).

Additional details and impacted files

@@           Coverage Diff           @@
##           master     #534   +/-   ##
=======================================
  Coverage   59.09%   59.09%           
=======================================
  Files          90       90           
  Lines        8504     8504           
=======================================
  Hits         5025     5025           
  Misses       3479     3479           

:umbrella: View full report in Codecov by Sentry.
:loudspeaker: Have feedback on the report? Share it here.

[captain-yoshi]

I'm confused... You can add whatever properties you like. How would you add something that can't be simulated otherwise?

{
  auto stage = std::make_unique<moveit::task_constructor::stages::PropertyPassThrough>(stage_name);

  stage->setProperty("controller_name", controller_name);
  stage->setProperty("controller_goal", goal);
}

Actually does one of these names makes more sense ?

auto stage = std::make_unique<moveit::task_constructor::stages::NoOp>(stage_name);
// or
auto stage = std::make_unique<moveit::task_constructor::stages::NOP>(stage_name);
// or
auto stage = std::make_unique<moveit::task_constructor::stages::NOOP>(stage_name);
// or
auto stage = std::make_unique<moveit::task_constructor::stages::NoOperation>(stage_name);

[rhaschke]

I'm not sure about your aim yet. I guess you access the properties of the stage via the solution's creator member? Naming the stage NoOp would be more suitable I think.

[captain-yoshi]

I first get the solution from this

task.solutions().front()->toMsg(solution_msg, &task.introspection());

and convert the stage id to a stage pointer. I then get the properties that I want based on the controller name.

[captain-yoshi]

Naming the stage NoOp would be more suitable I think.

Made the changes. It will also remove potential ambiguity with the passthrough wrapper.

[captain-yoshi]

Can a WrapperBase be used on the NoOp stage ?

  auto serial = std::make_unique<moveit::task_constructor::SerialContainer>(stage_name);

  {
    auto stage = std::make_unique<moveit::task_constructor::stages::NoOp>("no-op");

    // If object_name is a robot link skip the filter, otherwise verify that the collision object is attached to the robot
    auto applicability_filter = std::make_unique<stages::PredicateFilter>("applicability test", std::move(stage));
    applicability_filter->setPredicate([from_object_name](const SolutionBase& s, std::string& comment) {
      return true;
    });
    serial->insert(std::move(applicability_filter));
  }

I get a segfault :

Thread 1 "insert_tip_to_p" received signal SIGSEGV, Segmentation fault.
0x00007ffff797b832 in std::__shared_ptr<moveit::core::RobotState, (__gnu_cxx::_Lock_policy)2>::operator bool (this=0x50) at /usr/include/c++/9/bits/shared_ptr_base.h:1313
1313	      { return _M_ptr == 0 ? false : true; }
(gdb) bt
#0  0x00007ffff797b832 in std::__shared_ptr<moveit::core::RobotState, (__gnu_cxx::_Lock_policy)2>::operator bool (this=0x50) at /usr/include/c++/9/bits/shared_ptr_base.h:1313
#1  0x00007ffff7978b8e in planning_scene::PlanningScene::getCurrentState (this=0x0)
    at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h:232
#2  0x00007ffff7a1a323 in moveit::task_constructor::ensureUpdated (scene=std::shared_ptr<planning_scene::PlanningScene> (empty) = {...})
    at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/storage.cpp:51
#3  0x00007ffff7a1a3a1 in moveit::task_constructor::InterfaceState::InterfaceState (this=0x7fffffff9870, ps=std::shared_ptr<planning_scene::PlanningScene> (empty) = {...})
    at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/storage.cpp:56
#4  0x00007ffff7a0f834 in moveit::task_constructor::PropagatingEitherWay::computeGeneric<(moveit::task_constructor::Interface::Direction)1> (this=0x555555e04010, start=...)
    at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/stage.cpp:643
#5  0x00007ffff7a095fb in moveit::task_constructor::PropagatingEitherWay::computeBackward (this=0x555555e04010, to=...)
    at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/stage.cpp:632
#6  0x00007ffff7a09429 in moveit::task_constructor::PropagatingEitherWayPrivate::compute (this=0x555555dfc1e0) at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/stage.cpp:598
#7  0x00007ffff797694f in moveit::task_constructor::StagePrivate::runCompute (this=0x555555dfc1e0)
    at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:149
#8  0x00007ffff796d25e in moveit::task_constructor::WrapperBase::compute (this=0x555555e69010) at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/container.cpp:840
#9  0x00007ffff79687d9 in moveit::task_constructor::ContainerBasePrivate::compute (this=0x555555e36360) at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/container.cpp:163
#10 0x00007ffff797694f in moveit::task_constructor::StagePrivate::runCompute (this=0x555555e36360)
    at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:149
#11 0x00007ffff796c02f in moveit::task_constructor::SerialContainer::compute (this=0x555555e4ad60) at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/container.cpp:704
#12 0x00007ffff79687d9 in moveit::task_constructor::ContainerBasePrivate::compute (this=0x555555e03d30) at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/container.cpp:163
#13 0x00007ffff797694f in moveit::task_constructor::StagePrivate::runCompute (this=0x555555e03d30)
    at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:149
#14 0x00007ffff796c02f in moveit::task_constructor::SerialContainer::compute (this=0x5555555dc4a0) at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/container.cpp:704
#15 0x00007ffff79687d9 in moveit::task_constructor::ContainerBasePrivate::compute (this=0x5555555dc8e0) at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/container.cpp:163
#16 0x00007ffff797694f in moveit::task_constructor::StagePrivate::runCompute (this=0x5555555dc8e0)
    at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:149
#17 0x00007ffff7a25628 in moveit::task_constructor::Task::compute (this=0x7fffffffa670) at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/task.cpp:252
#18 0x00007ffff7a2581b in moveit::task_constructor::Task::plan (this=0x7fffffffa670, max_solutions=5) at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/task.cpp:275
#19 0x0000555555566114 in main ()
(gdb) quit
A debugging session is active.

[rhaschke]

@captain-yoshi, your NoOp implementation was missing the creation of the target planning scene :wink: