Add hybrid planning

[AndyZe]

This continues from #488. It addresses most of the comments there.

The big comment that's not addressed yet relates to the timer that is used in launching hybrid_planning_manager/hybrid_planning_manager.cpp. I made an issue for that. #811 and #763 tried to tackle that issue but they both crash. So I think it's something we can work on improving later.

I'll work on cleaning up the corresponding tutorial PR and adding some tests for this.

Suggestions on how to clean up the commit history are welcome. I'd be fine with squashing into five commits or so.

This work is sponsored by RE2 Robotics.

[codecov]

Codecov Report

Merging #813 (5cd1a5f) into main (482950a) will decrease coverage by 3.07%.
The diff coverage is 27.86%.

@@            Coverage Diff             @@
##             main     #813      +/-   ##
==========================================
- Coverage   60.86%   57.79%   -3.06%     
==========================================
  Files         273      305      +32     
  Lines       23880    26060    +2180     
==========================================
+ Hits        14533    15060     +527     
- Misses       9347    11000    +1653     

Impacted Files	Coverage Δ
...rid_planning_manager/moveit_error_code_interface.h	0.00% <0.00%> (ø)
...ogic_plugins/src/replan_invalidated_trajectory.cpp	0.00% <0.00%> (ø)
.../hybrid_planning_manager/planner_logic_interface.h	3.85% <3.85%> (ø)
...lanner_logic_plugins/src/single_plan_execution.cpp	9.38% <9.38%> (ø)
...nstraint_solver_plugins/src/forward_trajectory.cpp	14.04% <14.04%> (ø)
..._manager_component/src/hybrid_planning_manager.cpp	20.84% <20.84%> (ø)
...trajectory_operator_plugins/src/simple_sampler.cpp	23.34% <23.34%> (ø)
..._planner_component/src/local_planner_component.cpp	33.11% <33.11%> (ø)
...l_planner_plugins/src/moveit_planning_pipeline.cpp	35.56% <35.56%> (ø)
...planner_component/src/global_planner_component.cpp	38.64% <38.64%> (ø)
... and 36 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 482950a...5cd1a5f. Read the comment docs.

[AndyZe]

@sjahr I would appreciate it if you take a look at c9e1691. The logic for the action result isn't working here because reaction_result always reflects a SUCCESS.

I'm not really sure what it's intended to do. These are the lines:

  // Add result callback to print the result
  local_goal_options.result_callback =
      [this](const rclcpp_action::ClientGoalHandle<moveit_msgs::action::LocalPlanner>::WrappedResult& action_result) {
        RCLCPP_ERROR(LOGGER, "local action callback");
        RCLCPP_ERROR_STREAM(LOGGER, (bool)(action_result.code == rclcpp_action::ResultCode::SUCCEEDED));

        // Note: action_result is not used beyond this point

        // Reaction result from the latest event
        ReactionResult reaction_result =
            planner_logic_instance_->react(HybridPlanningEvent::LOCAL_PLANNING_ACTION_FINISHED);
        // TODO: This always thinks the action was successful!
        if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
        {
          auto result = std::make_shared<moveit_msgs::action::HybridPlanner::Result>();
          result->error_code.val = reaction_result.error_code.val;
          result->error_message = reaction_result.error_message;
          hybrid_planning_goal_handle_->abort(result);
          RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to  '%s'", reaction_result.event.c_str());
        }
      };

If you run it a few times, I think you'll easily be able to see the issue. ros2 launch moveit_hybrid_planning hybrid_planning_demo.launch.py

[sjahr]

@sjahr I believe that f8b0cbd fixed my comment above about reaction_result always reflecting SUCCESS. Take a look if you have time.
@AndyZe I think this is not the correct way to fix it, sorry. I've answered in your review comment above.

[tylerjw]

I left some comments. I'll create a PR to try to fix those things.

[sjahr]

I left some comments. I'll create a PR to try to fix those things.

@tylerjw Thanks for reviewing this again! I am aware of this issue but I was not able to fix it yet. I've opened a WIP PR with my progress so far but I encountered an issue I have not fixed yet. If you have the time to take a look at this you can use my PR as a start and I'd be happy to hear your feedback on this approach or to review your own solution.

[tylerjw]

I tried fixing this in a PR to andy's branch and failed to fix it easily. I think it is important to acknowledge that one of the wonderful parts about software is that we can change it in the future and that nice software often takes many iterations to be really nice. In this case, I don't have the time to try to fix the issues I raised so I think we should just merge this as it is for now. Eventually, when one of us has time to work on this we should rewrite how the initialization of plugins work and accept that the plugin interface will probably have to change as a result.

[sjahr]

There are still some build warnings (check CI). They are not enforced yet, but it would be nice if they were fixed. Also, I noticed that the include folders are not installed locally. This is only apparent when depending on hybrid planning from another package. Here is an error log when compiling the hybrid_planning_demo from the UR10e welding repo. Since this worked with an older version of hybrid planning, there must be some newer changes that broke this.

I had the same issue and I've fixed it with this PR

[sjahr]

I re-read through all of this again. I think I'm ok with it for now. Eventually I want to figure out the initialization and lifetime of these plugins so there isn't a circular relationship.

Thanks! I think the key to solving this is to have a better solution for how hybrid_planning_manager and its logic plugin can share the same ros2 communication interfaces. When you have the time to work on it, I'd be happy to discuss the issue with you as I am struggling to find a good solution too

[AndyZe]

I am now getting a parameter loading error. This started to happen after I rebased. I think it is related to the loading of global_planner.yaml by moveit_planning_pipeline.cpp:

[component_container_mt-1] [FATAL] [1638568924.298387221] [moveit_ros.trajectory_execution_manager]: Exception while loading controller manager '<undefined>': According to the loaded plugin descriptions the class <undefined> with base class type moveit_controller_manager::MoveItControllerManager does not exist. Declared types are  moveit_ros_control_interface/MoveItControllerManager moveit_ros_control_interface/MoveItMultiControllerManager moveit_simple_controller_manager/MoveItSimpleControllerManager
[component_container_mt-1] [ERROR] [1638568924.298412408] [moveit_ros.trajectory_execution_manager]: Failed to reload controllers: `controller_manager_` does not exist.

A parameter dump for the global planner shows this:

/global_planner:
  ros__parameters:
    global_planner_name: moveit_hybrid_planning/MoveItPlanningPipeline
    hand:
      kinematics_solver: null
    moveit_controller_manager: <undefined>
    ...

[AndyZe]

Re. parameter loading:

moveit_controller_manager wasn't loaded because of a typo.

More importantly, all of these ompl parameters aren't declared or retrieved at all. No idea how this ever worked. I found this by comparing against the moveit_cpp demo. Maybe we can re-use the parameter loading code from moveit_cpp.

    ompl:
      panda_arm:
        planner_configs:
        - SBLkConfigDefault
        - ESTkConfigDefault
        - ...

[sjahr]

Re. parameter loading:

moveit_controller_manager wasn't loaded because of a typo.

More importantly, all of these ompl parameters aren't declared or retrieved at all. No idea how this ever worked. I found this by comparing against the moveit_cpp demo. Maybe we can re-use the parameter loading code from moveit_cpp.

    ompl:
      panda_arm:
        planner_configs:
        - SBLkConfigDefault
        - ESTkConfigDefault
        - ...

There is a default_planner_config within the ompl interface class which is used if nothing else is declared. For the simple demos, this was sufficient but you are right, that we need to add the option to pass ompl parameters. The MoveIt cpp demo just uses node_options.automatically_declare_parameters_from_overrides(true); which simply declares every parameter given to the node by yaml file but this is considered bad practice (although it could be an interim solution as the parameter handling best practice pattern is a MoveIt2 "global" issue). I think a good place to declare the ompl parameters would be in the ompl interface class because the MoveItPlanningPipeline global planner plugin should be agnostic to the MoveIt planner plugin used (i.e. the Fraunhofer demo uses the PILZ planner I think). @JafarAbdi and @tylerjw put some work into parameter handling, what do you think?

[AndyZe]

@sjahr I found the magic line that makes it all work again 👍

node->declare_parameter<std::string>("ompl.planning_plugin", "ompl_interface/OMPLPlanner");

I don't know why it was working previously without this.

Will make an issue about parameter loading. I think your idea to load the parameters from ompl is a good one. It's not just hybrid planning but many components of MoveIt that are having this issue. (#892)

[tylerjw]

I fixed the merge conflict but it might be better to rebase this commit on main so my merge commit goes away (as we are going to merge this and not squash it).