diff --git a/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp b/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp index a68e9c5..08b94b3 100644 --- a/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp +++ b/coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp @@ -222,6 +222,10 @@ TEST_F(IntegrationTest, CreateStringPublisher) auto response = std::make_shared(); request->topic_name = "/new_test_topic"; + request->qos_history = "KEEP_LAST"; + request->qos_queue = 10; + request->qos_reliability = "RELIABLE"; + request->qos_durability = "VOLATILE"; client->async_send_request( request, @@ -236,7 +240,7 @@ TEST_F(IntegrationTest, CreateStringPublisher) executor.spin_once(std::chrono::milliseconds(100)); } - ASSERT_EQ(response->success, false); + ASSERT_EQ(response->success, true); } TEST_F(IntegrationTest, DeleteStringPublisher) @@ -282,7 +286,7 @@ TEST_F(IntegrationTest, DeleteStringPublisher) executor.spin_once(std::chrono::milliseconds(100)); } - ASSERT_EQ(response_create->success, false); + ASSERT_EQ(response_create->success, true); auto request_delete = std::make_shared(); @@ -309,7 +313,7 @@ TEST_F(IntegrationTest, DeleteStringPublisher) executor.spin_once(std::chrono::milliseconds(100)); } - ASSERT_EQ(response_delete->success, false); + ASSERT_EQ(response_delete->success, true); } TEST_F(IntegrationTest, CreateTwistSubscription) @@ -355,7 +359,7 @@ TEST_F(IntegrationTest, CreateTwistSubscription) executor.spin_once(std::chrono::milliseconds(100)); } - ASSERT_EQ(response->success, false); + ASSERT_EQ(response->success, true); } TEST_F(IntegrationTest, DeleteTwistSubscription) @@ -401,7 +405,7 @@ TEST_F(IntegrationTest, DeleteTwistSubscription) executor.spin_once(std::chrono::milliseconds(100)); } - ASSERT_EQ(response_create->success, false); + ASSERT_EQ(response_create->success, true); auto request_delete = std::make_shared(); @@ -428,7 +432,42 @@ TEST_F(IntegrationTest, DeleteTwistSubscription) executor.spin_once(std::chrono::milliseconds(100)); } - ASSERT_EQ(response_delete->success, false); + ASSERT_EQ(response_delete->success, true); +} + +TEST_F(IntegrationTest, GetStatus) +{ + rclcpp::executors::SingleThreadedExecutor executor; + + auto node = + std::make_shared>( + rclcpp::NodeOptions().append_parameter_override( + "topic", + "/test_topic").append_parameter_override( + "topic_type", "std_msgs::msg::String"). + append_parameter_override("type", "Producer")); + + auto node_sub = rclcpp::Node::make_shared("node_sub"); + + executor.add_node(node->get_node_base_interface()); + executor.add_node(node_sub->get_node_base_interface()); + + auto node_info = std::make_shared(); + + auto sub = node_sub->create_subscription( + "/status", 10, + [&node_info](const coresense_instrumentation_interfaces::msg::NodeInfo::SharedPtr msg) { + node_info = std::move(msg); + }); + + for (int i = 0; i < 30; i++) { + executor.spin_once(std::chrono::milliseconds(100)); + } + + ASSERT_EQ(node_info->node_name, "lifecycle_node"); + ASSERT_EQ(node_info->type, "Producer"); + ASSERT_EQ(node_info->state, 1); + ASSERT_EQ(node_info->type_msg, "std_msgs::msg::String"); } TEST_F(IntegrationTest, GetTopic)