Foxy and later compatible goal response callback

hsd-dev · Nov 17, 2020 · 4cf1491 · 4cf1491
1 parent 205a3c0
commit 4cf1491
Showing 1 changed file with 7 additions and 4 deletions.
diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp
@@ -136,10 +136,13 @@ class ActionFactory_1_2 : public ActionFactoryInterface
                 return;
             }
 
+            // goal_response_callback signature changed after foxy, this implementation
+            // works with both
+            std::shared_future<ROS2GoalHandle> gh2_future;
             //Changes as per Dashing
             auto send_goal_ops = ROS2SendGoalOptions();
             send_goal_ops.goal_response_callback =
-                [this](auto gh2) mutable {
+                [this, &gh2_future](auto gh2) mutable {
                     if (!gh2)
                     {
                         gh1_.setRejected(); // goal was not accepted by remote server
@@ -150,11 +153,11 @@ class ActionFactory_1_2 : public ActionFactoryInterface
 
                     {
                         std::lock_guard<std::mutex> lock(mutex_);
-                        gh2_ = gh2;
+                        gh2_ = gh2_future.get();
 
                         if (canceled_)
                         { // cancel was called in between
-                            auto fut = client_->async_cancel_goal(gh2_);
+                          auto fut = client_->async_cancel_goal(gh2_);
                         }
                     }
                 };
@@ -167,7 +170,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface
                 };
 
             // send goal to ROS2 server, set-up feedback
-            auto gh2_future = client_->async_send_goal(goal2, send_goal_ops);
+            gh2_future = client_->async_send_goal(goal2, send_goal_ops);
 
             auto future_result = client_->async_get_result(gh2_future.get());
             auto res2 = future_result.get();