diff --git a/README.md b/README.md index 488eb28042..fe0364bc3b 100644 --- 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 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. diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 5725a6e634..cac7910c5b 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.1.12 + 1.1.13

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 index b2c30c432f..84e16e7d4e 100644 --- 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 index 44fe8b374e..3d8b694f1c 100644 --- 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 index c841446540..f8a2170ddb 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.12 + 1.1.13 TODO Michael Jeronimo Carlos Orduno 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 index d95cbd4a84..12c25d30d6 100644 --- 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) { + 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> 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> 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::SharedPtr action_server_; std::chrono::milliseconds sleep_duration_; + std::chrono::nanoseconds server_loop_rate_; }; class FibonacciAction : public nav2_behavior_tree::BtActionNode @@ -121,6 +144,13 @@ class FibonacciAction : public nav2_behavior_tree::BtActionNodeset>("sequence", result_.result->sequence); + config().blackboard->set("on_cancelled_triggered", true); + return BT::NodeStatus::SUCCESS; + } + static BT::PortsList providedPorts() { return providedBasicPorts({BT::InputPort("order", "Fibonacci order")}); @@ -144,6 +174,7 @@ class BTActionNodeTestFixture : public ::testing::Test config_->blackboard->set("server_timeout", 20ms); config_->blackboard->set("bt_loop_duration", 10ms); config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("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("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("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("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("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"( + + + + + )"; + + // 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("server_timeout", 100ms); + config_->blackboard->set("bt_loop_duration", 10ms); + + tree_ = std::make_shared(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("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("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("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 index 898573b9a3..5efaa152e2 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.12 + 1.1.13 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 559608e61f..1e41f7a1ff 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.12 + 1.1.13 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski 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 index d6e218d860..17a67b1623 100644 --- 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 index 4f89b94fe5..02a5b4cad2 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.12 + 1.1.13 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 0672683c27..db17606e37 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.12 + 1.1.13 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 69523e3c14..57125501cf 100644 --- 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 index 42b183c6e9..456c8167c8 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.12 + 1.1.13 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index b1879c5743..cde44f1696 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.12 + 1.1.13 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 8c1208f9cc..307180c8bf 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.12 + 1.1.13 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index e88f728efe..c4f2d2e77e 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.12 + 1.1.13 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index d957ed8550..41c31f39ac 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.1.12 + 1.1.13 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_costmap_2d/test/module_tests.cpp b/nav2_costmap_2d/test/module_tests.cpp index 989d81fe72..7a5fe811e4 100644 --- a/nav2_costmap_2d/test/module_tests.cpp +++ b/nav2_costmap_2d/test/module_tests.cpp @@ -1123,7 +1123,7 @@ TEST(costmap, testTrickyPropagation) { // Add a dynamic obstacle pcl::PointCloud c2; c2.points.resize(3); - // Dynamic obstacle that raytaces. + // Dynamic obstacle that raytraces. c2.points[0].x = 7.0; c2.points[0].y = 8.0; c2.points[0].z = 1.0; @@ -1167,7 +1167,7 @@ TEST(costmap, testTrickyPropagation) { pcl::PointCloud c; c.points.resize(1); - // Dynamic obstacle that raytaces the one at (3.0, 4.0). + // Dynamic obstacle that raytraces the one at (3.0, 4.0). c.points[0].x = 4.0; c.points[0].y = 5.0; c.points[0].z = 1.0; diff --git a/nav2_costmap_2d/test/unit/binary_filter_test.cpp b/nav2_costmap_2d/test/unit/binary_filter_test.cpp index 31bdfc025b..fa7fed4e10 100644 --- a/nav2_costmap_2d/test/unit/binary_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/binary_filter_test.cpp @@ -703,7 +703,7 @@ void TestNode::reset() TEST_F(TestNode, testBinaryState) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); ASSERT_TRUE(createBinaryFilter("map", 10.0)); @@ -718,7 +718,7 @@ TEST_F(TestNode, testBinaryState) TEST_F(TestNode, testBinaryStateScaled) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 100.0, -1.0); ASSERT_TRUE(createBinaryFilter("map", 35.0)); @@ -733,7 +733,7 @@ TEST_F(TestNode, testBinaryStateScaled) TEST_F(TestNode, testInvertedBinaryState) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); setDefaultState(true); @@ -749,7 +749,7 @@ TEST_F(TestNode, testInvertedBinaryState) TEST_F(TestNode, testOutOfBounds) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); ASSERT_TRUE(createBinaryFilter("map", 10.0)); @@ -764,7 +764,7 @@ TEST_F(TestNode, testOutOfBounds) TEST_F(TestNode, testInfoRePublish) { - // Initilize test system + // Initialize test system createMaps("map"); // Publish Info with incorrect dummy mask topic publishMaps(nav2_costmap_2d::BINARY_FILTER, "dummy_topic", 0.0, 1.0); @@ -805,7 +805,7 @@ TEST_F(TestNode, testMaskRePublish) TEST_F(TestNode, testIncorrectFilterType) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(INCORRECT_TYPE, MASK_TOPIC, 0.0, 1.0); ASSERT_FALSE(createBinaryFilter("map", 10.0)); @@ -817,7 +817,7 @@ TEST_F(TestNode, testIncorrectFilterType) TEST_F(TestNode, testDifferentFrame) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); ASSERT_TRUE(createBinaryFilter("odom", 10.0)); @@ -833,7 +833,7 @@ TEST_F(TestNode, testDifferentFrame) TEST_F(TestNode, testIncorrectFrame) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); ASSERT_TRUE(createBinaryFilter("odom", 10.0)); @@ -849,7 +849,7 @@ TEST_F(TestNode, testIncorrectFrame) TEST_F(TestNode, testResetState) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); ASSERT_TRUE(createBinaryFilter("map", 10.0)); diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp index 304b423f5c..3f236dd4d4 100644 --- a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp @@ -341,7 +341,7 @@ void TestNode::reset() TEST_F(TestNode, testFreeMasterLethalKeepout) { - // Initilize test system + // Initialize test system createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map"); publishMaps(); createKeepoutFilter("map"); @@ -356,7 +356,7 @@ TEST_F(TestNode, testFreeMasterLethalKeepout) TEST_F(TestNode, testUnknownMasterNonLethalKeepout) { - // Initilize test system + // Initialize test system createMaps( nav2_costmap_2d::NO_INFORMATION, (nav2_util::OCC_GRID_OCCUPIED - nav2_util::OCC_GRID_FREE) / 2, @@ -376,7 +376,7 @@ TEST_F(TestNode, testUnknownMasterNonLethalKeepout) TEST_F(TestNode, testFreeKeepout) { - // Initilize test system + // Initialize test system createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_FREE, "map"); publishMaps(); createKeepoutFilter("map"); @@ -395,7 +395,7 @@ TEST_F(TestNode, testFreeKeepout) TEST_F(TestNode, testUnknownKeepout) { - // Initilize test system + // Initialize test system createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_UNKNOWN, "map"); publishMaps(); createKeepoutFilter("map"); @@ -414,7 +414,7 @@ TEST_F(TestNode, testUnknownKeepout) TEST_F(TestNode, testInfoRePublish) { - // Initilize test system + // Initialize test system createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map"); publishMaps(); createKeepoutFilter("map"); @@ -433,7 +433,7 @@ TEST_F(TestNode, testInfoRePublish) TEST_F(TestNode, testMaskRePublish) { - // Initilize test system + // Initialize test system createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map"); publishMaps(); createKeepoutFilter("map"); @@ -451,7 +451,7 @@ TEST_F(TestNode, testMaskRePublish) TEST_F(TestNode, testDifferentFrames) { - // Initilize test system + // Initialize test system createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map"); publishMaps(); createKeepoutFilter("odom"); diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp index d419cade36..2e1d2b4e8c 100644 --- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp @@ -633,7 +633,7 @@ void TestNode::reset() TEST_F(TestNode, testPercentSpeedLimit) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0); EXPECT_TRUE(createSpeedFilter("map")); @@ -648,7 +648,7 @@ TEST_F(TestNode, testPercentSpeedLimit) TEST_F(TestNode, testIncorrectPercentSpeedLimit) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, -50.0, 2.0); EXPECT_TRUE(createSpeedFilter("map")); @@ -663,7 +663,7 @@ TEST_F(TestNode, testIncorrectPercentSpeedLimit) TEST_F(TestNode, testAbsoluteSpeedLimit) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5); EXPECT_TRUE(createSpeedFilter("map")); @@ -678,7 +678,7 @@ TEST_F(TestNode, testAbsoluteSpeedLimit) TEST_F(TestNode, testIncorrectAbsoluteSpeedLimit) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, -50.0, 2.0); EXPECT_TRUE(createSpeedFilter("map")); @@ -693,7 +693,7 @@ TEST_F(TestNode, testIncorrectAbsoluteSpeedLimit) TEST_F(TestNode, testOutOfBounds) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0); EXPECT_TRUE(createSpeedFilter("map")); @@ -708,7 +708,7 @@ TEST_F(TestNode, testOutOfBounds) TEST_F(TestNode, testInfoRePublish) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5); EXPECT_TRUE(createSpeedFilter("map")); @@ -728,7 +728,7 @@ TEST_F(TestNode, testInfoRePublish) TEST_F(TestNode, testMaskRePublish) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5); EXPECT_TRUE(createSpeedFilter("map")); @@ -747,7 +747,7 @@ TEST_F(TestNode, testMaskRePublish) TEST_F(TestNode, testIncorrectFilterType) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(INCORRECT_TYPE, 1.23, 4.5); EXPECT_FALSE(createSpeedFilter("map")); @@ -759,7 +759,7 @@ TEST_F(TestNode, testIncorrectFilterType) TEST_F(TestNode, testDifferentFrame) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0); EXPECT_TRUE(createSpeedFilter("odom")); diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 0e99b4609c..e784e64823 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.1.12 + 1.1.13 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 3bb4035cdb..56607270d2 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.1.12 + 1.1.13 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index a8ffe5a974..1dbf1dbd4d 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.1.12 + 1.1.13 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 1be7a8306f..7419bfd239 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.1.12 + 1.1.13 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 406a8443df..a21aef7e06 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.1.12 + 1.1.13 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 627be612b6..5675e9455f 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.1.12 + 1.1.13 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 0f253c4ce0..195ee76e46 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.1.12 + 1.1.13 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index e785710e8d..31adaf7278 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.1.12 + 1.1.13 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 0848de2034..2fc9ef060c 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.1.12 + 1.1.13 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 8ba7ad0271..0852f14e22 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.1.12 + 1.1.13 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 29dfde9672..1501e4321a 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -15,6 +15,10 @@ find_package(ament_cmake REQUIRED) find_package(xtensor REQUIRED) find_package(xsimd REQUIRED) +include_directories( + include +) + set(dependencies_pkgs rclcpp nav2_common @@ -90,7 +94,7 @@ set(libraries mppi_controller mppi_critics) foreach(lib IN LISTS libraries) target_compile_options(${lib} PUBLIC -fconcepts) - target_include_directories(${lib} PUBLIC include ${xsimd_INCLUDE_DIRS}) # ${OpenMP_INCLUDE_DIRS} + target_include_directories(${lib} PUBLIC ${xsimd_INCLUDE_DIRS}) # ${OpenMP_INCLUDE_DIRS} target_link_libraries(${lib} xtensor xtensor::optimize xtensor::use_xsimd) ament_target_dependencies(${lib} ${dependencies_pkgs}) endforeach() diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index d28cbb5672..1e8c7c2eba 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -187,7 +187,7 @@ controller_server: TrajectoryVisualizer: trajectory_step: 5 time_step: 3 - AckermannConstrains: + AckermannConstraints: min_turning_r: 0.2 critics: ["ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] ConstraintCritic: diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index db61bacd28..fceccc0ac1 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,10 +2,10 @@ nav2_mppi_controller - 1.1.12 + 1.1.13 nav2_mppi_controller - Aleksei Budyakov Steve Macenski + Aleksei Budyakov MIT ament_cmake diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp index db8667e940..485cc375b0 100644 --- a/nav2_mppi_controller/test/noise_generator_test.cpp +++ b/nav2_mppi_controller/test/noise_generator_test.cpp @@ -17,7 +17,9 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "nav2_mppi_controller/tools/noise_generator.hpp" +#include "nav2_mppi_controller/tools/parameters_handler.hpp" #include "nav2_mppi_controller/models/optimizer_settings.hpp" #include "nav2_mppi_controller/models/state.hpp" #include "nav2_mppi_controller/models/control_sequence.hpp" diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 8e6c2e4e50..38f83b4805 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -318,7 +318,7 @@ TEST(UtilsTests, SmootherTest) noisey_sequence.vy = 0.0 * xt::ones({30}); noisey_sequence.wz = 0.3 * xt::ones({30}); - // Make the sequence noisey + // Make the sequence noisy auto noises = xt::random::randn({30}, 0.0, 0.2); noisey_sequence.vx += noises; noisey_sequence.vy += noises; diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 49ec7f14bd..593337b085 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.1.12 + 1.1.13 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 8239816736..8139ee9838 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.1.12 + 1.1.13 TODO Steve Macenski Carlos Orduno diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 2ede7f38b8..d15ae89622 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -211,8 +211,6 @@ NavfnPlanner::makePlan( plan.header.stamp = clock_->now(); plan.header.frame_id = global_frame_; - // TODO(orduno): add checks for start and goal reference frame -- should be in global frame - double wx = start.position.x; double wy = start.position.y; @@ -263,9 +261,6 @@ NavfnPlanner::makePlan( map_goal[0] = mx; map_goal[1] = my; - // TODO(orduno): Explain why we are providing 'map_goal' to setStart(). - // Same for setGoal, seems reversed. Computing backwards? - planner_->setStart(map_goal); planner_->setGoal(map_start); if (use_astar_) { diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 58f154da09..36b13c0e5f 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.1.12 + 1.1.13 TODO Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index f095b39a25..fde5ca9939 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.1.12 + 1.1.13 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index ff3f2bbf91..19834eb5f8 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.1.12 + 1.1.13 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index ac4f352e34..192a0a4d65 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.1.12 + 1.1.13 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index 67ca36792d..4f4b03332b 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.1.12 + 1.1.13 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 4b4fe55111..001f5df8d6 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -70,8 +70,7 @@ class AStarAlgorithm typedef std::priority_queue, NodeComparator> NodeQueue; /** - * @brief A constructor for nav2_smac_planner::PlannerServer - * @param neighborhood The type of neighborhood to use for search (4 or 8 connected) + * @brief A constructor for nav2_smac_planner::AStarAlgorithm */ explicit AStarAlgorithm(const MotionModel & motion_model, const SearchInfo & search_info); @@ -191,7 +190,7 @@ class AStarAlgorithm inline NodePtr getNextNode(); /** - * @brief Get pointer to next goal in open set + * @brief Add a node to the open set * @param cost The cost to sort into the open set of the node * @param node Node pointer reference to add to open set */ @@ -199,8 +198,7 @@ class AStarAlgorithm /** * @brief Adds node to graph - * @param cost The cost to sort into the open set of the node - * @param node Node pointer reference to add to open set + * @param index Node index to add */ inline NodePtr addToGraph(const unsigned int & index); @@ -213,9 +211,8 @@ class AStarAlgorithm /** * @brief Get cost of heuristic of node - * @param node Node index current - * @param node Node index of new - * @return Heuristic cost between the nodes + * @param node Node pointer to get heuristic for + * @return Heuristic cost for node */ inline float getHeuristicCost(const NodePtr & node); diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp index 407cde5c79..9df951c4f3 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp @@ -47,7 +47,6 @@ class NodeBasic public: /** * @brief A constructor for nav2_smac_planner::NodeBasic - * @param cost_in The costmap cost at this node * @param index The index of this node for self-reference */ explicit NodeBasic(const unsigned int index) diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 7f6a82d31c..4fe6a284cf 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -58,8 +58,6 @@ struct LatticeMotionTable /** * @brief Initializing state lattice planner's motion model * @param size_x_in Size of costmap in X - * @param size_y_in Size of costmap in Y - * @param angle_quantization_in Size of costmap in bin sizes * @param search_info Parameters for searching */ void initMotionModel( diff --git a/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h b/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h index 5d3812a3a3..4d3977937b 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h +++ b/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h @@ -111,7 +111,7 @@ static Counts& counts() { # error Unsupported bitness #endif -// endianess +// endianness #ifdef _MSC_VER # define ROBIN_HOOD_PRIVATE_DEFINITION_LITTLE_ENDIAN() 1 # define ROBIN_HOOD_PRIVATE_DEFINITION_BIG_ENDIAN() 0 @@ -2132,7 +2132,7 @@ class Table return maxElements * MaxLoadFactor100 / 100; } - // we might be a bit inprecise, but since maxElements is quite large that doesn't matter + // we might be a bit imprecise, but since maxElements is quite large that doesn't matter return (maxElements / 100) * MaxLoadFactor100; } diff --git a/nav2_smac_planner/lattice_primitives/lattice_generator.py b/nav2_smac_planner/lattice_primitives/lattice_generator.py index 3bcf735de8..e8968c2009 100644 --- a/nav2_smac_planner/lattice_primitives/lattice_generator.py +++ b/nav2_smac_planner/lattice_primitives/lattice_generator.py @@ -119,7 +119,7 @@ def _get_heading_discretization(self, number_of_headings: int) -> list: A list of headings in radians """ - max_val = int((((number_of_headings + 4) / 4) - 1) / 2) + max_val = int(number_of_headings / 8) outer_edge_x = [] outer_edge_y = [] diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 56d4f3fa6d..c03b7de7fc 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.1.12 + 1.1.13 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 59afdb9299..3d25dd7698 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.1.12 + 1.1.13 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index e083dbd959..8a14df53eb 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.1.12 + 1.1.13 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp index 557f5b7959..98ddae63ac 100644 --- a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp @@ -140,7 +140,7 @@ bool SpinBehaviorTester::defaultSpinBehaviorTest( auto goal_msg = Spin::Goal(); goal_msg.target_yaw = target_yaw; - // Intialize fake costmap + // Initialize fake costmap if (make_fake_costmap_) { sendFakeCostmap(target_yaw); sendFakeOdom(0.0); @@ -158,7 +158,7 @@ bool SpinBehaviorTester::defaultSpinBehaviorTest( fabs(tf2::getYaw(initial_pose.pose.orientation))); RCLCPP_INFO(node_->get_logger(), "Before sending goal"); - // Intialize fake costmap + // Initialize fake costmap if (make_fake_costmap_) { sendFakeCostmap(target_yaw); sendFakeOdom(0.0); diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index b849e89908..7a5f8d4b7f 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -62,6 +62,9 @@ class NavFnPlannerTester : public nav2_planner::PlannerServer void setCostmap(nav2_util::Costmap * costmap) { + std::unique_lock lock( + *(costmap_ros_->getCostmap()->getMutex())); + nav2_msgs::msg::CostmapMetaData prop; nav2_msgs::msg::Costmap cm = costmap->get_costmap(prop); prop = cm.metadata; diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index f750f4fe3b..7366ee3fe4 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.1.12 + 1.1.13 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 9d42dee49a..86b4fab06c 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.1.12 + 1.1.13 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 9fe27742d5..3cf71d4e71 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.1.12 + 1.1.13 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp b/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp index baff10d684..07c391fff5 100644 --- a/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp +++ b/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp @@ -112,7 +112,7 @@ class VoxelGrid unsigned int marked_bits = *col >> 16; - // make sure the number of bits in each is below our thesholds + // make sure the number of bits in each is below our thresholds return !bitsBelowThreshold(marked_bits, marked_threshold); } @@ -146,7 +146,7 @@ class VoxelGrid unsigned int unknown_bits = uint16_t(*col >> 16) ^ uint16_t(*col); unsigned int marked_bits = *col >> 16; - // make sure the number of bits in each is below our thesholds + // make sure the number of bits in each is below our thresholds if (bitsBelowThreshold(unknown_bits, 1) && bitsBelowThreshold(marked_bits, 1)) { costmap[index] = 0; } @@ -392,7 +392,7 @@ class VoxelGrid unsigned int unknown_bits = uint16_t(*col >> 16) ^ uint16_t(*col); unsigned int marked_bits = *col >> 16; - // make sure the number of bits in each is below our thesholds + // make sure the number of bits in each is below our thresholds if (bitsBelowThreshold(marked_bits, marked_clear_threshold_)) { if (bitsBelowThreshold(unknown_bits, unknown_clear_threshold_)) { costmap_[offset] = free_cost_; diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index eecbeaa500..d9ad807fa4 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.1.12 + 1.1.13 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp index fc4aee5c5e..180acc1101 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp @@ -108,7 +108,7 @@ class PhotoAtWaypoint : public nav2_core::WaypointTaskExecutor sensor_msgs::msg::Image::SharedPtr curr_frame_msg_; // global logger rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")}; - // ros susbcriber to get camera image + // ros subscriber to get camera image rclcpp::Subscription::SharedPtr camera_image_subscriber_; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 90c74bf7b6..507251b514 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.1.12 + 1.1.13 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 8494a40f05..8e47a24f0e 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.1.12 + 1.1.13 ROS2 Navigation Stack