From a73268a7cd6f1102f711a03ae37c5fdb89cd3222 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 18 Aug 2023 11:01:04 +0000 Subject: [PATCH 1/2] Catch pluginlib exceptions --- .../src/gazebo_ros2_control_plugin.cpp | 25 ++++++++++++++++--- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index 49d85b7c..4cc8a230 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -288,10 +288,24 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element for (unsigned int i = 0; i < control_hardware_info.size(); i++) { std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_plugin_name; - auto gazeboSystem = std::unique_ptr( - impl_->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); - - rclcpp::Node::SharedPtr node_ros2 = std::dynamic_pointer_cast(impl_->model_nh_); + RCLCPP_DEBUG( + impl_->model_nh_->get_logger(), "Load hw interface %s ...", + robot_hw_sim_type_str_.c_str()); + std::unique_ptr gazeboSystem; + try { + gazeboSystem = std::unique_ptr( + impl_->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); + } catch (pluginlib::PluginlibException & ex) { + RCLCPP_ERROR( + impl_->model_nh_->get_logger(), "The plugin failed to load for some reason. Error: %s\n", + ex.what()); + continue; + } + rclcpp::Node::SharedPtr node_ros2 = std::dynamic_pointer_cast( + impl_->model_nh_); + RCLCPP_DEBUG( + impl_->model_nh_->get_logger(), "Loaded hw interface %s!", + robot_hw_sim_type_str_.c_str()); if (!gazeboSystem->initSim( node_ros2, impl_->parent_model_, @@ -302,6 +316,9 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element impl_->model_nh_->get_logger(), "Could not initialize robot simulation interface"); return; } + RCLCPP_DEBUG( + impl_->model_nh_->get_logger(), "Initialized robot simulation interface %s!", + robot_hw_sim_type_str_.c_str()); resource_manager_->import_component(std::move(gazeboSystem), control_hardware_info[i]); From ec902538688aeea94cdb1eef53f1a925b3ce85d3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 18 Aug 2023 13:35:12 +0200 Subject: [PATCH 2/2] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index 4cc8a230..b0a8bb07 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -289,7 +289,7 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element for (unsigned int i = 0; i < control_hardware_info.size(); i++) { std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_plugin_name; RCLCPP_DEBUG( - impl_->model_nh_->get_logger(), "Load hw interface %s ...", + impl_->model_nh_->get_logger(), "Load hardware interface %s ...", robot_hw_sim_type_str_.c_str()); std::unique_ptr gazeboSystem; try { @@ -304,7 +304,7 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element rclcpp::Node::SharedPtr node_ros2 = std::dynamic_pointer_cast( impl_->model_nh_); RCLCPP_DEBUG( - impl_->model_nh_->get_logger(), "Loaded hw interface %s!", + impl_->model_nh_->get_logger(), "Loaded hardware interface %s!", robot_hw_sim_type_str_.c_str()); if (!gazeboSystem->initSim( node_ros2,