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,