botsandus · doisyg · Oct 11, 2023 · Oct 12, 2023 · Oct 12, 2023 · Oct 14, 2023
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
@@ -268,7 +268,7 @@ class BtActionNode : public BT::ActionNodeBase
         // Action related failure that should not fail the tree, but the node
         return BT::NodeStatus::FAILURE;
       } else {
-        // Internal exception to propogate to the tree
+        // Internal exception to propagate to the tree
         throw e;
       }
     }

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -231,6 +231,11 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
   // Create the Behavior Tree from the XML input
   try {
     tree_ = bt_->createTreeFromFile(filename, blackboard_);
+    for (auto & blackboard : tree_.blackboard_stack) {
+      blackboard->set<rclcpp::Node::SharedPtr>("node", client_node_);
+      blackboard->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);
+      blackboard->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
+    }
   } catch (const std::exception & e) {
     RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what());
     return false;

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp
@@ -53,7 +53,7 @@ class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
   {
     return providedBasicPorts(
       {
-        BT::InputPort<int>("wait_duration", 1, "Wait time")
+        BT::InputPort<double>("wait_duration", 1.0, "Wait time")
       });
   }
 };

diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp
@@ -14,6 +14,7 @@
 
 #include <string>
 #include <memory>
+#include <cmath>
 
 #include "nav2_behavior_tree/plugins/action/wait_action.hpp"
 
@@ -26,16 +27,16 @@ WaitAction::WaitAction(
   const BT::NodeConfiguration & conf)
 : BtActionNode<nav2_msgs::action::Wait>(xml_tag_name, action_name, conf)
 {
-  int duration;
+  double duration;
   getInput("wait_duration", duration);
   if (duration <= 0) {
     RCLCPP_WARN(
       node_->get_logger(), "Wait duration is negative or zero "
-      "(%i). Setting to positive.", duration);
+      "(%f). Setting to positive.", duration);
     duration *= -1;
   }
 
-  goal_.time.sec = duration;
+  goal_.time = rclcpp::Duration::from_seconds(duration);
 }
 
 void WaitAction::on_tick()

diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp
@@ -55,7 +55,12 @@ BehaviorTreeEngine::run(
 
       onLoop();
 
-      loopRate.sleep();
+      if (!loopRate.sleep()) {
+        RCLCPP_WARN(
+          rclcpp::get_logger("BehaviorTreeEngine"),
+          "Behavior Tree tick rate %0.2f was exceeded!",
+          1.0 / (loopRate.period().count() * 1.0e-9));
+      }
     }
   } catch (const std::exception & ex) {
     RCLCPP_ERROR(

diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp
@@ -117,18 +117,18 @@ TEST_F(WaitActionTestFixture, test_ports)
       </root>)";
 
   tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
-  EXPECT_EQ(tree_->rootNode()->getInput<int>("wait_duration"), 1);
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("wait_duration"), 1.0);
 
   xml_txt =
     R"(
       <root main_tree_to_execute = "MainTree" >
         <BehaviorTree ID="MainTree">
-            <Wait wait_duration="10" />
+            <Wait wait_duration="10.0" />
         </BehaviorTree>
       </root>)";
 
   tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
-  EXPECT_EQ(tree_->rootNode()->getInput<int>("wait_duration"), 10);
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("wait_duration"), 10.0);
 }
 
 TEST_F(WaitActionTestFixture, test_tick)
@@ -137,7 +137,7 @@ TEST_F(WaitActionTestFixture, test_tick)
     R"(
       <root main_tree_to_execute = "MainTree" >
         <BehaviorTree ID="MainTree">
-            <Wait wait_duration="-5"/>
+            <Wait wait_duration="-5.0"/>
         </BehaviorTree>
       </root>)";
 

diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py
@@ -25,7 +25,7 @@
 from launch_ros.actions import Node
 from launch_ros.actions import PushROSNamespace
 from launch_ros.descriptions import ParameterFile
-from nav2_common.launch import RewrittenYaml, ReplaceString
+from nav2_common.launch import ReplaceString, RewrittenYaml
 
 
 def generate_launch_description():

diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py
@@ -14,6 +14,7 @@
 
 
 import os
+
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import (DeclareLaunchArgument, ExecuteProcess, GroupAction,

diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml
@@ -19,7 +19,6 @@
   <exec_depend>navigation2</exec_depend>
   <exec_depend>nav2_common</exec_depend>
   <exec_depend>slam_toolbox</exec_depend>
-  <exec_depend>turtlebot3_gazebo</exec_depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

diff --git a/...tor/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/...tor/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml
@@ -37,7 +37,7 @@
             <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
           </Sequence>
           <Spin spin_dist="1.57"/>
-          <Wait wait_duration="5"/>
+          <Wait wait_duration="5.0"/>
           <BackUp backup_dist="0.30" backup_speed="0.05"/>
         </RoundRobin>
       </ReactiveFallback>

diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
@@ -29,7 +29,7 @@
             <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
           </Sequence>
           <Spin spin_dist="1.57" error_code_id="{spin_error_code}"/>
-          <Wait wait_duration="5"/>
+          <Wait wait_duration="5.0"/>
           <BackUp backup_dist="0.30" backup_speed="0.05" error_code_id="{backup_error_code}"/>
         </RoundRobin>
       </ReactiveFallback>

diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
@@ -38,7 +38,7 @@
               <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
             </Sequence>
             <Spin spin_dist="1.57" error_code_id="{spin_error_code}"/>
-            <Wait wait_duration="5"/>
+            <Wait wait_duration="5.0"/>
             <BackUp backup_dist="0.30" backup_speed="0.05" error_code_id="{backup_code_id}"/>
           </RoundRobin>
         </ReactiveFallback>

diff --git a/..._bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/..._bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml
@@ -20,7 +20,7 @@
             <RetryUntilSuccessful num_attempts="1">
               <SequenceStar name="CancelingControlAndWait">
                 <CancelControl name="ControlCancel"/>
-                <Wait wait_duration="5"/>
+                <Wait wait_duration="5.0"/>
               </SequenceStar>
             </RetryUntilSuccessful>
           </PathLongerOnApproach>
@@ -38,7 +38,7 @@
             <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
           </Sequence>
           <Spin spin_dist="1.57"/>
-          <Wait wait_duration="5"/>
+          <Wait wait_duration="5.0"/>
           <BackUp backup_dist="0.30" backup_speed="0.05"/>
         </RoundRobin>
       </ReactiveFallback>

diff --git a/...igator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/...igator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml
@@ -35,7 +35,7 @@
             <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
           </Sequence>
           <Spin name="SpinRecovery" spin_dist="1.57"/>
-          <Wait name="WaitRecovery" wait_duration="5"/>
+          <Wait name="WaitRecovery" wait_duration="5.0"/>
           <BackUp name="BackUpRecovery" backup_dist="0.30" backup_speed="0.05"/>
         </RoundRobin>
       </ReactiveFallback>

diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp
@@ -32,7 +32,7 @@ namespace nav2_bt_navigator
 {
 
 /**
- * @class NavigateToPoseNavigator
+ * @class NavigateThroughPosesNavigator
  * @brief A navigator for navigating to a a bunch of intermediary poses
  */
 class NavigateThroughPosesNavigator

diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt
@@ -15,6 +15,7 @@ find_package(nav2_common REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
 
 ### Header ###
 
@@ -37,6 +38,7 @@ set(dependencies
   nav2_util
   nav2_costmap_2d
   nav2_msgs
+  visualization_msgs
 )
 
 set(monitor_executable_name collision_monitor)

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp
@@ -28,6 +28,7 @@
 
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_msgs/msg/collision_detector_state.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
 
 #include "nav2_collision_monitor/types.hpp"
 #include "nav2_collision_monitor/polygon.hpp"
@@ -147,6 +148,9 @@ class CollisionDetector : public nav2_util::LifecycleNode
   /// @brief collision monitor state publisher
   rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CollisionDetectorState>::SharedPtr
     state_pub_;
+  /// @brief Collision points marker publisher
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+    collision_points_marker_pub_;
   /// @brief timer that runs actions
   rclcpp::TimerBase::SharedPtr timer_;
 

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
@@ -21,6 +21,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/twist.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
 
 #include "tf2/time.h"
 #include "tf2_ros/buffer.h"
@@ -107,6 +108,7 @@ class CollisionMonitor : public nav2_util::LifecycleNode
    * @param cmd_vel_in_topic Output name of cmd_vel_in topic
    * @param cmd_vel_out_topic Output name of cmd_vel_out topic
    * is required.
+   * @param state_topic topic name for publishing collision monitor state
    * @return True if all parameters were obtained or false in failure case
    */
   bool getParameters(
@@ -210,6 +212,10 @@ class CollisionMonitor : public nav2_util::LifecycleNode
   rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CollisionMonitorState>::SharedPtr
     state_pub_;
 
+  /// @brief Collision points marker publisher
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+    collision_points_marker_pub_;
+
   /// @brief Whether main routine is active
   bool process_active_;
 

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp
@@ -69,8 +69,9 @@ class PointCloud : public Source
    * @param curr_time Current node time for data interpolation
    * @param data Array where the data from source to be added.
    * Added data is transformed to base_frame_id_ coordinate system at curr_time.
+   * @return false if an invalid source should block the robot
    */
-  void getData(
+  bool getData(
     const rclcpp::Time & curr_time,
     std::vector<Point> & data) const;
 

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp
@@ -69,8 +69,9 @@ class Range : public Source
    * @param curr_time Current node time for data interpolation
    * @param data Array where the data from source to be added.
    * Added data is transformed to base_frame_id_ coordinate system at curr_time.
+   * @return false if an invalid source should block the robot
    */
-  void getData(
+  bool getData(
     const rclcpp::Time & curr_time,
     std::vector<Point> & data) const;
 

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp
@@ -69,8 +69,9 @@ class Scan : public Source
    * @param curr_time Current node time for data interpolation
    * @param data Array where the data from source to be added.
    * Added data is transformed to base_frame_id_ coordinate system at curr_time.
+   * @return false if an invalid source should block the robot
    */
-  void getData(
+  bool getData(
     const rclcpp::Time & curr_time,
     std::vector<Point> & data) const;
 

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp
@@ -69,8 +69,9 @@ class Source
    * @param curr_time Current node time for data interpolation
    * @param data Array where the data from source to be added.
    * Added data is transformed to base_frame_id_ coordinate system at curr_time.
+   * @return false if an invalid source should block the robot
    */
-  virtual void getData(
+  virtual bool getData(
     const rclcpp::Time & curr_time,
     std::vector<Point> & data) const = 0;
 
@@ -80,6 +81,18 @@ class Source
    */
   bool getEnabled() const;
 
+  /**
+   * @brief Obtains the name of the data source
+   * @return Name of the data source
+   */
+  std::string getSourceName() const;
+
+  /**
+   * @brief Obtains the source_timeout parameter of the data source
+   * @return source_timeout parameter value of the data source
+   */
+  rclcpp::Duration getSourceTimeout() const;
+
 protected:
   /**
    * @brief Source configuration routine.

diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml
@@ -21,6 +21,7 @@
   <depend>nav2_util</depend>
   <depend>nav2_costmap_2d</depend>
   <depend>nav2_msgs</depend>
+  <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_common</test_depend>
-Original file line number
+Diff line change
@@ Expand Up @@
       {
         return providedBasicPorts(
           {
-            BT::InputPort<int>("wait_duration", 1, "Wait time")
+            BT::InputPort<double>("wait_duration", 1.0, "Wait time")
           });
       }
     };
@@ Expand Down @@