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..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 @@ -165,6 +166,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, 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?"); + } + 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 +190,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_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);