From ec1b95893fa933cb3e2cc5341bb65dd621645785 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?=
 <christophfroehlich@users.noreply.github.com>
Date: Mon, 10 Jun 2024 09:47:51 +0200
Subject: [PATCH 1/2] Don't crash if a wrong config was detected (#324)

---
 gz_ros2_control/src/gz_ros2_control_plugin.cpp | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/gz_ros2_control/src/gz_ros2_control_plugin.cpp b/gz_ros2_control/src/gz_ros2_control_plugin.cpp
index 54c3bb7b..5980e9d5 100644
--- a/gz_ros2_control/src/gz_ros2_control_plugin.cpp
+++ b/gz_ros2_control/src/gz_ros2_control_plugin.cpp
@@ -235,6 +235,9 @@ GazeboSimROS2ControlPlugin::GazeboSimROS2ControlPlugin()
 GazeboSimROS2ControlPlugin::~GazeboSimROS2ControlPlugin()
 {
   // Stop controller manager thread
+  if (!this->dataPtr->controller_manager_) {
+    return;
+  }
   this->dataPtr->executor_->remove_node(this->dataPtr->controller_manager_);
   this->dataPtr->executor_->cancel();
   this->dataPtr->thread_executor_spin_.join();
@@ -514,6 +517,9 @@ void GazeboSimROS2ControlPlugin::PreUpdate(
   const sim::UpdateInfo & _info,
   sim::EntityComponentManager & /*_ecm*/)
 {
+  if (!this->dataPtr->controller_manager_) {
+    return;
+  }
   static bool warned{false};
   if (!warned) {
     rclcpp::Duration gazebo_period(_info.dt);
@@ -548,6 +554,9 @@ void GazeboSimROS2ControlPlugin::PostUpdate(
   const sim::UpdateInfo & _info,
   const sim::EntityComponentManager & /*_ecm*/)
 {
+  if (!this->dataPtr->controller_manager_) {
+    return;
+  }
   // Get the simulation time and period
   rclcpp::Time sim_time_ros(std::chrono::duration_cast<std::chrono::nanoseconds>(
       _info.simTime).count(), RCL_ROS_TIME);

From 5d2d5eb6c867875c3c8d00a03cd472eac176e67c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?=
 <christophfroehlich@users.noreply.github.com>
Date: Mon, 10 Jun 2024 09:50:39 +0200
Subject: [PATCH 2/2] 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<sim::components::JointVelocity>(
@@ -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<sim::components::JointVelocityCmd>(
           this->dataPtr->joints_[i].sim_joint))