Bond support + fixing various action server issues (#1894)

* prototype of lifecycle bond system

* adding more structure to get around weak ptr issue

* working prototype for manager

* adding some ns -> s conversions

* changing to service node

* adding bond connections to all servers

* update logs

* fixing review comments

* fix types

* remove extraneous functions

* make linters happy

* simplifications

* adding spinner to get working but now unstable

* moving bond connections to activate state

* adding defaults

* working complete prototype for review

* update dependencies

* adding connection logging

* remove accidental file

* fix server side timeout for heartbeats

* adding complete unit coverage of bond support

* fixing lifecycle test

* trying to activate since autostart was removed

Author: SteveMacenski

doc/parameters/param_list.md

@@ -400,6 +400,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame
 | ----------| --------| ------------|
 | node_names | N/A | Ordered list of node names to bringup through lifecycle transition |
 | autostart | false | Whether to transition nodes to active state on startup |
+| bond_timeout_ms | 4000 | Timeout for bond to fail if no heartbeat can be found, in milliseconds. If set to 0, it will be disabled. Must be larger than 300ms for stable bringup. |
 
 # map_server
 

nav2_amcl/src/amcl_node.cpp

@@ -300,6 +300,9 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/)
     handleInitialPose(last_published_pose_);
   }
 
+  // create bond connection
+  createBond();
+
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -315,6 +318,9 @@ AmclNode::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
   particlecloud_pub_->on_deactivate();
   particle_cloud_pub_->on_deactivate();
 
+  // destroy bond connection
+  destroyBond();
+
   return nav2_util::CallbackReturn::SUCCESS;
 }
 

nav2_bt_navigator/src/bt_navigator.cpp

@@ -108,7 +108,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
     get_node_clock_interface(),
     get_node_logging_interface(),
     get_node_waitables_interface(),
-    "navigate_to_pose", std::bind(&BtNavigator::navigateToPose, this), false);
+    "navigate_to_pose", std::bind(&BtNavigator::navigateToPose, this));
 
   // Get the libraries to pull plugins from
   plugin_lib_names_ = get_parameter("plugin_lib_names").as_string_array();
@@ -133,11 +133,6 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   // Get the BT filename to use from the node parameter
   get_parameter("default_bt_xml_filename", default_bt_xml_filename_);
 
-  if (!loadBehaviorTree(default_bt_xml_filename_)) {
-    RCLCPP_ERROR(get_logger(), "Error loading XML file: %s", default_bt_xml_filename_.c_str());
-    return nav2_util::CallbackReturn::FAILURE;
-  }
-
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -176,8 +171,16 @@ BtNavigator::on_activate(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Activating");
 
+  if (!loadBehaviorTree(default_bt_xml_filename_)) {
+    RCLCPP_ERROR(get_logger(), "Error loading XML file: %s", default_bt_xml_filename_.c_str());
+    return nav2_util::CallbackReturn::FAILURE;
+  }
+
   action_server_->activate();
 
+  // create bond connection
+  createBond();
+
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -188,6 +191,9 @@ BtNavigator::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
 
   action_server_->deactivate();
 
+  // destroy bond connection
+  destroyBond();
+
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -332,7 +338,7 @@ BtNavigator::initializeGoalPose()
   blackboard_->set<int>("number_recoveries", 0);  // NOLINT
 
   // Update the goal pose on the blackboard
-  blackboard_->set("goal", goal->pose);
+  blackboard_->set<geometry_msgs::msg::PoseStamped>("goal", goal->pose);
 }
 
 void

nav2_controller/src/nav2_controller.cpp

@@ -72,6 +72,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
 
   RCLCPP_INFO(get_logger(), "Configuring controller interface");
 
+
   get_parameter("progress_checker_plugin", progress_checker_id_);
   if (progress_checker_id_ == default_progress_checker_id_) {
     nav2_util::declare_parameter_if_not_declared(
@@ -93,6 +94,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
         rclcpp::ParameterValue(default_types_[i]));
     }
   }
+
   controller_types_.resize(controller_ids_.size());
 
   get_parameter("controller_frequency", controller_frequency_);
@@ -172,6 +174,9 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & state)
   vel_publisher_->on_activate();
   action_server_->activate();
 
+  // create bond connection
+  createBond();
+
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -190,6 +195,9 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & state)
   publishZeroVelocity();
   vel_publisher_->on_deactivate();
 
+  // destroy bond connection
+  destroyBond();
+
   return nav2_util::CallbackReturn::SUCCESS;
 }
 

nav2_lifecycle_manager/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
+find_package(bondcpp REQUIRED)
 
 nav2_package()
 
@@ -38,6 +39,7 @@ set(dependencies
   std_msgs
   std_srvs
   tf2_geometry_msgs
+  bondcpp
 )
 
 ament_target_dependencies(${library_name}

nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp

@@ -22,10 +22,12 @@
 #include <vector>
 
 #include "nav2_util/lifecycle_service_client.hpp"
+#include "nav2_util/node_thread.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "std_srvs/srv/empty.hpp"
 #include "nav2_msgs/srv/manage_lifecycle_nodes.hpp"
 #include "std_srvs/srv/trigger.hpp"
+#include "bondcpp/bond.hpp"
 
 namespace nav2_lifecycle_manager
 {
@@ -52,6 +54,8 @@ class LifecycleManager : public rclcpp::Node
 protected:
   // The ROS node to use when calling lifecycle services
   rclcpp::Node::SharedPtr service_client_node_;
+  rclcpp::Node::SharedPtr bond_client_node_;
+  std::unique_ptr<nav2_util::NodeThread> bond_node_thread_;
 
   // The services provided by this node
   rclcpp::Service<ManageLifecycleNodes>::SharedPtr manager_srv_;
@@ -121,10 +125,37 @@ class LifecycleManager : public rclcpp::Node
    */
   void destroyLifecycleServiceClients();
 
+  // Support function for creating bond timer
+  /**
+   * @brief Support function for creating bond timer
+   */
+  void createBondTimer();
+
+  // Support function for creating bond connection
+  /**
+   * @brief Support function for creating bond connections
+   */
+  bool createBondConnection(const std::string & node_name);
+
+  // Support function for killing bond connections
+  /**
+   * @brief Support function for killing bond connections
+   */
+  void destroyBondTimer();
+
+  // Support function for checking on bond connections
+  /**
+   * @ brief Support function for checking on bond connections
+   * will take down system if there's something non-responsive
+   */
+  void checkBondConnections();
+
   /**
    * @brief For a node, transition to the new target state
    */
-  bool changeStateForNode(const std::string & node_name, std::uint8_t transition);
+  bool changeStateForNode(
+    const std::string & node_name,
+    std::uint8_t transition);
 
   /**
    * @brief For each node in the map, transition to the new target state
@@ -137,6 +168,13 @@ class LifecycleManager : public rclcpp::Node
    */
   void message(const std::string & msg);
 
+  // Timer thread to look at bond connections
+  rclcpp::TimerBase::SharedPtr bond_timer_;
+  std::chrono::milliseconds bond_timeout_;
+
+  // A map of all nodes to check bond connection
+  std::map<std::string, std::shared_ptr<bond::Bond>> bond_map_;
+
   // A map of all nodes to be controlled
   std::map<std::string, std::shared_ptr<nav2_util::LifecycleServiceClient>> node_map_;
 

nav2_lifecycle_manager/package.xml

@@ -18,6 +18,7 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>std_srvs</build_depend>
   <build_depend>tf2_geometry_msgs</build_depend>
+  <build_depend>bondcpp</build_depend>
   <build_depend>nav2_common</build_depend>
 
   <exec_depend>geometry_msgs</exec_depend>
@@ -28,6 +29,7 @@
   <exec_depend>rclcpp_lifecycle</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>std_srvs</exec_depend>
+  <exec_depend>bondcpp</exec_depend>
   <exec_depend>tf2_geometry_msgs</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>