diff --git a/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.cpp b/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.cpp index 9b0a5c1967..9cb6e4a474 100644 --- a/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.cpp @@ -78,16 +78,14 @@ void GetUrdfService::initialize() res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; return; } - // Get robot description string - std::string full_urdf_string; - context_->moveit_cpp_->getNode()->get_parameter_or("robot_description", full_urdf_string, std::string("")); - // Check if string is empty + std::string full_urdf_string = + context_->moveit_cpp_->getPlanningSceneMonitor()->getRobotModelLoader()->getRDFLoader()->getURDFString(); + + // Check if robot description string is empty if (full_urdf_string.empty()) { - const auto error_string = - std::string("Couldn't load the urdf from parameter server. Is the '/robot_description' parameter " - "initialized?"); + const auto error_string = std::string("Couldn't get the robot description string from MoveItCpp"); RCLCPP_ERROR(getLogger(), "%s", error_string.c_str()); res->error_code.message = error_string; res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; diff --git a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h index 85d1fa28aa..cd89eda6a6 100644 --- a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h +++ b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h @@ -81,6 +81,12 @@ class RDFLoader return ros_name_; } + /** @brief Get the URDF string*/ + const std::string& getURDFString() const + { + return urdf_string_; + } + /** @brief Get the parsed URDF model*/ const urdf::ModelInterfaceSharedPtr& getURDF() const {