Humble Sync 5 (Feb 10) (ros-navigation#3405)

* Std::string doesn't have such constructor, therefore wrong exception happens when waiting for action server is over (ros-navigation#3333) * Update bt_action_node.hpp * Update bt_cancel_action_node.hpp * Update bt_action_node.hpp * Update bt_cancel_action_node.hpp * Do only consider enabled costmap plugins/filters in the isCurrent() method of a LayeredCostmap (ros-navigation#3356) * Do only consider enabled costmap plugins/filters in the isCurrent() method of a LayeredCostmap When clearing entirely a costmap (see the `clear_entirely_*` service), its layers are reset. This sets their `current_` attribute to False, until they are updated (`updateCosts()`). However, for disabled plugins/filters, the `updateCosts()` is bypassed so that the `current_` attribute is never reset to True, and the costmap never becomes "current" again; because of that the controller_server used to get stuck in an endless loop (see `while (!costmap_ros_->isCurrent())` in `ControllerServer::computeControl()`). This patch fixes that by adding a condition for not considering disabled plugins/filters in the `LayeredCostmap::isCurrent()`. * Add forgotten Layer::isEnabled() definition * Export ompl include directories from nav2_smac_planner. (ros-navigation#3358) * Update default w_smooth to 2M (ros-navigation#3341) * Fix nav2_velocity_smoother for sim time (ros-navigation#3360) * Fix nav2_velocity_smoother for sim time * Update nav2_velocity_smoother/src/velocity_smoother.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * catch general exceptions (ros-navigation#3373) * catch general exceptions * Update controller_server.cpp * nav2_tree_nodes: rename "service_name" input port for action nodes (ros-navigation#3370) Classes inheriting from nav2_behavior_tree::BtActionNode use "server_name" as input port. The "service_name" input port is reserved to sub-classes of nav2_behavior_tree::BtServiceNode. There seemed to be a confusion in the nav2_tree_nodes.xml file. * Fixing Theta* test failures (ros-navigation#3380) * adding printouts since can't see locally * one more to isolate * request in bounds * remove prints * Fix SpeedController (ros-navigation#3378) * Fix SpeedController * Using blackboard get() method that does not throw error * Share odom_smoother on the blackboard Signed-off-by: Borong Yuan <yuanborong@hotmail.com> * Fix SpeedController linting and test errors Signed-off-by: Borong Yuan <yuanborong@hotmail.com> Signed-off-by: Borong Yuan <yuanborong@hotmail.com> * node_name change during initialization (ros-navigation#3387) * Added missing conditions to nav2_tree_nodes (ros-navigation#3401) * Add WouldAControllerRecoveryHelp and WouldAPlannerRecoveryHelp to nav2_tree_nodes * Add WouldAControllerRecoveryHelp and WouldAPlannerRecoveryHelp...... to nav2_tree_nodes * Update nav2_tree_nodes.xml --------- Co-authored-by: blanker <you@example.com> * Fix multi-robot launch issues (ros-navigation#3383) * Fix multi-robot launch issues: * Use full name of composable nodes container * Correct and update BT plugins and error codes list * Fix and update local and global costmap used plugins parameters * Do not publish voxel map for multi-robot case * Update controller_server.cpp * Update nav2_smoother.cpp --------- Signed-off-by: Borong Yuan <yuanborong@hotmail.com> Co-authored-by: Andrii Maistruk <71632363+andriimaistruk@users.noreply.github.com> Co-authored-by: milidam <milidam@users.noreply.github.com> Co-authored-by: Marc Alban <marcalban@gmail.com> Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Co-authored-by: HovorunB <87417416+HovorunB@users.noreply.github.com> Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com> Co-authored-by: Borong Yuan <yuanborong@hotmail.com> Co-authored-by: Austin Greisman <92941098+austin-InDro@users.noreply.github.com> Co-authored-by: muttistefano <mutti.stefano.jp@gmail.com> Co-authored-by: blanker <you@example.com>
lge-ros2 · Mar 6, 2023 · 322b1b2 · 322b1b2
1 parent 0007953
commit 322b1b2
Show file tree

Hide file tree

Showing 24 changed files with 262 additions and 150 deletions.
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
@@ -97,7 +97,9 @@ class BtActionNode : public BT::ActionNodeBase
       RCLCPP_ERROR(
         node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
         action_name.c_str());
-      throw std::runtime_error(std::string("Action server %s not available", action_name.c_str()));
+      throw std::runtime_error(
+              std::string("Action server ") + action_name +
+              std::string(" not available"));
     }
   }
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp
@@ -93,7 +93,9 @@ class BtCancelActionNode : public BT::ActionNodeBase
       RCLCPP_ERROR(
         node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
         action_name.c_str());
-      throw std::runtime_error(std::string("Action server %s not available", action_name.c_str()));
+      throw std::runtime_error(
+              std::string("Action server ") + action_name +
+              std::string(" not available"));
     }
   }
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp
@@ -57,7 +57,6 @@ class SpeedController : public BT::DecoratorNode
       BT::InputPort<double>("max_rate", 1.0, "Maximum rate"),
       BT::InputPort<double>("min_speed", 0.0, "Minimum speed"),
       BT::InputPort<double>("max_speed", 0.5, "Maximum speed"),
-      BT::InputPort<double>("filter_duration", 0.3, "Duration (secs) for velocity smoothing filter")
     };
   }
 

diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -11,35 +11,35 @@
       <input_port name="backup_dist">Distance to backup</input_port>
       <input_port name="backup_speed">Speed at which to backup</input_port>
       <input_port name="time_allowance">Allowed time for reversing</input_port>
-      <input_port name="server_name">Service name</input_port>
+      <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
     <Action ID="DriveOnHeading">
       <input_port name="dist_to_travel">Distance to travel</input_port>
       <input_port name="speed">Speed at which to travel</input_port>
       <input_port name="time_allowance">Allowed time for reversing</input_port>
-      <input_port name="server_name">Service name</input_port>
+      <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
     <Action ID="CancelControl">
-      <input_port name="service_name">Service name to cancel the controller server</input_port>
+      <input_port name="server_name">Server name to cancel the controller server</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
     <Action ID="CancelBackUp">
-      <input_port name="service_name">Service name to cancel the backup behavior</input_port>
+      <input_port name="server_name">Server name to cancel the backup behavior</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
     <Action ID="CancelDriveOnHeading">
-      <input_port name="service_name">Service name to cancel the drive on heading behavior</input_port>
+      <input_port name="server_name">Service name to cancel the drive on heading behavior</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
     <Action ID="CancelSpin">
-      <input_port name="service_name">Service name to cancel the spin behavior</input_port>
+      <input_port name="server_name">Server name to cancel the spin behavior</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
@@ -49,7 +49,7 @@
     </Action>
 
     <Action ID="CancelWait">
-      <input_port name="service_name">Service name to cancel the wait behavior</input_port>
+      <input_port name="server_name">Server name to cancel the wait behavior</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
@@ -74,15 +74,15 @@
       <input_port name="goal">Destination to plan to</input_port>
       <input_port name="start">Start pose of the path if overriding current robot pose</input_port>
       <input_port name="planner_id">Mapped name to the planner plugin type to use</input_port>
-      <input_port name="service_name">Service name</input_port>
+      <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
       <output_port name="path">Path created by ComputePathToPose node</output_port>
     </Action>
 
     <Action ID="ComputePathThroughPoses">
       <input_port name="goals">Destinations to plan through</input_port>
       <input_port name="start">Start pose of the path if overriding current robot pose</input_port>
-      <input_port name="service_name">Service name</input_port>
+      <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
       <input_port name="planner_id">Mapped name to the planner plugin type to use</input_port>
       <output_port name="path">Path created by ComputePathToPose node</output_port>
@@ -110,20 +110,20 @@
       <input_port name="controller_id" default="FollowPath"/>
       <input_port name="path">Path to follow</input_port>
       <input_port name="goal_checker_id" default="GoalChecker">Goal checker</input_port>
-      <input_port name="service_name">Service name</input_port>
+      <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
     <Action ID="NavigateToPose">
         <input_port name="goal">Goal</input_port>
-        <input_port name="service_name">Service name</input_port>
+        <input_port name="server_name">Server name</input_port>
         <input_port name="server_timeout">Server timeout</input_port>
         <input_port name="behavior_tree">Behavior tree to run</input_port>
     </Action>
 
     <Action ID="NavigateThroughPoses">
         <input_port name="goals">Goals</input_port>
-        <input_port name="service_name">Service name</input_port>
+        <input_port name="server_name">Server name</input_port>
         <input_port name="server_timeout">Server timeout</input_port>
         <input_port name="behavior_tree">Behavior tree to run</input_port>
     </Action>
@@ -177,13 +177,13 @@
     <Action ID="Spin">
       <input_port name="spin_dist">Spin distance</input_port>
       <input_port name="time_allowance">Allowed time for spinning</input_port>
-      <input_port name="server_name">Service name</input_port>
+      <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
     <Action ID="Wait">
       <input_port name="wait_duration">Wait time</input_port>
-      <input_port name="server_name">Service name</input_port>
+      <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
     </Action>
 
@@ -242,6 +242,24 @@
       <input_port name="server_timeout"> Server timeout </input_port>
     </Condition>
 
+    <Condition ID="WouldAControllerRecoveryHelp">
+      <input_port name="error_code">Error code</input_port>
+    </Condition>
+
+    <Condition ID="WouldAPlannerRecoveryHelp">
+      <input_port name="error_code">Error code</input_port>
+    </Condition>
+
+    <Condition ID="WouldASmootherRecoveryHelp">
+      <input_port name="error_code">Error code</input_port>
+    </Condition>
+
+    <Condition ID="AreErrorCodesPresent">
+      <input_port name="error_code">Error code</input_port>
+      <input_port name="error_codes_to_check">Error codes to check, user defined</input_port>
+    </Condition>
+
+
     <!-- ############################### CONTROL NODES ################################ -->
     <Control ID="PipelineSequence"/>
 
@@ -275,7 +293,6 @@
       <input_port name="max_rate">Maximum rate</input_port>
       <input_port name="min_speed">Minimum speed</input_port>
       <input_port name="max_speed">Maximum speed</input_port>
-      <input_port name="filter_duration">Duration (secs) for velocity smoothing filter</input_port>
     </Decorator>
 
     <Decorator ID="PathLongerOnApproach">

diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp
@@ -50,18 +50,28 @@ SpeedController::SpeedController(
   d_rate_ = max_rate_ - min_rate_;
   d_speed_ = max_speed_ - min_speed_;
 
-  double duration = 0.3;
-  getInput("filter_duration", duration);
   std::string odom_topic;
   node_->get_parameter_or("odom_topic", odom_topic, std::string("odom"));
-  odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>(node_, duration, odom_topic);
+  odom_smoother_ = config().blackboard->get<std::shared_ptr<nav2_util::OdomSmoother>>(
+    "odom_smoother");
 }
 
 inline BT::NodeStatus SpeedController::tick()
 {
-  auto current_goal = config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal");
-  auto current_goals =
-    config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals");
+  if (status() == BT::NodeStatus::IDLE) {
+    // Reset since we're starting a new iteration of
+    // the speed controller (moving from IDLE to RUNNING)
+    config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", goals_);
+    config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", goal_);
+    period_ = 1.0 / max_rate_;
+    start_ = node_->now();
+    first_tick_ = true;
+  }
+
+  std::vector<geometry_msgs::msg::PoseStamped> current_goals;
+  config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", current_goals);
+  geometry_msgs::msg::PoseStamped current_goal;
+  config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", current_goal);
 
   if (goal_ != current_goal || goals_ != current_goals) {
     // Reset state and set period to max since we have a new goal

diff --git a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp
@@ -34,6 +34,10 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi
 public:
   void SetUp()
   {
+    odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>(node_);
+    config_->blackboard->set<std::shared_ptr<nav2_util::OdomSmoother>>(
+      "odom_smoother", odom_smoother_);  // NOLINT
+
     geometry_msgs::msg::PoseStamped goal;
     goal.header.stamp = node_->now();
     config_->blackboard->set("goal", goal);
@@ -50,13 +54,17 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi
   {
     dummy_node_.reset();
     bt_node_.reset();
+    odom_smoother_.reset();
   }
 
 protected:
+  static std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
   static std::shared_ptr<nav2_behavior_tree::SpeedController> bt_node_;
   static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
 };
 
+std::shared_ptr<nav2_util::OdomSmoother>
+SpeedControllerTestFixture::odom_smoother_ = nullptr;
 std::shared_ptr<nav2_behavior_tree::SpeedController>
 SpeedControllerTestFixture::bt_node_ = nullptr;
 std::shared_ptr<nav2_behavior_tree::DummyNode>

diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py
@@ -37,6 +37,7 @@ def generate_launch_description():
     params_file = LaunchConfiguration('params_file')
     use_composition = LaunchConfiguration('use_composition')
     container_name = LaunchConfiguration('container_name')
+    container_name_full = (namespace, '/', container_name)
     use_respawn = LaunchConfiguration('use_respawn')
     log_level = LaunchConfiguration('log_level')
 
@@ -141,7 +142,7 @@ def generate_launch_description():
 
     load_composable_nodes = LoadComposableNodes(
         condition=IfCondition(use_composition),
-        target_container=container_name,
+        target_container=container_name_full,
         composable_node_descriptions=[
             ComposableNode(
                 package='nav2_map_server',

diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py
@@ -37,6 +37,7 @@ def generate_launch_description():
     params_file = LaunchConfiguration('params_file')
     use_composition = LaunchConfiguration('use_composition')
     container_name = LaunchConfiguration('container_name')
+    container_name_full = (namespace, '/', container_name)
     use_respawn = LaunchConfiguration('use_respawn')
     log_level = LaunchConfiguration('log_level')
     default_nav_to_pose_bt_xml = LaunchConfiguration('default_nav_to_pose_bt_xml')
@@ -199,7 +200,7 @@ def generate_launch_description():
 
     load_composable_nodes = LoadComposableNodes(
         condition=IfCondition(use_composition),
-        target_container=container_name,
+        target_container=container_name_full,
         composable_node_descriptions=[
             ComposableNode(
                 package='nav2_controller',