From c0a8f4679d118b960fb694253dda7437dbfe939b Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 10 Jan 2023 13:32:12 -0800 Subject: [PATCH] Wait on result Signed-off-by: methylDragon --- fuse_models/src/graph_ignition.cpp | 1 + fuse_models/src/unicycle_2d_ignition.cpp | 1 + fuse_models/test/test_graph_ignition.cpp | 10 ++++++---- fuse_models/test/test_unicycle_2d_ignition.cpp | 10 ++++++---- 4 files changed, 14 insertions(+), 8 deletions(-) diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp index 5a5bc7915..1c26d55a9 100644 --- a/fuse_models/src/graph_ignition.cpp +++ b/fuse_models/src/graph_ignition.cpp @@ -182,6 +182,7 @@ void GraphIgnition::process(const fuse_msgs::msg::SerializedGraph& msg) auto srv = std::make_shared(); // No need to spin since node is optimizer node, which should be spinning auto result_future = reset_client_->async_send_request(srv); + result_future.wait(); } // Now that the optimizer has been reset, actually send the initial state constraints to the optimizer diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp index f41131f67..d72bca8e0 100644 --- a/fuse_models/src/unicycle_2d_ignition.cpp +++ b/fuse_models/src/unicycle_2d_ignition.cpp @@ -274,6 +274,7 @@ void Unicycle2DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceSta auto srv = std::make_shared(); // No need to spin since node is optimizer node, which should be spinning auto result_future = reset_client_->async_send_request(srv); + result_future.wait(); } // Now that the pose has been validated and the optimizer has been reset, actually send the initial state constraints diff --git a/fuse_models/test/test_graph_ignition.cpp b/fuse_models/test/test_graph_ignition.cpp index 708893cb5..002d39640 100644 --- a/fuse_models/test/test_graph_ignition.cpp +++ b/fuse_models/test/test_graph_ignition.cpp @@ -222,7 +222,7 @@ TEST_F(GraphIgnitionTestFixture, SetGraphService) rclcpp::NodeOptions options; options.arguments({ "--ros-args", - "-p", "ignition_sensor.set_graph_service:=/set_graph", + "-p", "ignition_sensor.set_graph_service:=set_graph", "-p", "ignition_sensor.reset_service:=''"}); auto node = rclcpp::Node::make_shared("graph_ignition_test", options); executor_->add_node(node); @@ -266,7 +266,8 @@ TEST_F(GraphIgnitionTestFixture, SetGraphService) // Call the SetGraph service auto srv = std::make_shared(); fuse_core::serializeGraph(graph, srv->graph); - auto client = node->create_client("/set_graph"); + auto client = node->create_client("graph_ignition_test/set_graph"); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); auto result = client->async_send_request(srv); ASSERT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); EXPECT_TRUE(result.get()->success); @@ -337,7 +338,7 @@ TEST_F(GraphIgnitionTestFixture, SetGraphServiceWithStampedVariables) rclcpp::NodeOptions options; options.arguments({ "--ros-args", - "-p", "ignition_sensor.set_graph_service:=/set_graph", + "-p", "ignition_sensor.set_graph_service:=set_graph", "-p", "ignition_sensor.reset_service:=''"}); auto node = rclcpp::Node::make_shared("graph_ignition_test", options); executor_->add_node(node); @@ -381,7 +382,8 @@ TEST_F(GraphIgnitionTestFixture, SetGraphServiceWithStampedVariables) // Call the SetGraph service auto srv = std::make_shared(); fuse_core::serializeGraph(graph, srv->graph); - auto client = node->create_client("/set_graph"); + auto client = node->create_client("graph_ignition_test/set_graph"); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); auto result = client->async_send_request(srv); ASSERT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); EXPECT_TRUE(result.get()->success); diff --git a/fuse_models/test/test_unicycle_2d_ignition.cpp b/fuse_models/test/test_unicycle_2d_ignition.cpp index 0ac90e65e..4470b183e 100644 --- a/fuse_models/test/test_unicycle_2d_ignition.cpp +++ b/fuse_models/test/test_unicycle_2d_ignition.cpp @@ -236,7 +236,7 @@ TEST_F(Unicycle2DIgnitionTestFixture, SetPoseService) "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", "-p", "ignition_sensor.initial_sigma:=" "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]", - "-p", "ignition_sensor.set_pose_service:=/set_pose", + "-p", "ignition_sensor.set_pose_service:=set_pose", "-p", "ignition_sensor.reset_service:=''", "-p", "ignition_sensor.publish_on_startup:=false"}); auto node = rclcpp::Node::make_shared("unicycle_2d_ignition_test", options); @@ -261,7 +261,8 @@ TEST_F(Unicycle2DIgnitionTestFixture, SetPoseService) srv->pose.pose.covariance[0] = 1.0; srv->pose.pose.covariance[7] = 2.0; srv->pose.pose.covariance[35] = 3.0; - auto client = node->create_client("/set_pose"); + auto client = node->create_client("unicycle_2d_ignition_test/set_pose"); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); auto result = client->async_send_request(srv); ASSERT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); EXPECT_TRUE(result.get()->success); @@ -334,7 +335,7 @@ TEST_F(Unicycle2DIgnitionTestFixture, SetPoseDeprecatedService) "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", "-p", "ignition_sensor.initial_sigma:=" "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]", - "-p", "ignition_sensor.set_pose_deprecated_service:=/set_pose_deprecated", + "-p", "ignition_sensor.set_pose_deprecated_service:=set_pose_deprecated", "-p", "ignition_sensor.reset_service:=''", "-p", "ignition_sensor.publish_on_startup:=false"}); auto node = rclcpp::Node::make_shared("unicycle_2d_ignition_test", options); @@ -359,7 +360,8 @@ TEST_F(Unicycle2DIgnitionTestFixture, SetPoseDeprecatedService) srv->pose.pose.covariance[0] = 1.0; srv->pose.pose.covariance[7] = 2.0; srv->pose.pose.covariance[35] = 3.0; - auto client = node->create_client("/set_pose_deprecated"); + auto client = node->create_client("unicycle_2d_ignition_test/set_pose_deprecated"); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); auto result = client->async_send_request(srv); ASSERT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10)));