Skip to content

Commit

Permalink
Humble Sync 9: Jan 23, 2024 (#4064)
Browse files Browse the repository at this point in the history
* 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 <cihataltiparmak1@gmail.com>

* 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 <cihataltiparmak1@gmail.com>

---------

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* 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 <antonkesy@gmail.com>

* fix use after free (#3910)

* fix build mppi (#3927)

Signed-off-by: kevin <kevin@floatic.io>
Co-authored-by: kevin <kevin@floatic.io>

* 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 <cihataltiparmak1@gmail.com>
Signed-off-by: Anton Kesy <antonkesy@gmail.com>
Signed-off-by: kevin <kevin@floatic.io>
Co-authored-by: thandal <than@timbrel.org>
Co-authored-by: Anton Kesy <antonkesy@gmail.com>
Co-authored-by: CihatAltiparmak <cihataltiparmak1@gmail.com>
Co-authored-by: Anil Kumar Chavali <44644339+akchobby@users.noreply.github.com>
Co-authored-by: 정찬희 <60467877+ladianchad@users.noreply.github.com>
Co-authored-by: kevin <kevin@floatic.io>
Co-authored-by: Abiel Fernandez <empoleom@gmail.com>
Co-authored-by: Michael Ferguson <mfergs7@gmail.com>
  • Loading branch information
9 people authored Jan 24, 2024
1 parent d29de3d commit abe4149
Show file tree
Hide file tree
Showing 62 changed files with 244 additions and 102 deletions.
6 changes: 4 additions & 2 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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.
Expand Down
2 changes: 1 addition & 1 deletion nav2_amcl/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion nav2_behavior_tree/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
15 changes: 13 additions & 2 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -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;
}
}
Expand Down Expand Up @@ -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)
Expand All @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion nav2_behavior_tree/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
<maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
Expand Down
133 changes: 132 additions & 1 deletion nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_);
}
Expand All @@ -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) {
Expand All @@ -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);
Expand All @@ -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>
Expand All @@ -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")});
Expand All @@ -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)
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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) {
Expand All @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion nav2_behaviors/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
Expand Down
2 changes: 1 addition & 1 deletion nav2_bringup/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion nav2_bt_navigator/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
<license>Apache-2.0</license>
Expand Down
2 changes: 1 addition & 1 deletion nav2_collision_monitor/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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="alexey.merzlyakov@samsung.com">Alexey Merzlyakov</maintainer>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
Expand Down
2 changes: 1 addition & 1 deletion nav2_collision_monitor/src/collision_monitor_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion nav2_common/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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="carl.r.delsey@intel.com">Carl Delsey</maintainer>
<license>Apache-2.0</license>
Expand Down
2 changes: 1 addition & 1 deletion nav2_constrained_smoother/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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="vargovcik@robotechvision.com">Matej Vargovcik</maintainer>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
Expand Down
Loading

0 comments on commit abe4149

Please sign in to comment.