diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index e4ffa0fe..086137a9 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -149,21 +149,47 @@ class ActionFactory_1_2 : public ActionFactoryInterface auto send_goal_ops = ROS2SendGoalOptions(); send_goal_ops.goal_response_callback = [this, &gh2_future](std::shared_future gh2) mutable { - auto goal_handle = gh2_future.get(); - if (!goal_handle) { - gh1_.setRejected(); // goal was not accepted by remote server - return; - } + try { + // this is a workaround for the underlying actionlib implementation bug + // this callback function should never get called the the shared_future + // is not ready/valid + int counter = 0; + while(!gh2_future.valid()) + { + if( counter >= 10) + { + std::cout<<"std::shared_future not valid for more than 1.0 seconds. " + "Rejecting action goal."< lock(mutex_); - gh2_ = goal_handle; + { + std::lock_guard lock(mutex_); + gh2_ = goal_handle; - if (canceled_) { // cancel was called in between - auto fut = client_->async_cancel_goal(gh2_); + if (canceled_) { // cancel was called in between + auto fut = client_->async_cancel_goal(gh2_); + } } + } catch (const std::future_error& e) { + std::cout << "Caught a future_error with code \"" << e.code() + << "\"\nMessage: \"" << e.what() << "\"\n"; + throw; } };