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

Check service and action servers before calling
BehaviorTree · Mar 15, 2024 · ce923e1 · ce923e1
2 parents d4da1cf + f5513d8
commit ce923e1
Show file tree

Hide file tree

Showing 2 changed files with 8 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();
 

diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/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();