MTC compatiblity fixes

moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h

@@ -81,10 +81,7 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase
                               const std::string& logger_name)
     : ActionBasedControllerHandleBase(name, logger_name), node_(node), done_(true), namespace_(ns)
   {
-    controller_action_client_ =
-        rclcpp_action::create_client<T>(node_->get_node_base_interface(), node_->get_node_graph_interface(),
-                                        node_->get_node_logging_interface(), node_->get_node_waitables_interface(),
-                                        getActionName());
+    controller_action_client_ = rclcpp_action::create_client<T>(node_, getActionName());
 
     unsigned int attempts = 0;
     double timeout;
@@ -139,19 +136,32 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase
     return true;
   }
 
+  virtual void
+  controllerDoneCallback(const typename rclcpp_action::ClientGoalHandle<T>::WrappedResult& wrapped_result) = 0;
+
   bool waitForExecution(const rclcpp::Duration& timeout = rclcpp::Duration(0)) override
   {
-    auto result_future = controller_action_client_->async_get_result(current_goal_);
+    std::promise<bool> result_callback_done;
+    auto result_future = controller_action_client_->async_get_result(
+        current_goal_, [this, &result_callback_done](const auto& wrapped_result) {
+          controllerDoneCallback(wrapped_result);
+          result_callback_done.set_value(true);
+        });
     if (timeout.seconds() == 0.0)
     {
       result_future.wait();
     }
     else
     {
-      std::future_status status = result_future.wait_for(timeout.to_chrono<std::chrono::seconds>());
+      std::future_status status = result_future.wait_for(timeout.to_chrono<std::chrono::duration<double>>());
       if (status == std::future_status::timeout)
+      {
         RCLCPP_WARN(LOGGER, "waitForExecution timed out");
+        return false;
+      }
     }
+    // To accommodate for the delay after the future for the result is ready and the time controllerDoneCallback takes to finish
+    result_callback_done.get_future().wait();
     return true;
   }
 
@@ -182,7 +192,7 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase
 
   void finishControllerExecution(const rclcpp_action::ResultCode& state)
   {
-    RCLCPP_DEBUG_STREAM(LOGGER, "Controller " << name_ << " is done with state " << static_cast<int8_t>(state));
+    RCLCPP_DEBUG_STREAM(LOGGER, "Controller " << name_ << " is done with state " << static_cast<int>(state));
     if (state == rclcpp_action::ResultCode::SUCCEEDED)
       last_exec_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
     else if (state == rclcpp_action::ResultCode::ABORTED)

moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h

@@ -69,7 +69,8 @@ class FollowJointTrajectoryControllerHandle
                                                          const std::string& name);
 
   void controllerDoneCallback(
-      const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::WrappedResult& wrapped_result);
+      const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::WrappedResult& wrapped_result)
+      override;
 
   control_msgs::action::FollowJointTrajectory::Goal goal_template_;
 };

moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h

@@ -139,9 +139,6 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
     send_goal_options.goal_response_callback = [this](const auto& /* unused-arg */) {
       RCLCPP_DEBUG_STREAM(LOGGER, name_ << " started execution");
     };
-    // Result callback
-    send_goal_options.result_callback =
-        std::bind(&GripperControllerHandle::controllerDoneCallback, this, std::placeholders::_1);
     // Send goal
     auto current_goal_future = controller_action_client_->async_send_goal(goal, send_goal_options);
     current_goal_ = current_goal_future.get();
@@ -181,7 +178,7 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
 
 private:
   void controllerDoneCallback(
-      const rclcpp_action::ClientGoalHandle<control_msgs::action::GripperCommand>::WrappedResult& wrapped_result)
+      const rclcpp_action::ClientGoalHandle<control_msgs::action::GripperCommand>::WrappedResult& wrapped_result) override
   {
     if (wrapped_result.code == rclcpp_action::ResultCode::ABORTED && allow_failure_)
       finishControllerExecution(rclcpp_action::ResultCode::SUCCEEDED);

moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp

@@ -71,9 +71,6 @@ bool FollowJointTrajectoryControllerHandle::sendTrajectory(const moveit_msgs::ms
     else
       RCLCPP_INFO(LOGGER, "Goal request accepted!");
   };
-  // Result callback
-  send_goal_options.result_callback =
-      std::bind(&FollowJointTrajectoryControllerHandle::controllerDoneCallback, this, _1);
 
   done_ = false;
   last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING;

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

@@ -276,12 +276,12 @@ bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(const rclc
 {
   rclcpp::Time start = node_->now();
   rclcpp::Duration elapsed(0, 0);
-  rclcpp::Duration timeout(wait_time, 0);
+  rclcpp::Duration timeout = rclcpp::Duration::from_seconds(wait_time);
 
   std::unique_lock<std::mutex> lock(state_update_lock_);
   while (current_state_time_ < t)
   {
-    state_update_condition_.wait_for(lock, (timeout - elapsed).to_chrono<std::chrono::seconds>());
+    state_update_condition_.wait_for(lock, (timeout - elapsed).to_chrono<std::chrono::duration<double>>());
     elapsed = node_->now() - start;
     if (elapsed > timeout)
     {

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

@@ -1471,7 +1471,7 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index)
 
       // expected duration is the duration of the longest part
       expected_trajectory_duration =
-          std::max(d * current_scaling + rclcpp::Duration(current_margin), expected_trajectory_duration);
+          std::max(d * current_scaling + rclcpp::Duration::from_seconds(current_margin), expected_trajectory_duration);
     }
 
     // construct a map from expected time to state index, for easy access to expected state location

moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp

@@ -49,7 +49,8 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.planning_sce
 class PlanningSceneInterface::PlanningSceneInterfaceImpl
 {
 public:
-  explicit PlanningSceneInterfaceImpl(const std::string& ns = "", bool wait = true) : node_(new rclcpp::Node(ns))
+  explicit PlanningSceneInterfaceImpl(const std::string& ns = "", bool wait = true)
+    : node_(new rclcpp::Node("planning_scene_interface_" + std::to_string(reinterpret_cast<std::size_t>(this)), ns))
   {
     planning_scene_diff_publisher_ = node_->create_publisher<moveit_msgs::msg::PlanningScene>("planning_scene", 1);
     planning_scene_service_ =

moveit_ros/visualization/CMakeLists.txt

@@ -79,6 +79,12 @@ pluginlib_export_plugin_description_file(rviz_common robot_state_rviz_plugin_des
 #  add_rostest(test/moveit_joy.test)
 #endif()
 
+ament_export_libraries(moveit_motion_planning_rviz_plugin_core)
+ament_export_libraries(moveit_planning_scene_rviz_plugin_core)
+ament_export_libraries(moveit_robot_state_rviz_plugin_core)
+ament_export_libraries(moveit_rviz_plugin_render_tools)
+ament_export_libraries(moveit_trajectory_rviz_plugin_core)
+ament_export_include_directories(include)
 ament_export_dependencies(class_loader)
 ament_export_dependencies(geometric_shapes)
 ament_export_dependencies(interactive_markers)

moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt

@@ -61,8 +61,3 @@ install(TARGETS ${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin)
-
-ament_export_libraries(
-  ${MOVEIT_LIB_NAME}_core
-  ${MOVEIT_LIB_NAME}
-)

moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt

@@ -32,8 +32,3 @@ install(TARGETS ${MOVEIT_LIB_NAME}_core ${MOVEIT_LIB_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin)
-
-ament_export_libraries(
-    ${MOVEIT_LIB_NAME}_core
-    ${MOVEIT_LIB_NAME}
-)

moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt

@@ -34,8 +34,3 @@ install(TARGETS ${MOVEIT_LIB_NAME}_core ${MOVEIT_LIB_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin)
-
-ament_export_libraries(
-    ${MOVEIT_LIB_NAME}_core
-    ${MOVEIT_LIB_NAME}
-)

moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt

@@ -48,8 +48,3 @@ install(TARGETS ${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core
   ARCHIVE DESTINATION lib
   RUNTIME DESTINATION bin
 )
-
-ament_export_libraries(
-  ${MOVEIT_LIB_NAME}_core
-  ${MOVEIT_LIB_NAME}
-)