Check action before calling

BehaviorTree · Jan 31, 2024 · f5513d8 · f5513d8
1 parent e52663a
commit f5513d8
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp
@@ -352,6 +352,10 @@ template<class T> inline
     };
     //--------------------
 
+    // Check if server is ready
+    if(!action_client_->action_server_is_ready())
+      return onFailure(SERVER_UNREACHABLE);
+
     future_goal_handle_ = action_client_->async_send_goal( goal, goal_options );
     time_goal_sent_ = node_->now();