diff --git a/tracetools_test/src/test_ping.cpp b/tracetools_test/src/test_ping.cpp index 4f590a9..5a1cafa 100644 --- a/tracetools_test/src/test_ping.cpp +++ b/tracetools_test/src/test_ping.cpp @@ -27,8 +27,8 @@ using namespace std::chrono_literals; class PingNode : public rclcpp::Node { public: - explicit PingNode(rclcpp::NodeOptions options) - : Node(NODE_NAME, options) + PingNode(rclcpp::NodeOptions options, bool do_only_one) + : Node(NODE_NAME, options), do_only_one_(do_only_one) { sub_ = this->create_subscription( SUB_TOPIC_NAME, @@ -42,11 +42,16 @@ public: std::bind(&PingNode::timer_callback, this)); } + explicit PingNode(rclcpp::NodeOptions options) + : PingNode(options, true) {} + private: void callback(const std_msgs::msg::String::SharedPtr msg) { RCLCPP_INFO(this->get_logger(), "[output] %s", msg->data.c_str()); - rclcpp::shutdown(); + if (do_only_one_) { + rclcpp::shutdown(); + } } void timer_callback() @@ -59,14 +64,17 @@ private: rclcpp::Subscription::SharedPtr sub_; rclcpp::Publisher::SharedPtr pub_; rclcpp::TimerBase::SharedPtr timer_; + bool do_only_one_; }; int main(int argc, char * argv[]) { + bool do_only_one = argc == 1; + rclcpp::init(argc, argv); rclcpp::executors::SingleThreadedExecutor exec; - auto ping_node = std::make_shared(rclcpp::NodeOptions()); + auto ping_node = std::make_shared(rclcpp::NodeOptions(), do_only_one); exec.add_node(ping_node); printf("spinning\n"); diff --git a/tracetools_test/src/test_pong.cpp b/tracetools_test/src/test_pong.cpp index 23e2312..f1d52b8 100644 --- a/tracetools_test/src/test_pong.cpp +++ b/tracetools_test/src/test_pong.cpp @@ -24,8 +24,8 @@ class PongNode : public rclcpp::Node { public: - explicit PongNode(rclcpp::NodeOptions options) - : Node(NODE_NAME, options) + PongNode(rclcpp::NodeOptions options, bool do_only_one) + : Node(NODE_NAME, options), do_only_one_(do_only_one) { sub_ = this->create_subscription( SUB_TOPIC_NAME, @@ -36,6 +36,9 @@ public: rclcpp::QoS(10)); } + explicit PongNode(rclcpp::NodeOptions options) + : PongNode(options, true) {} + private: void callback(const std_msgs::msg::String::SharedPtr msg) { @@ -43,19 +46,24 @@ private: auto next_msg = std::make_shared(); next_msg->data = "some random pong string"; pub_->publish(*next_msg); - rclcpp::shutdown(); + if (do_only_one_) { + rclcpp::shutdown(); + } } rclcpp::Subscription::SharedPtr sub_; rclcpp::Publisher::SharedPtr pub_; + bool do_only_one_; }; int main(int argc, char * argv[]) { + bool do_only_one = argc == 1; + rclcpp::init(argc, argv); rclcpp::executors::SingleThreadedExecutor exec; - auto pong_node = std::make_shared(rclcpp::NodeOptions()); + auto pong_node = std::make_shared(rclcpp::NodeOptions(), do_only_one); exec.add_node(pong_node); printf("spinning\n");