Humble Sync 9: Jan 23, 2024 (#4064)

* Update README.md: fix typo (#3842) * Update package.xml * fix typo (#3850) * adjust link to point to v3.8 of behavior tree docs (#3851) BT.CPP_v3 is used, thereby the correct docs should be linked * Fix bug in nav2_behavior_tree/bt_action_node (#3849) * Fix bug in nav2_behavior_tree/bt_action_node * Fixed the bug in halt function inside nav2_behavior_tree/plugin/action/bt_action_node.hpp * Added new case to nav2_behavior_tree/plugin/action/bt_action_node.hpp for testing the scenario to cancel * Refactored existing cases in nav2_behavior_tree/plugin/action/bt_action_node.hpp Signed-off-by: CihatAltiparmak <[email protected]> * Fix bug in nav2_behavior_tree/bt_action_node * Fixed the bug in halt function inside nav2_behavior_tree/plugin/action/bt_action_node.hpp * Added new case to nav2_behavior_tree/plugin/action/bt_action_node.hpp for testing the scenario to cancel * Refactored existing cases in nav2_behavior_tree/plugin/action/bt_action_node.hpp Signed-off-by: CihatAltiparmak <[email protected]> --------- Signed-off-by: CihatAltiparmak <[email protected]> * simplication of lattice_generator.py, fixes #3858 (#3859) * simplification of equation to compute the max_value/outer edge of the lattice based on number of headings. * Fix class doxygen * fix minor typos (#3892) Signed-off-by: Anton Kesy <[email protected]> * fix use after free (#3910) * fix build mppi (#3927) Signed-off-by: kevin <[email protected]> Co-authored-by: kevin <[email protected]> * Removing old TODOs * Fixed links for install and build in README (#3963) Currently the readme is linking to an invalida page in the docs (404 error). * fix a few outdated comments in smac planners (#3978) * Update README.md * bump to 1.1.13 for release --------- Signed-off-by: CihatAltiparmak <[email protected]> Signed-off-by: Anton Kesy <[email protected]> Signed-off-by: kevin <[email protected]> Co-authored-by: thandal <[email protected]> Co-authored-by: Anton Kesy <[email protected]> Co-authored-by: CihatAltiparmak <[email protected]> Co-authored-by: Anil Kumar Chavali <[email protected]> Co-authored-by: 정찬희 <[email protected]> Co-authored-by: kevin <[email protected]> Co-authored-by: Abiel Fernandez <[email protected]> Co-authored-by: Michael Ferguson <[email protected]>
ros-navigation · Jan 24, 2024 · abe4149 · abe4149
1 parent d29de3d
commit abe4149
Show file tree

Hide file tree

Showing 62 changed files with 244 additions and 102 deletions.
diff --git a/README.md b/README.md
@@ -9,8 +9,8 @@
 For detailed instructions on how to:
 - [Getting Started](https://navigation.ros.org/getting_started/index.html)
 - [Concepts](https://navigation.ros.org/concepts/index.html)
-- [Build](https://navigation.ros.org/build_instructions/index.html#build)
-- [Install](https://navigation.ros.org/build_instructions/index.html#install)
+- [Build](https://navigation.ros.org/development_guides/build_docs/index.html#build)
+- [Install](https://navigation.ros.org/development_guides/build_docs/index.html#install)
 - [General Tutorials](https://navigation.ros.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://navigation.ros.org/plugin_tutorials/index.html)
 - [Configure](https://navigation.ros.org/configuration/index.html)
 - [Navigation Plugins](https://navigation.ros.org/plugins/index.html)
@@ -20,6 +20,8 @@ For detailed instructions on how to:
 
 Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate).
 
+If you need professional services related to Nav2, please contact Open Navigation at [email protected].
+
 ## Our Sponsors
 
 Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community. 

diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md
@@ -63,4 +63,4 @@ The BehaviorTree engine has a run method that accepts an XML description of a BT
 
 See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine.
 
-For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/learn-the-basics/bt_basics/
+For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/3.8/learn-the-basics/BT_basics
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
@@ -162,7 +162,7 @@ class BtActionNode : public BT::ActionNodeBase
   }
 
   /**
-   * @brief Function to perform some user-defined operation whe the action is aborted.
+   * @brief Function to perform some user-defined operation when the action is aborted.
    * @return BT::NodeStatus Returns FAILURE by default, user may override return another value
    */
   virtual BT::NodeStatus on_aborted()
@@ -265,7 +265,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;
       }
     }
@@ -299,6 +299,7 @@ class BtActionNode : public BT::ActionNodeBase
   void halt() override
   {
     if (should_cancel_goal()) {
+      auto future_result = action_client_->async_get_result(goal_handle_);
       auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
       if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) !=
         rclcpp::FutureReturnCode::SUCCESS)
@@ -307,6 +308,16 @@ class BtActionNode : public BT::ActionNodeBase
           node_->get_logger(),
           "Failed to cancel action server for %s", action_name_.c_str());
       }
+
+      if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) !=
+        rclcpp::FutureReturnCode::SUCCESS)
+      {
+        RCLCPP_ERROR(
+          node_->get_logger(),
+          "Failed to get result for %s in node halt!", action_name_.c_str());
+      }
+
+      on_cancelled();
     }
 
     setStatus(BT::NodeStatus::IDLE);

diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behavior_tree</name>
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>

diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp
@@ -54,11 +54,17 @@ class FibonacciActionServer : public rclcpp::Node
     sleep_duration_ = sleep_duration;
   }
 
+  void setServerLoopRate(std::chrono::nanoseconds server_loop_rate)
+  {
+    server_loop_rate_ = server_loop_rate;
+  }
+
 protected:
   rclcpp_action::GoalResponse handle_goal(
     const rclcpp_action::GoalUUID &,
     std::shared_ptr<const test_msgs::action::Fibonacci::Goal>)
   {
+    RCLCPP_INFO(this->get_logger(), "Goal is received..");
     if (sleep_duration_ > 0ms) {
       std::this_thread::sleep_for(sleep_duration_);
     }
@@ -73,6 +79,13 @@ class FibonacciActionServer : public rclcpp::Node
 
   void handle_accepted(
     const std::shared_ptr<rclcpp_action::ServerGoalHandle<test_msgs::action::Fibonacci>> handle)
+  {
+    // this needs to return quickly to avoid blocking the executor, so spin up a new thread
+    std::thread{std::bind(&FibonacciActionServer::execute, this, _1), handle}.detach();
+  }
+
+  void execute(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<test_msgs::action::Fibonacci>> handle)
   {
     // this needs to return quickly to avoid blocking the executor, so spin up a new thread
     if (handle) {
@@ -88,8 +101,17 @@ class FibonacciActionServer : public rclcpp::Node
       sequence.push_back(0);
       sequence.push_back(1);
 
+      rclcpp::Rate rate(server_loop_rate_);
       for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
+        if (handle->is_canceling()) {
+          RCLCPP_INFO(this->get_logger(), "Goal is canceling.");
+          handle->canceled(result);
+          return;
+        }
+
+        RCLCPP_INFO(this->get_logger(), "Goal is feedbacking.");
         sequence.push_back(sequence[i] + sequence[i - 1]);
+        rate.sleep();
       }
 
       handle->succeed(result);
@@ -99,6 +121,7 @@ class FibonacciActionServer : public rclcpp::Node
 protected:
   rclcpp_action::Server<test_msgs::action::Fibonacci>::SharedPtr action_server_;
   std::chrono::milliseconds sleep_duration_;
+  std::chrono::nanoseconds server_loop_rate_;
 };
 
 class FibonacciAction : public nav2_behavior_tree::BtActionNode<test_msgs::action::Fibonacci>
@@ -121,6 +144,13 @@ class FibonacciAction : public nav2_behavior_tree::BtActionNode<test_msgs::actio
     return BT::NodeStatus::SUCCESS;
   }
 
+  BT::NodeStatus on_cancelled() override
+  {
+    config().blackboard->set<std::vector<int>>("sequence", result_.result->sequence);
+    config().blackboard->set<bool>("on_cancelled_triggered", true);
+    return BT::NodeStatus::SUCCESS;
+  }
+
   static BT::PortsList providedPorts()
   {
     return providedBasicPorts({BT::InputPort<int>("order", "Fibonacci order")});
@@ -144,6 +174,7 @@ class BTActionNodeTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>("server_timeout", 20ms);
     config_->blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 10ms);
     config_->blackboard->set<bool>("initial_pose_received", false);
+    config_->blackboard->set<bool>("on_cancelled_triggered", false);
 
     BT::NodeBuilder builder =
       [](const std::string & name, const BT::NodeConfiguration & config)
@@ -220,6 +251,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success)
 
   // setting a small action server goal handling duration
   action_server_->setHandleGoalSleepDuration(2ms);
+  action_server_->setServerLoopRate(10ns);
 
   // to keep track of the number of ticks it took to reach a terminal result
   int ticks = 0;
@@ -255,15 +287,22 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success)
   // start a new execution cycle with the previous BT to ensure previous state doesn't leak into
   // the new cycle
 
-  // halt BT for a new execution cycle
+  // halt BT for a new execution cycle,
+  // get if the on_cancelled is triggered from blackboard and assert
+  // that the on_cancelled triggers after halting node
+  RCLCPP_INFO(node_->get_logger(), "Tree is halting.");
   tree_->haltTree();
+  bool on_cancelled_triggered = config_->blackboard->get<bool>("on_cancelled_triggered");
+  EXPECT_EQ(on_cancelled_triggered, false);
 
   // setting a large action server goal handling duration
   action_server_->setHandleGoalSleepDuration(100ms);
+  action_server_->setServerLoopRate(10ns);
 
   // reset state variables
   ticks = 0;
   result = BT::NodeStatus::RUNNING;
+  config_->blackboard->set<bool>("on_cancelled_triggered", false);
 
   // main BT execution loop
   while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) {
@@ -300,6 +339,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure)
 
   // the action server will take 100ms before accepting the goal
   action_server_->setHandleGoalSleepDuration(100ms);
+  action_server_->setServerLoopRate(10ns);
 
   // to keep track of the number of ticks it took to reach a terminal result
   int ticks = 0;
@@ -327,14 +367,21 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure)
   // the new cycle
 
   // halt BT for a new execution cycle
+  // get if the on_cancel is triggered from blackboard and assert
+  // that the on_cancelled never can trigger after halting node
+  RCLCPP_INFO(node_->get_logger(), "Tree is halting.");
   tree_->haltTree();
+  bool on_cancelled_triggered = config_->blackboard->get<bool>("on_cancelled_triggered");
+  EXPECT_EQ(on_cancelled_triggered, false);
 
   // setting a small action server goal handling duration
   action_server_->setHandleGoalSleepDuration(25ms);
+  action_server_->setServerLoopRate(10ns);
 
   // reset state variables
   ticks = 0;
   result = BT::NodeStatus::RUNNING;
+  config_->blackboard->set<bool>("on_cancelled_triggered", false);
 
   // main BT execution loop
   while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) {
@@ -348,6 +395,90 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure)
   EXPECT_EQ(result, BT::NodeStatus::SUCCESS);
 }
 
+TEST_F(BTActionNodeTestFixture, test_server_cancel)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <Fibonacci order="1000000" />
+        </BehaviorTree>
+      </root>)";
+
+  // setting a server timeout smaller than the time the action server will take to accept the goal
+  // to simulate a server timeout scenario
+  config_->blackboard->set<std::chrono::milliseconds>("server_timeout", 100ms);
+  config_->blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 10ms);
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // the action server will take 2ms before accepting the goal
+  // and the feedback period of the action server will be 50ms
+  action_server_->setHandleGoalSleepDuration(2ms);
+  action_server_->setServerLoopRate(50ms);
+
+  // to keep track of the number of ticks it took to reach expected tick count
+  int ticks = 0;
+
+  BT::NodeStatus result = BT::NodeStatus::RUNNING;
+
+  // BT loop execution rate
+  rclcpp::WallRate loopRate(100ms);
+
+  // main BT execution loop
+  while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 5) {
+    result = tree_->tickRoot();
+    ticks++;
+    loopRate.sleep();
+  }
+
+  // halt BT for testing if the action node cancels the goal correctly
+  RCLCPP_INFO(node_->get_logger(), "Tree is halting.");
+  tree_->haltTree();
+
+  // get if the on_cancel is triggered from blackboard and assert
+  // that the on_cancel is triggered after halting node
+  bool on_cancelled_triggered = config_->blackboard->get<bool>("on_cancelled_triggered");
+  EXPECT_EQ(on_cancelled_triggered, true);
+
+  // ticks variable must be 5 because execution time of the action server
+  // is at least 1000000 x 50 ms
+  EXPECT_EQ(ticks, 5);
+
+  // send new goal to the action server for a new execution cycle
+
+  // the action server will take 2ms before accepting the goal
+  // and the feedback period of the action server will be 1000ms
+  action_server_->setHandleGoalSleepDuration(2ms);
+  action_server_->setServerLoopRate(50ms);
+
+  // reset state variable
+  ticks = 0;
+  config_->blackboard->set<bool>("on_cancelled_triggered", false);
+  result = BT::NodeStatus::RUNNING;
+
+  // main BT execution loop
+  while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 7) {
+    result = tree_->tickRoot();
+    ticks++;
+    loopRate.sleep();
+  }
+
+  // halt BT for testing if the action node cancels the goal correctly
+  RCLCPP_INFO(node_->get_logger(), "Tree is halting.");
+  tree_->haltTree();
+
+  // get if the on_cancel is triggered from blackboard and assert
+  // that the on_cancel is triggered after halting node
+  on_cancelled_triggered = config_->blackboard->get<bool>("on_cancelled_triggered");
+  EXPECT_EQ(on_cancelled_triggered, true);
+
+  // ticks variable must be 7 because execution time of the action server
+  // is at least 1000000 x 50 ms
+  EXPECT_EQ(ticks, 7);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behaviors</name>
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bringup</name>
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <description>Bringup scripts and configurations for the Nav2 stack</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

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_bt_navigator/package.xml b/nav2_bt_navigator/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bt_navigator</name>
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_collision_monitor</name>
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <description>Collision Monitor</description>
   <maintainer email="[email protected]">Alexey Merzlyakov</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp
@@ -388,7 +388,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
     printAction(robot_action, action_polygon);
   }
 
-  // Publish requred robot velocity
+  // Publish required robot velocity
   publishVelocity(robot_action);
 
   // Publish polygons for better visualization

diff --git a/nav2_common/package.xml b/nav2_common/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_common</name>
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <description>Common support functionality used throughout the navigation 2 stack</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_constrained_smoother</name>
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <description>Ceres constrained smoother</description>
   <maintainer email="[email protected]">Matej Vargovcik</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>