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 @@
message_filters
rclcpp
+ std_msgs
sensor_msgs
vision_msgs
assimp
diff --git a/isaac_ros_foundationpose/src/foundationpose_selector_node.cpp b/isaac_ros_foundationpose/src/foundationpose_selector_node.cpp
index 2081cd9..ef002da 100644
--- a/isaac_ros_foundationpose/src/foundationpose_selector_node.cpp
+++ b/isaac_ros_foundationpose/src/foundationpose_selector_node.cpp
@@ -20,6 +20,7 @@
#include
#include
#include
+#include
#include
#include
@@ -107,13 +108,18 @@ class Selector : public rclcpp::Node
this->create_subscription(
"pose_estimation/pose_matrix_output", 1, std::bind(&Selector::poseResetCallback, this, _1));
+ // Create subscriber for reset requests
+ reset_request_sub_ =
+ this->create_subscription(
+ "reset", 1, std::bind(&Selector::resetRequestCallback, this, _1));
+
// reset period in ms after which pose estimation will be triggered
this->declare_parameter("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,14 @@ class Selector : public rclcpp::Node
state_ = kTracking;
}
- void timerCallback()
+ void resetRequestCallback(const std_msgs::msg::Empty::ConstSharedPtr & request_msg)
+ {
+ (void) request_msg; // Supress unused parameter warning
+
+ resetCallback();
+ }
+
+ void resetCallback()
{
std::unique_lock lock(mutex_);
state_ = State::kPoseEstimation;
@@ -198,6 +211,8 @@ class Selector : public rclcpp::Node
rclcpp::Subscription::SharedPtr
pose_estimation_output_sub_;
+ rclcpp::Subscription::SharedPtr reset_request_sub_;
+
enum State
{
kTracking,