From 5d2d5eb6c867875c3c8d00a03cd472eac176e67c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 10 Jun 2024 09:50:39 +0200 Subject: [PATCH] Harden behavior if a joint is not found in the model (#325) * Don't crash if a joint does not exist --- gz_ros2_control/src/gz_system.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 363c37a1..0a82f50e 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -224,6 +224,14 @@ bool GazeboSimSystem::initSim( auto & joint_info = hardware_info.joints[j]; std::string joint_name = this->dataPtr->joints_[j].name = joint_info.name; + auto it_joint = enableJoints.find(joint_name); + if (it_joint == enableJoints.end()) { + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), "Skipping joint in the URDF named '" << joint_name << + "' which is not in the gazebo model."); + continue; + } + sim::Entity simjoint = enableJoints[joint_name]; this->dataPtr->joints_[j].sim_joint = simjoint; @@ -501,6 +509,10 @@ hardware_interface::return_type GazeboSimSystem::read( const rclcpp::Duration & /*period*/) { for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { + if (this->dataPtr->joints_[i].sim_joint == sim::kNullEntity) { + continue; + } + // Get the joint velocity const auto * jointVelocity = this->dataPtr->ecm->Component( @@ -593,6 +605,10 @@ hardware_interface::return_type GazeboSimSystem::write( const rclcpp::Duration & /*period*/) { for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { + if (this->dataPtr->joints_[i].sim_joint == sim::kNullEntity) { + continue; + } + if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { if (!this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint))