From 7ee291ab13adba52ab5889deb9e520009fe2283d Mon Sep 17 00:00:00 2001
From: Karanbir Chahal <kchahal@nvidia.com>
Date: Mon, 22 Sep 2025 15:41:21 -0700
Subject: [PATCH] Changes to enable RL workflow in Isaac Sim

---
 include/topic_based_ros2_control/topic_based_system.hpp | 5 ++++-
 src/topic_based_system.cpp                              | 9 +++++++++
 2 files changed, 13 insertions(+), 1 deletion(-)

diff --git a/include/topic_based_ros2_control/topic_based_system.hpp b/include/topic_based_ros2_control/topic_based_system.hpp
index cf67d61..9b32065 100644
--- a/include/topic_based_ros2_control/topic_based_system.hpp
+++ b/include/topic_based_ros2_control/topic_based_system.hpp
@@ -45,7 +45,7 @@
 #include <rclcpp/node.hpp>
 #include <rclcpp/publisher.hpp>
 #include <rclcpp/subscription.hpp>
-
+#include <std_msgs/msg/bool.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
 
 namespace topic_based_ros2_control
@@ -68,8 +68,11 @@ class TopicBasedSystem : public hardware_interface::SystemInterface
 private:
   rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr topic_based_joint_states_subscriber_;
   rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr topic_based_joint_commands_publisher_;
+  // Add a new subscriber for a signal whether to stop publishing joint commands.
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr stop_joint_commands_subscriber_;
   rclcpp::Node::SharedPtr node_;
   sensor_msgs::msg::JointState latest_joint_state_;
+  bool stop_joint_commands_{ false };
   bool sum_wrapped_joint_states_{ false };
 
   /// Use standard interfaces for joints because they are relevant for dynamic behavior
diff --git a/src/topic_based_system.cpp b/src/topic_based_system.cpp
index 67a9ec9..39798d8 100644
--- a/src/topic_based_system.cpp
+++ b/src/topic_based_system.cpp
@@ -151,6 +151,10 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo&
       get_hardware_parameter("joint_states_topic", "/robot_joint_states"), rclcpp::SensorDataQoS(),
       [this](const sensor_msgs::msg::JointState::SharedPtr joint_state) { latest_joint_state_ = *joint_state; });
 
+  stop_joint_commands_subscriber_ = node_->create_subscription<std_msgs::msg::Bool>(
+      get_hardware_parameter("stop_joint_commands_topic", "/stop_joint_commands"), rclcpp::QoS(1),
+      [this](const std_msgs::msg::Bool::SharedPtr stop_joint_commands) { stop_joint_commands_ = (*stop_joint_commands).data; });
+
   // if the values on the `joint_states_topic` are wrapped between -2*pi and 2*pi (like they are in Isaac Sim)
   // sum the total joint rotation returned on the `joint_states_` interface
   if (get_hardware_parameter("sum_wrapped_joint_states", "false") == "true")
@@ -276,6 +280,11 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
     return hardware_interface::return_type::OK;
   }
 
+  if (stop_joint_commands_)
+  {
+    return hardware_interface::return_type::OK;
+  }
+
   sensor_msgs::msg::JointState joint_state;
   for (std::size_t i = 0; i < info_.joints.size(); ++i)
   {