From 37d48b2dfbd290ab3e07cdec0ca058284b018153 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 10 Jun 2024 09:48:45 +0200 Subject: [PATCH] Harden behavior if a joint is not found in the model (backport #325) (#326) * Don't crash if a joint does not exist --- ign_ros2_control/src/ign_system.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 4cc755c6..3c057514 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -204,6 +204,14 @@ bool IgnitionSystem::initSim( auto & joint_info = hardware_info.joints[j]; std::string joint_name = this->dataPtr->joints_[j].name = joint_info.name; + auto it = enableJoints.find(joint_name); + if (it == 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; + } + ignition::gazebo::Entity simjoint = enableJoints[joint_name]; this->dataPtr->joints_[j].sim_joint = simjoint; @@ -521,6 +529,10 @@ hardware_interface::return_type IgnitionSystem::read( const rclcpp::Duration & /*period*/) { for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { + if (this->dataPtr->joints_[i].sim_joint == ignition::gazebo::v6::kNullEntity) { + continue; + } + // Get the joint velocity const auto * jointVelocity = this->dataPtr->ecm->Component( @@ -613,6 +625,10 @@ hardware_interface::return_type IgnitionSystem::write( const rclcpp::Duration & /*period*/) { for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { + if (this->dataPtr->joints_[i].sim_joint == ignition::gazebo::v6::kNullEntity) { + continue; + } + if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { if (!this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint))