Wait on result

Signed-off-by: methylDragon <[email protected]>

fuse_models/src/graph_ignition.cpp

@@ -182,6 +182,7 @@ void GraphIgnition::process(const fuse_msgs::msg::SerializedGraph& msg)
     auto srv = std::make_shared<std_srvs::srv::Empty::Request>();
     // 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

fuse_models/src/unicycle_2d_ignition.cpp

@@ -274,6 +274,7 @@ void Unicycle2DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceSta
     auto srv = std::make_shared<std_srvs::srv::Empty::Request>();
     // 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

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_msgs::srv::SetGraph::Request>();
   fuse_core::serializeGraph(graph, srv->graph);
-  auto client = node->create_client<fuse_msgs::srv::SetGraph>("/set_graph");
+  auto client = node->create_client<fuse_msgs::srv::SetGraph>("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_msgs::srv::SetGraph::Request>();
   fuse_core::serializeGraph(graph, srv->graph);
-  auto client = node->create_client<fuse_msgs::srv::SetGraph>("/set_graph");
+  auto client = node->create_client<fuse_msgs::srv::SetGraph>("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);

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<fuse_msgs::srv::SetPose>("/set_pose");
+  auto client = node->create_client<fuse_msgs::srv::SetPose>("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<fuse_msgs::srv::SetPoseDeprecated>("/set_pose_deprecated");
+  auto client = node->create_client<fuse_msgs::srv::SetPoseDeprecated>("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)));