diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index 0a23ea8a..71084a3d 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -183,6 +183,14 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element impl_->robot_description_node_ = "robot_state_publisher"; // default } + // Read urdf from ros parameter server + std::string urdf_string; + urdf_string = impl_->getURDF(impl_->robot_description_); + if (urdf_string.empty()) { + RCLCPP_ERROR_STREAM(impl_->model_nh_->get_logger(), "An empty URDF was passed. Exiting."); + return; + } + // There's currently no direct way to set parameters to the plugin's node // So we have to parse the plugin file manually and set it to the node's context. auto rcl_context = impl_->model_nh_->get_node_base_interface()->get_context()->get_rcl_context(); @@ -202,6 +210,12 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element impl_->model_nh_->get_logger(), "No parameter file provided. Configuration might be wrong"); } + // set the robot description parameter + // to propagate it among controller manager and controllers + std::string rb_arg = std::string("robot_description:=") + urdf_string; + arguments.push_back(RCL_PARAM_FLAG); + arguments.push_back(rb_arg); + if (sdf->HasElement("ros")) { sdf = sdf->GetElement("ros"); @@ -257,13 +271,10 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element std::chrono::duration( impl_->parent_model_->GetWorld()->Physics()->GetMaxStepSize()))); - // Read urdf from ros parameter server then // setup actuators and mechanism control node. // This call will block if ROS is not properly initialized. - std::string urdf_string; std::vector control_hardware_info; try { - urdf_string = impl_->getURDF(impl_->robot_description_); control_hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf_string); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM( @@ -435,7 +446,7 @@ std::string GazeboRosControlPrivate::getURDF(std::string param_name) const RCLCPP_ERROR( model_nh_->get_logger(), "Interrupted while waiting for %s service. Exiting.", robot_description_node_.c_str()); - return 0; + return ""; } RCLCPP_ERROR( model_nh_->get_logger(), "%s service not available, waiting again...",