ros-navigation
diff --git a/‎README.md
Lines changed: 4 additions & 2 deletions b/‎README.md
Lines changed: 4 additions & 2 deletions
diff --git a/‎nav2_amcl/package.xml
Lines changed: 1 addition & 1 deletion b/‎nav2_amcl/package.xml
Lines changed: 1 addition & 1 deletion
diff --git a/‎nav2_behavior_tree/README.md
Lines changed: 1 addition & 1 deletion b/‎nav2_behavior_tree/README.md
Lines changed: 1 addition & 1 deletion
diff --git a/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
Lines changed: 13 additions & 2 deletions b/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
Lines changed: 13 additions & 2 deletions
diff --git a/‎nav2_behavior_tree/package.xml
Lines changed: 1 addition & 1 deletion b/‎nav2_behavior_tree/package.xml
Lines changed: 1 addition & 1 deletion
diff --git a/‎nav2_behavior_tree/plugins/condition/transform_available_condition.cpp
Lines changed: 1 addition & 1 deletion b/‎nav2_behavior_tree/plugins/condition/transform_available_condition.cpp
Lines changed: 1 addition & 1 deletion
diff --git a/‎nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp
Lines changed: 132 additions & 1 deletion b/‎nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp
Lines changed: 132 additions & 1 deletion
diff --git a/‎nav2_behaviors/package.xml
Lines changed: 1 addition & 1 deletion b/‎nav2_behaviors/package.xml
Lines changed: 1 addition & 1 deletion
diff --git a/‎nav2_behaviors/src/behavior_server.cpp
Lines changed: 2 additions & 2 deletions b/‎nav2_behaviors/src/behavior_server.cpp
Lines changed: 2 additions & 2 deletions
diff --git a/‎nav2_bringup/package.xml
Lines changed: 1 addition & 1 deletion b/‎nav2_bringup/package.xml
Lines changed: 1 addition & 1 deletion
@@ -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. 
 
@@ -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.2.5</version>
+  <version>1.2.6</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in
 
@@ -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
@@ -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()
@@ -266,7 +266,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;
       }
     }
@@ -300,6 +300,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)
@@ -308,6 +309,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);
 
@@ -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.2.5</version>
+  <version>1.2.6</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>
 
@@ -39,7 +39,7 @@ TransformAvailableCondition::TransformAvailableCondition(
     RCLCPP_FATAL(
       node_->get_logger(), "Child frame (%s) or parent frame (%s) were empty.",
       child_frame_.c_str(), parent_frame_.c_str());
-    exit(-1);
+    throw std::runtime_error("TransformAvailableCondition: Child or parent frames not provided!");
   }
 
   RCLCPP_DEBUG(node_->get_logger(), "Initialized an TransformAvailableCondition BT node");
 
@@ -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);
 
@@ -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.2.5</version>
+  <version>1.2.6</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
 
@@ -105,13 +105,13 @@ BehaviorServer::loadBehaviorPlugins()
   auto node = shared_from_this();
 
   for (size_t i = 0; i != behavior_ids_.size(); i++) {
-    behavior_types_[i] = nav2_util::get_plugin_type_param(node, behavior_ids_[i]);
     try {
+      behavior_types_[i] = nav2_util::get_plugin_type_param(node, behavior_ids_[i]);
       RCLCPP_INFO(
         get_logger(), "Creating behavior plugin %s of type %s",
         behavior_ids_[i].c_str(), behavior_types_[i].c_str());
       behaviors_.push_back(plugin_loader_.createUniqueInstance(behavior_types_[i]));
-    } catch (const pluginlib::PluginlibException & ex) {
+    } catch (const std::exception & ex) {
       RCLCPP_FATAL(
         get_logger(), "Failed to create behavior %s of type %s."
         " Exception: %s", behavior_ids_[i].c_str(), behavior_types_[i].c_str(),
 
@@ -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.2.5</version>
+  <version>1.2.6</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>
Original file line number	Diff line number	Diff line change
`@@ -63,4 +63,4 @@ The BehaviorTree engine has a run method that accepts an XML description of a BT`
`63`	`63`
`64`	`64`	`See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine.`
`65`	`65`
`66`		`-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/`
	`66`	`+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`
Original file line number	Diff line number	Diff line change
`@@ -39,7 +39,7 @@ TransformAvailableCondition::TransformAvailableCondition(`
`39`	`39`	`RCLCPP_FATAL(`
`40`	`40`	`node_->get_logger(), "Child frame (%s) or parent frame (%s) were empty.",`
`41`	`41`	`child_frame_.c_str(), parent_frame_.c_str());`
`42`		`- exit(-1);`
	`42`	`+ throw std::runtime_error("TransformAvailableCondition: Child or parent frames not provided!");`
`43`	`43`	`}`
`44`	`44`
`45`	`45`	`RCLCPP_DEBUG(node_->get_logger(), "Initialized an TransformAvailableCondition BT node");`