From ab98c40494699fe6898bf912e809ee9874715988 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 9 Jun 2024 20:53:27 +0000 Subject: [PATCH 1/3] Don't crash if a joint does not exist --- gz_ros2_control/src/gz_system.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 363c37a1..979d0a30 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 = 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; + } + 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 == ignition::gazebo::kNullEntity) { + continue; + } + // Get the joint velocity const auto * jointVelocity = this->dataPtr->ecm->Component( @@ -593,6 +605,11 @@ 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 == ignition::gazebo::kNullEntity) { + continue; + } + if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { if (!this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint)) From 313004a974d606a19f6862964226596d47c4e649 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 9 Jun 2024 21:00:35 +0000 Subject: [PATCH 2/3] Fix for rolling --- gz_ros2_control/src/gz_system.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 979d0a30..6f32b447 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -224,8 +224,8 @@ bool GazeboSimSystem::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()) { + 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."); @@ -509,7 +509,7 @@ 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 == ignition::gazebo::kNullEntity) { + if(this->dataPtr->joints_[i].sim_joint == sim::kNullEntity) { continue; } @@ -605,11 +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 == ignition::gazebo::kNullEntity) { + 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)) From 135f9fe80f4f940fdcbfb991ede4ad7efab57be4 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 9 Jun 2024 21:23:24 +0000 Subject: [PATCH 3/3] Fix linter --- gz_ros2_control/src/gz_system.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 6f32b447..0a82f50e 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -509,7 +509,7 @@ 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) { + if (this->dataPtr->joints_[i].sim_joint == sim::kNullEntity) { continue; } @@ -605,7 +605,7 @@ 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) { + if (this->dataPtr->joints_[i].sim_joint == sim::kNullEntity) { continue; }