[lifecycle_manager] expose service_timeout

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp

@@ -232,6 +232,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);
@@ -245,8 +251,8 @@ 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, 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;
@@ -538,7 +544,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

@@ -42,16 +42,13 @@ 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 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;
@@ -62,7 +62,7 @@ LifecycleServiceClient::LifecycleServiceClient(
 
 bool LifecycleServiceClient::change_state(
   const uint8_t transition,
-  const seconds timeout)
+  const milliseconds timeout)
 {
   if (!change_state_.wait_for_service(timeout)) {
     throw std::runtime_error("change_state service is not available!");
@@ -74,21 +74,8 @@ bool LifecycleServiceClient::change_state(
   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!");
-  }
-
-  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!");