[lifecycle_manager] expose service_timeout (#4838)

* [lifecycle_manager] expose service_timeout

Signed-off-by: Guillaume Doisy <[email protected]>

* restore original change_state, and detect non_default_timeout

Signed-off-by: Guillaume Doisy <[email protected]>

* lint

Signed-off-by: Guillaume Doisy <[email protected]>

* spell

Signed-off-by: Guillaume Doisy <[email protected]>

* collapse change_state and remove non_default_timeout logic

Signed-off-by: Guillaume Doisy <[email protected]>

* Update nav2_util/src/lifecycle_service_client.cpp

Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Guillaume Doisy <[email protected]>

---------

Signed-off-by: Guillaume Doisy <[email protected]>
Signed-off-by: Guillaume Doisy <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

Author: 3 people

nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp

@@ -239,6 +239,7 @@ class LifecycleManager : public rclcpp::Node
   rclcpp::TimerBase::SharedPtr bond_timer_;
   rclcpp::TimerBase::SharedPtr bond_respawn_timer_;
   std::chrono::milliseconds bond_timeout_;
+  std::chrono::milliseconds service_timeout_;
 
   // A map of all nodes to check bond connection
   std::map<std::string, std::shared_ptr<bond::Bond>> bond_map_;

nav2_lifecycle_manager/src/lifecycle_manager.cpp

@@ -42,6 +42,7 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options)
   declare_parameter("node_names", rclcpp::PARAMETER_STRING_ARRAY);
   declare_parameter("autostart", rclcpp::ParameterValue(false));
   declare_parameter("bond_timeout", 4.0);
+  declare_parameter("service_timeout", 5.0);
   declare_parameter("bond_respawn_max_duration", 10.0);
   declare_parameter("attempt_respawn_reconnection", true);
 
@@ -54,6 +55,11 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options)
   bond_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(
     std::chrono::duration<double>(bond_timeout_s));
 
+  double service_timeout_s;
+  get_parameter("service_timeout", service_timeout_s);
+  service_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(
+    std::chrono::duration<double>(service_timeout_s));
+
   double respawn_timeout_s;
   get_parameter("bond_respawn_max_duration", respawn_timeout_s);
   bond_respawn_max_duration_ = rclcpp::Duration::from_seconds(respawn_timeout_s);
@@ -254,8 +260,9 @@ LifecycleManager::changeStateForNode(const std::string & node_name, std::uint8_t
 {
   message(transition_label_map_[transition] + node_name);
 
-  if (!node_map_[node_name]->change_state(transition) ||
-    !(node_map_[node_name]->get_state() == transition_state_map_[transition]))
+  if (!node_map_[node_name]->change_state(transition, std::chrono::milliseconds(-1),
+      service_timeout_) ||
+    !(node_map_[node_name]->get_state(service_timeout_) == transition_state_map_[transition]))
   {
     RCLCPP_ERROR(get_logger(), "Failed to change state for node: %s", node_name.c_str());
     return false;
@@ -547,7 +554,7 @@ LifecycleManager::checkBondRespawnConnection()
     }
 
     try {
-      node_map_[node_name]->get_state();  // Only won't throw if the server exists
+      node_map_[node_name]->get_state(service_timeout_);  // Only won't throw if the server exists
       live_servers++;
     } catch (...) {
       break;

nav2_util/include/nav2_util/lifecycle_service_client.hpp

@@ -24,6 +24,7 @@
 #include "nav2_util/service_client.hpp"
 #include "nav2_util/node_utils.hpp"
 
+
 namespace nav2_util
 {
 
@@ -43,16 +44,14 @@ class LifecycleServiceClient
    */
   bool change_state(
     const uint8_t transition,  // takes a lifecycle_msgs::msg::Transition id
-    const std::chrono::seconds timeout);
-
-  /// Trigger a state change, returning result
-  bool change_state(std::uint8_t transition);
+    const std::chrono::milliseconds transition_timeout = std::chrono::milliseconds(-1),
+    const std::chrono::milliseconds wait_for_service_timeout = std::chrono::milliseconds(5000));
 
   /// Get the current state as a lifecycle_msgs::msg::State id value
   /**
    * Throws std::runtime_error on failure
    */
-  uint8_t get_state(const std::chrono::seconds timeout = std::chrono::seconds(2));
+  uint8_t get_state(const std::chrono::milliseconds timeout = std::chrono::milliseconds(2000));
 
 protected:
   rclcpp::Node::SharedPtr node_;

nav2_util/src/lifecycle_service_client.cpp

@@ -22,7 +22,7 @@
 #include "lifecycle_msgs/srv/get_state.hpp"
 
 using nav2_util::generate_internal_node;
-using std::chrono::seconds;
+using std::chrono::milliseconds;
 using std::make_shared;
 using std::string;
 using namespace std::chrono_literals;
@@ -67,33 +67,26 @@ LifecycleServiceClient::LifecycleServiceClient(
 
 bool LifecycleServiceClient::change_state(
   const uint8_t transition,
-  const seconds timeout)
+  const milliseconds transition_timeout,
+  const milliseconds wait_for_service_timeout)
 {
-  if (!change_state_.wait_for_service(timeout)) {
+  if (!change_state_.wait_for_service(wait_for_service_timeout)) {
     throw std::runtime_error("change_state service is not available!");
   }
 
   auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();
   request->transition.id = transition;
-  auto response = change_state_.invoke(request, timeout);
-  return response.get();
-}
-
-bool LifecycleServiceClient::change_state(
-  std::uint8_t transition)
-{
-  if (!change_state_.wait_for_service(5s)) {
-    throw std::runtime_error("change_state service is not available!");
+  if (transition_timeout > 0ms) {
+    auto response = change_state_.invoke(request, transition_timeout);
+    return response.get();
+  } else {
+    auto response = std::make_shared<lifecycle_msgs::srv::ChangeState::Response>();
+    return change_state_.invoke(request, response);
   }
-
-  auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();
-  auto response = std::make_shared<lifecycle_msgs::srv::ChangeState::Response>();
-  request->transition.id = transition;
-  return change_state_.invoke(request, response);
 }
 
 uint8_t LifecycleServiceClient::get_state(
-  const seconds timeout)
+  const milliseconds timeout)
 {
   if (!get_state_.wait_for_service(timeout)) {
     throw std::runtime_error("get_state service is not available!");