From e9e7758b8f26cdb7837aca1df5cbb7f307343523 Mon Sep 17 00:00:00 2001 From: Aravindh Date: Sun, 22 Mar 2026 02:02:54 +0530 Subject: [PATCH 1/2] Fix race condition in service client initialization by waiting for service availability Service calls could occur before the corresponding server was available, leading to intermittent failures in distributed setups. This change ensures the service is available before making the call. --- .../src/move_group_interface.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index db04772600..b087ceb962 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -165,6 +165,16 @@ class MoveGroupInterface::MoveGroupInterfaceImpl execute_action_client_ = rclcpp_action::create_client( node_, rclcpp::names::append(opt_.move_group_namespace_, move_group::EXECUTE_ACTION_NAME), callback_group_); execute_action_client_->wait_for_action_server(wait_for_servers.to_chrono>()); + + // Function to check if the service server is available + auto wait_for_service = [this](auto& client, const std::string& service_name) { + if (!client->wait_for_service(std::chrono::seconds(10))) + { + throw std::runtime_error("Service server not available for service: " + service_name + + ". Is move_group running?"); + } + RCLCPP_INFO(node_->get_logger(), "Service '%s' found.", service_name.c_str()); + }; query_service_ = node_->create_client( rclcpp::names::append(opt_.move_group_namespace_, move_group::QUERY_PLANNERS_SERVICE_NAME), @@ -179,6 +189,12 @@ class MoveGroupInterface::MoveGroupInterfaceImpl cartesian_path_service_ = node_->create_client( rclcpp::names::append(opt_.move_group_namespace_, move_group::CARTESIAN_PATH_SERVICE_NAME), rmw_qos_profile_services_default, callback_group_); + + // Check if service servers are available for all the clients + wait_for_service(query_service_, move_group::QUERY_PLANNERS_SERVICE_NAME); + wait_for_service(get_params_service_, move_group::GET_PLANNER_PARAMS_SERVICE_NAME); + wait_for_service(set_params_service_, move_group::SET_PLANNER_PARAMS_SERVICE_NAME); + wait_for_service(cartesian_path_service_, move_group::CARTESIAN_PATH_SERVICE_NAME); // plan_grasps_service_ = pnode_->create_client(GRASP_PLANNING_SERVICE_NAME); From 9ae86c559dcf50acd3e25de2ef4f8ce3f4699941 Mon Sep 17 00:00:00 2001 From: Aravindh Date: Thu, 2 Apr 2026 00:04:00 +0530 Subject: [PATCH 2/2] Added another argument in constructor call of MoveGroupInterfaceImpl to include a rclcpp::Duration for wait_for_service delay time, which can be configured at the place of construction of MoveGroupInterfaceImpl object --- .../src/move_group_interface.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index b087ceb962..d9231260c1 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -96,7 +96,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl public: MoveGroupInterfaceImpl(const rclcpp::Node::SharedPtr& node, const Options& opt, - const std::shared_ptr& tf_buffer, const rclcpp::Duration& wait_for_servers) + const std::shared_ptr& tf_buffer, const rclcpp::Duration& wait_for_servers, + const rclcpp::Duration wait_for_services = rclcpp::Duration(10,0)) : opt_(opt), node_(node), tf_buffer_(tf_buffer) { // We have no control on how the passed node is getting executed. To make sure MGI is functional, we're creating @@ -167,8 +168,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl execute_action_client_->wait_for_action_server(wait_for_servers.to_chrono>()); // Function to check if the service server is available - auto wait_for_service = [this](auto& client, const std::string& service_name) { - if (!client->wait_for_service(std::chrono::seconds(10))) + auto wait_for_service = [this](auto& client, const std::string& service_name, const rclcpp::Duration& duration) { + if (!client->wait_for_service(duration.to_chrono>())) { throw std::runtime_error("Service server not available for service: " + service_name + ". Is move_group running?"); @@ -191,10 +192,10 @@ class MoveGroupInterface::MoveGroupInterfaceImpl rmw_qos_profile_services_default, callback_group_); // Check if service servers are available for all the clients - wait_for_service(query_service_, move_group::QUERY_PLANNERS_SERVICE_NAME); - wait_for_service(get_params_service_, move_group::GET_PLANNER_PARAMS_SERVICE_NAME); - wait_for_service(set_params_service_, move_group::SET_PLANNER_PARAMS_SERVICE_NAME); - wait_for_service(cartesian_path_service_, move_group::CARTESIAN_PATH_SERVICE_NAME); + wait_for_service(query_service_, move_group::QUERY_PLANNERS_SERVICE_NAME, wait_for_services); + wait_for_service(get_params_service_, move_group::GET_PLANNER_PARAMS_SERVICE_NAME, wait_for_services); + wait_for_service(set_params_service_, move_group::SET_PLANNER_PARAMS_SERVICE_NAME, wait_for_services); + wait_for_service(cartesian_path_service_, move_group::CARTESIAN_PATH_SERVICE_NAME, wait_for_services); // plan_grasps_service_ = pnode_->create_client(GRASP_PLANNING_SERVICE_NAME);