Iron Sync 4: Jan 23, 2024

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 info@opennav.org.
+
 ## 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. 

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.2.5</version>
+ <version>1.2.6</version>
  <description>
  <p>
  amcl is a probabilistic localization system for a robot moving in

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

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()
@@ -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);

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.2.5</version>
+ <version>1.2.6</version>
  <description>TODO</description>
  <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
  <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>

nav2_behavior_tree/plugins/condition/transform_available_condition.cpp

@@ -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");

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);

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.2.5</version>
+ <version>1.2.6</version>
  <description>TODO</description>
  <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
  <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_behaviors/src/behavior_server.cpp

@@ -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(),

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.2.5</version>
+ <version>1.2.6</version>
  <description>Bringup scripts and configurations for the Nav2 stack</description>
  <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
  <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

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

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.2.5</version>
+ <version>1.2.6</version>
  <description>TODO</description>
  <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
  <license>Apache-2.0</license>