Merge

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_dwb_controller/nav_2d_utils/CMakeLists.txt

@@ -43,7 +43,15 @@ ament_target_dependencies(path_ops
   ${dependencies}
 )
 
-install(TARGETS conversions path_ops
+add_library(tf_help SHARED
+  src/tf_help.cpp
+)
+
+ament_target_dependencies(tf_help
+  ${dependencies}
+)
+
+install(TARGETS conversions path_ops tf_help
         ARCHIVE DESTINATION lib
         LIBRARY DESTINATION lib
         RUNTIME DESTINATION bin
@@ -60,7 +68,7 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include)
-ament_export_libraries(conversions path_ops)
+ament_export_libraries(conversions path_ops tf_help)
 ament_export_dependencies(${dependencies})
 
 ament_package()

nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp

@@ -37,6 +37,7 @@
 
 #include <string>
 #include <memory>
+#include "tf2_ros/buffer.h"
 #include "nav_2d_utils/conversions.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
@@ -55,46 +56,12 @@ namespace nav_2d_utils
  * @return True if successful transform
  */
 bool transformPose(
-  const std::shared_ptr<tf2_ros::Buffer> tf, const std::string frame,
-  const geometry_msgs::msg::PoseStamped & in_pose, geometry_msgs::msg::PoseStamped & out_pose,
-  rclcpp::Duration & transform_tolerance)
-{
-  if (in_pose.header.frame_id == frame) {
-    out_pose = in_pose;
-    return true;
-  }
-
-  try {
-    tf->transform(in_pose, out_pose, frame);
-    return true;
-  } catch (tf2::ExtrapolationException & ex) {
-    auto transform = tf->lookupTransform(frame, in_pose.header.frame_id, tf2::TimePointZero);
-    if ((rclcpp::Time(in_pose.header.stamp) - rclcpp::Time(transform.header.stamp)) >
-      transform_tolerance)
-    {
-      RCLCPP_ERROR(
-        rclcpp::get_logger("tf_help"),
-        "Transform data too old when converting from %s to %s",
-        in_pose.header.frame_id.c_str(),
-        frame.c_str());
-      RCLCPP_ERROR(
-        rclcpp::get_logger("tf_help"),
-        "Data time: %ds %uns, Transform time: %ds %uns",
-        in_pose.header.stamp.sec, in_pose.header.stamp.nanosec,
-        transform.header.stamp.sec, transform.header.stamp.nanosec);
-      return false;
-    } else {
-      tf2::doTransform(in_pose, out_pose, transform);
-      return true;
-    }
-  } catch (tf2::TransformException & ex) {
-    RCLCPP_ERROR(
-      rclcpp::get_logger("tf_help"),
-      "Exception in transformPose: %s", ex.what());
-    return false;
-  }
-  return false;
-}
+  const std::shared_ptr<tf2_ros::Buffer> tf,
+  const std::string frame,
+  const geometry_msgs::msg::PoseStamped & in_pose,
+  geometry_msgs::msg::PoseStamped & out_pose,
+  rclcpp::Duration & transform_tolerance
+);
 
 /**
  * @brief Transform a Pose2DStamped from one frame to another while catching exceptions
@@ -107,19 +74,12 @@ bool transformPose(
  * @return True if successful transform
  */
 bool transformPose(
-  const std::shared_ptr<tf2_ros::Buffer> tf, const std::string frame,
-  const nav_2d_msgs::msg::Pose2DStamped & in_pose, nav_2d_msgs::msg::Pose2DStamped & out_pose,
-  rclcpp::Duration & transform_tolerance)
-{
-  geometry_msgs::msg::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose);
-  geometry_msgs::msg::PoseStamped out_3d_pose;
-
-  bool ret = transformPose(tf, frame, in_3d_pose, out_3d_pose, transform_tolerance);
-  if (ret) {
-    out_pose = poseStampedToPose2D(out_3d_pose);
-  }
-  return ret;
-}
+  const std::shared_ptr<tf2_ros::Buffer> tf,
+  const std::string frame,
+  const nav_2d_msgs::msg::Pose2DStamped & in_pose,
+  nav_2d_msgs::msg::Pose2DStamped & out_pose,
+  rclcpp::Duration & transform_tolerance
+);
 
 }  // namespace nav_2d_utils
 

nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp

@@ -0,0 +1,115 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <memory>
+#include <string>
+#include "nav_2d_utils/tf_help.hpp"
+
+namespace nav_2d_utils
+{
+
+bool transformPose(
+  const std::shared_ptr<tf2_ros::Buffer> tf,
+  const std::string frame,
+  const geometry_msgs::msg::PoseStamped & in_pose,
+  geometry_msgs::msg::PoseStamped & out_pose,
+  rclcpp::Duration & transform_tolerance
+)
+{
+  if (in_pose.header.frame_id == frame) {
+    out_pose = in_pose;
+    return true;
+  }
+
+  try {
+    tf->transform(in_pose, out_pose, frame);
+    return true;
+  } catch (tf2::ExtrapolationException & ex) {
+    auto transform = tf->lookupTransform(
+      frame,
+      in_pose.header.frame_id,
+      tf2::TimePointZero
+    );
+    if (
+      (rclcpp::Time(in_pose.header.stamp) - rclcpp::Time(transform.header.stamp)) >
+      transform_tolerance)
+    {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("tf_help"),
+        "Transform data too old when converting from %s to %s",
+        in_pose.header.frame_id.c_str(),
+        frame.c_str()
+      );
+      RCLCPP_ERROR(
+        rclcpp::get_logger("tf_help"),
+        "Data time: %ds %uns, Transform time: %ds %uns",
+        in_pose.header.stamp.sec,
+        in_pose.header.stamp.nanosec,
+        transform.header.stamp.sec,
+        transform.header.stamp.nanosec
+      );
+      return false;
+    } else {
+      tf2::doTransform(in_pose, out_pose, transform);
+      return true;
+    }
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("tf_help"),
+      "Exception in transformPose: %s",
+      ex.what()
+    );
+    return false;
+  }
+  return false;
+}
+
+bool transformPose(
+  const std::shared_ptr<tf2_ros::Buffer> tf,
+  const std::string frame,
+  const nav_2d_msgs::msg::Pose2DStamped & in_pose,
+  nav_2d_msgs::msg::Pose2DStamped & out_pose,
+  rclcpp::Duration & transform_tolerance
+)
+{
+  geometry_msgs::msg::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose);
+  geometry_msgs::msg::PoseStamped out_3d_pose;
+
+  bool ret = transformPose(tf, frame, in_3d_pose, out_3d_pose, transform_tolerance);
+  if (ret) {
+    out_pose = poseStampedToPose2D(out_3d_pose);
+  }
+  return ret;
+}
+}  // namespace nav_2d_utils

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}