From 61648e98f5f22c294697a902de26763dfd067cc5 Mon Sep 17 00:00:00 2001 From: Benjamin Maidel Date: Thu, 2 Nov 2023 14:57:26 +0100 Subject: [PATCH] check if result form ros1 goal handle is valid --- include/ros1_bridge/action_factory.hpp | 26 ++++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index de2233ae..37ee9d66 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -388,18 +388,24 @@ class ActionFactory_2_1 : public ActionFactoryInterface } else if (goal_handle.getCommState() == actionlib::CommState::DONE) { auto result2 = std::make_shared(); auto result1 = goal_handle.getResult(); - translate_result_1_to_2(*result2, *result1); - RCLCPP_INFO( - this->logger_, - "ActionFactory_2_1::handle Goal [%s]", - goal_handle.getTerminalState().toString().c_str()); - if (goal_handle.getTerminalState() == actionlib::TerminalState::SUCCEEDED) { - gh2_->succeed(result2); - } else { + if(!result1) { + ROS_WARN("result from ros1 not initialized. goal lost => abort ros2 goal_handle"); gh2_->abort(result2); + cond_result.notify_one(); + } else { + translate_result_1_to_2(*result2, *result1); + RCLCPP_INFO( + this->logger_, + "ActionFactory_2_1::handle Goal [%s]", + goal_handle.getTerminalState().toString().c_str()); + if (goal_handle.getTerminalState() == actionlib::TerminalState::SUCCEEDED) { + gh2_->succeed(result2); + } else { + gh2_->abort(result2); + } + result_ready = true; + cond_result.notify_one(); } - result_ready = true; - cond_result.notify_one(); return; } },