MGI CallbackGroup doesn't trigger when running external Executor + Fast-DDS

[AndyZe]

Description

The OMPL constrained planning tutorial here no longer works.

I've traced it down to this:

  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);
  auto node = rclcpp::Node::make_shared("ompl_constrained_planning_demo_node", node_options);

  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  auto spinner = std::thread([&executor]() { executor.spin(); });
  
  moveit::planning_interface::MoveGroupInterface move_group_interface(node, "panda_arm");
  auto moveit_visual_tools =
      moveit_visual_tools::MoveItVisualTools{ node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC,
                                              move_group_interface.getRobotModel() };
      ...
   success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
   RCLCPP_INFO(LOGGER, "Plan with plane constraint %s", success ? "" : "FAILED");   // Never prints

The last line does not print. In other words, the mgi.plan() call is blocking. I suspect this has to do with conflicting callback groups.

I tried a multi-threaded executor as well as two separate nodes.

I'm suspicious that this PR caused the bug: #1305

[AndyZe]

Indeed, rolling back #1305 prevents the issue.

[henningkayser]

Does MoveGroup receive the planning call and does it return a result?

[AndyZe]

Yes, planning succeeds. I'm not sure if it tries to return a value or not. (If it does try to return a value, it doesn't make it.) Easy to test with the tutorial

[henningkayser]

What do you mean with "parallel with 'other things'"? Running MGI requests from multiple threads?

[AndyZe]

moveit_visual_tools in this case. You can look at the linked tutorial code

[henningkayser]

Ok, this is odd. I could reproduce this very easily, and indeed changing executor or callback group settings wouldn't fix this. However, moving MGI initialization before the SingleThreadedExecutor does the trick for me:

  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);
  auto node = rclcpp::Node::make_shared("ompl_constrained_planning_demo_node", node_options);
  
  moveit::planning_interface::MoveGroupInterface move_group_interface(node, "panda_arm");

  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  auto spinner = std::thread([&executor]() { executor.spin(); });
  
  auto moveit_visual_tools =
      moveit_visual_tools::MoveItVisualTools{ node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC,
                                              move_group_interface.getRobotModel() };
      ...
   success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
   RCLCPP_INFO(LOGGER, "Plan with plane constraint %s", success ? "" : "FAILED");   // Never prints

This looks like some rmw/CallbackGroup bug to me, possibly related to ros2/rclcpp#1611.
The code in MGI looks correct to me, otherwise.

[BrettHemes-3M]

I just encountered this yesterday as well... My ROS Answers post here from earlier today for reference.

[BrettHemes-3M]

However, moving MGI initialization before the SingleThreadedExecutor does the trick for me

I can confirm this works in my application as well.

[HammadB]

However, moving MGI initialization before the SingleThreadedExecutor does the trick for me

This works for me as well

[henningkayser]

Thanks @mikeferguson, I just confirmed that everything is fine when using Cyclone DDS (link for anyone who wants to reproduce). Was this the issue you were referring to? The bug has hopefully been fixed already with ros2/rmw_fastrtps#619, we should check that.

[AbduEhab]

However, moving MGI initialization before the SingleThreadedExecutor does the trick for me.

This does seem to fix the issue for me too, has it been changed in the documentation yet?