From 9d8d2d5fc366e91b65639ce15cf605e3d28e205b Mon Sep 17 00:00:00 2001
From: Tomasz Bialek <tbialek@ihmc.org>
Date: Fri, 10 Oct 2025 11:54:53 -0500
Subject: [PATCH 1/2] Add reset request subscription

---
 isaac_ros_foundationpose/package.xml            |  1 +
 .../src/foundationpose_selector_node.cpp        | 17 +++++++++++++++--
 2 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/isaac_ros_foundationpose/package.xml b/isaac_ros_foundationpose/package.xml
index f75e474..1168252 100644
--- a/isaac_ros_foundationpose/package.xml
+++ b/isaac_ros_foundationpose/package.xml
@@ -44,6 +44,7 @@
 
   <depend>message_filters</depend>
   <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>vision_msgs</depend>
   <depend>assimp</depend>
diff --git a/isaac_ros_foundationpose/src/foundationpose_selector_node.cpp b/isaac_ros_foundationpose/src/foundationpose_selector_node.cpp
index 2081cd9..4e52dc6 100644
--- a/isaac_ros_foundationpose/src/foundationpose_selector_node.cpp
+++ b/isaac_ros_foundationpose/src/foundationpose_selector_node.cpp
@@ -20,6 +20,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
 #include <rclcpp/time.hpp>
+#include <std_msgs/msg/empty.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
 
@@ -107,13 +108,18 @@ class Selector : public rclcpp::Node
       this->create_subscription<isaac_ros_tensor_list_interfaces::msg::TensorList>(
       "pose_estimation/pose_matrix_output", 1, std::bind(&Selector::poseResetCallback, this, _1));
 
+    // Create subscriber for reset requests
+    reset_request_sub_ =
+      this->create_subscription<std_msgs::msg::Empty>(
+      "reset", 1, std::bind(&Selector::resetRequestCallback, this, _1));
+
     // reset period in ms after which pose estimation will be triggered
     this->declare_parameter<int>("reset_period", 20000);
     this->get_parameter("reset_period", reset_period_);
 
     timer_ = this->create_wall_timer(
       std::chrono::milliseconds(reset_period_),
-      std::bind(&Selector::timerCallback, this));
+      std::bind(&Selector::resetCallback, this));
   }
 
   void selectionCallback(
@@ -159,7 +165,12 @@ class Selector : public rclcpp::Node
     state_ = kTracking;
   }
 
-  void timerCallback()
+  void resetRequestCallback(const std_msgs::msg::Empty::ConstSharedPtr & request_msg)
+  {
+    resetCallback();
+  }
+
+  void resetCallback()
   {
     std::unique_lock<std::mutex> lock(mutex_);
     state_ = State::kPoseEstimation;
@@ -198,6 +209,8 @@ class Selector : public rclcpp::Node
   rclcpp::Subscription<isaac_ros_tensor_list_interfaces::msg::TensorList>::SharedPtr
     pose_estimation_output_sub_;
 
+  rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr reset_request_sub_;
+
   enum State
   {
     kTracking,

From 715496d6265674d99f35a076b2bb6a7d4c753f8c Mon Sep 17 00:00:00 2001
From: Tomasz Bialek <tbialek@ihmc.org>
Date: Fri, 10 Oct 2025 12:34:23 -0500
Subject: [PATCH 2/2] Supress unused parameter warning

---
 isaac_ros_foundationpose/src/foundationpose_selector_node.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/isaac_ros_foundationpose/src/foundationpose_selector_node.cpp b/isaac_ros_foundationpose/src/foundationpose_selector_node.cpp
index 4e52dc6..ef002da 100644
--- a/isaac_ros_foundationpose/src/foundationpose_selector_node.cpp
+++ b/isaac_ros_foundationpose/src/foundationpose_selector_node.cpp
@@ -167,6 +167,8 @@ class Selector : public rclcpp::Node
 
   void resetRequestCallback(const std_msgs::msg::Empty::ConstSharedPtr & request_msg)
   {
+    (void) request_msg; // Supress unused parameter warning
+
     resetCallback();
   }