From 85aa1bcc392aa0728296864a82d6d01374676c1c 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 + 2 files changed, 2 insertions(+) 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