Merge pull request BehaviorTree#47 from marip8/update/check-server

Check service and action servers before calling

Author: facontidavide

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();
 

behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp

@@ -278,6 +278,10 @@ template<class T> inline
       return CheckStatus( onFailure(INVALID_REQUEST) );
     }
 
+    // Check if server is ready
+    if(!service_client_->service_is_ready())
+      return onFailure(SERVICE_UNREACHABLE);
+
     future_response_ = service_client_->async_send_request(request).share();
     time_request_sent_ = node_->now();