diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index a25e253e3e5..af37536267a 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.3.8 + 1.3.9

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 8ef74ccbdf4..42a905f81d9 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.3.8 + 1.3.9 Nav2 behavior tree wrappers, nodes, and utilities Michael Jeronimo Carlos Orduno diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 0fd2067e55a..6f2b66eb3a5 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.3.8 + 1.3.9 Nav2 behavior server Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index ac09db9fbb6..e4790604419 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.3.8 + 1.3.9 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 7f28a64167a..d00466879d5 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.3.8 + 1.3.9 Nav2 BT Navigator Server Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index c6caeeb5b2d..e41b61981d2 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -33,6 +33,7 @@ #include "nav2_util/twist_publisher.hpp" #include "nav2_util/twist_subscriber.hpp" #include "nav2_msgs/msg/collision_monitor_state.hpp" +#include "nav2_msgs/srv/toggle.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/polygon.hpp" @@ -200,6 +201,16 @@ class CollisionMonitor : public nav2_util::LifecycleNode */ void publishPolygons() const; + /** + * @brief Enable/disable collision monitor service callback + * @param request Service request + * @param response Service response + */ + void toggleCMServiceCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + // ----- Variables ----- /// @brief TF buffer @@ -227,6 +238,12 @@ class CollisionMonitor : public nav2_util::LifecycleNode rclcpp_lifecycle::LifecyclePublisher::SharedPtr collision_points_marker_pub_; + /// @brief Enable/disable collision monitor service + rclcpp::Service::SharedPtr toggle_cm_service_; + + /// @brief Whether collision monitor is enabled + bool enabled_; + /// @brief Whether main routine is active bool process_active_; diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index af35bb6af9e..7a4a0df2e96 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.3.8 + 1.3.9 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 c80e9159166..41495890e0c 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -25,12 +25,14 @@ #include "nav2_collision_monitor/kinematics.hpp" +using namespace std::placeholders; + namespace nav2_collision_monitor { CollisionMonitor::CollisionMonitor(const rclcpp::NodeOptions & options) : nav2_util::LifecycleNode("collision_monitor", "", options), - process_active_(false), robot_action_prev_{DO_NOTHING, {-1.0, -1.0, -1.0}, ""}, + enabled_{true}, process_active_(false), robot_action_prev_{DO_NOTHING, {-1.0, -1.0, -1.0}, ""}, stop_stamp_{0, 0, get_clock()->get_clock_type()}, stop_pub_timeout_(1.0, 0.0) { } @@ -82,6 +84,11 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state) collision_points_marker_pub_ = this->create_publisher( "~/collision_points_marker", 1); + // Toggle service initialization + toggle_cm_service_ = create_service( + "~/toggle", + std::bind(&CollisionMonitor::toggleCMServiceCallback, this, _1, _2, _3)); + nav2_util::declare_parameter_if_not_declared( node, "use_realtime_priority", rclcpp::ParameterValue(false)); bool use_realtime_priority = false; @@ -473,7 +480,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: } for (std::shared_ptr polygon : polygons_) { - if (!polygon->getEnabled()) { + if (!polygon->getEnabled() || !enabled_) { continue; } if (robot_action.action_type == STOP) { @@ -500,7 +507,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: } } - if (robot_action.polygon_name != robot_action_prev_.polygon_name) { + if ((robot_action.polygon_name != robot_action_prev_.polygon_name) && enabled_) { // Report changed robot behavior notifyActionState(robot_action, action_polygon); } @@ -653,12 +660,26 @@ void CollisionMonitor::notifyActionState( void CollisionMonitor::publishPolygons() const { for (std::shared_ptr polygon : polygons_) { - if (polygon->getEnabled()) { + if (polygon->getEnabled() || !enabled_) { polygon->publish(); } } } +void CollisionMonitor::toggleCMServiceCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response) +{ + enabled_ = request->enable; + + std::stringstream message; + message << "Collision monitor toggled " << (enabled_ ? "on" : "off") << " successfully"; + + response->success = true; + response->message = message.str(); +} + } // namespace nav2_collision_monitor #include "rclcpp_components/register_node_macro.hpp" diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index c4922518a4f..f41c97b992e 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -131,6 +131,11 @@ class CollisionMonitorWrapper : public nav2_collision_monitor::CollisionMonitor } return false; } + + bool isEnabled() const + { + return enabled_; + } }; // CollisionMonitorWrapper class Tester : public ::testing::Test @@ -180,6 +185,9 @@ class Tester : public ::testing::Test const std::chrono::nanoseconds & timeout); bool waitActionState(const std::chrono::nanoseconds & timeout); bool waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout); + bool waitToggle( + rclcpp::Client::SharedFuture result_future, + const std::chrono::nanoseconds & timeout); protected: void cmdVelOutCallback(geometry_msgs::msg::Twist::SharedPtr msg); @@ -215,6 +223,9 @@ class Tester : public ::testing::Test // Service client for setting CollisionMonitor parameters rclcpp::Client::SharedPtr parameters_client_; + + // Service client for toggling collision monitor + rclcpp::Client::SharedPtr toggle_client_; }; // Tester Tester::Tester() @@ -249,6 +260,8 @@ Tester::Tester() cm_->create_client( std::string( cm_->get_name()) + "/set_parameters"); + + toggle_client_ = cm_->create_client("~/toggle"); } Tester::~Tester() @@ -745,6 +758,22 @@ bool Tester::waitFuture( return false; } +bool Tester::waitToggle( + rclcpp::Client::SharedFuture result_future, + const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = cm_->now(); + while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { + std::future_status status = result_future.wait_for(10ms); + if (status == std::future_status::ready) { + return true; + } + rclcpp::spin_some(cm_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + bool Tester::waitActionState(const std::chrono::nanoseconds & timeout) { rclcpp::Time start_time = cm_->now(); @@ -786,6 +815,39 @@ void Tester::collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray: collision_points_marker_msg_ = msg; } +TEST_F(Tester, testToggleService) +{ + // Set parameters for collision monitor + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addSource(SCAN_NAME, SCAN); + setVectors({"Stop"}, {SCAN_NAME}); + + // Start collision monitor node + cm_->start(); + + auto request = std::make_shared(); + + // Disable test + request->enable = false; + { + auto result_future = toggle_client_->async_send_request(request); + ASSERT_TRUE(waitToggle(result_future.share(), 2s)); + } + ASSERT_FALSE(cm_->isEnabled()); + + // Enable test + request->enable = true; + { + auto result_future = toggle_client_->async_send_request(request); + ASSERT_TRUE(waitToggle(result_future.share(), 2s)); + } + ASSERT_TRUE(cm_->isEnabled()); + + // Stop the collision monitor + cm_->stop(); +} + TEST_F(Tester, testProcessStopSlowdownLimit) { rclcpp::Time curr_time = cm_->now(); diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 5894ff314df..c56c4e292c9 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.3.8 + 1.3.9 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 ab63c4c5ee1..ad57d0935bc 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.3.8 + 1.3.9 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 281705ddb30..14472553b75 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.3.8 + 1.3.9 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 06d58092f7c..e7a1ea513a9 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.3.8 + 1.3.9 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 babe9feabca..cc8e574aa23 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.3.8 + 1.3.9 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_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml index ad9bada2d9a..2884a1d4c64 100644 --- a/nav2_docking/opennav_docking/package.xml +++ b/nav2_docking/opennav_docking/package.xml @@ -2,7 +2,7 @@ opennav_docking - 1.3.8 + 1.3.9 A Task Server for robot charger docking Steve Macenski Apache-2.0 diff --git a/nav2_docking/opennav_docking_bt/package.xml b/nav2_docking/opennav_docking_bt/package.xml index 4efeb62e590..8c7152e36e2 100644 --- a/nav2_docking/opennav_docking_bt/package.xml +++ b/nav2_docking/opennav_docking_bt/package.xml @@ -2,7 +2,7 @@ opennav_docking_bt - 1.3.8 + 1.3.9 A set of BT nodes and XMLs for docking Steve Macenski Apache-2.0 diff --git a/nav2_docking/opennav_docking_core/package.xml b/nav2_docking/opennav_docking_core/package.xml index 73106139019..ac30d88d966 100644 --- a/nav2_docking/opennav_docking_core/package.xml +++ b/nav2_docking/opennav_docking_core/package.xml @@ -2,7 +2,7 @@ opennav_docking_core - 1.3.8 + 1.3.9 A set of headers for plugins core to the opennav docking framework Steve Macenski Apache-2.0 diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 961cbb734b5..6b34dba808d 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.3.8 + 1.3.9 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 84b7a1cb433..5b3c48ac31c 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.3.8 + 1.3.9 DWB core interfaces package Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 7a23d7bfe7b..73b3dbbfd78 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.3.8 + 1.3.9 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 5957c03493e..fcf171144cc 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.3.8 + 1.3.9 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 478999e7f42..57e6c374f0c 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.3.8 + 1.3.9 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 d7f496ec691..c6a3269ef70 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.3.8 + 1.3.9 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 128665c685c..c245b364df5 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.3.8 + 1.3.9 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 2011d4bee7d..1fbe214b99c 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.3.8 + 1.3.9 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_graceful_controller/package.xml b/nav2_graceful_controller/package.xml index 385f7de4c64..c1f4c5763ff 100644 --- a/nav2_graceful_controller/package.xml +++ b/nav2_graceful_controller/package.xml @@ -2,7 +2,7 @@ nav2_graceful_controller - 1.3.8 + 1.3.9 Graceful motion controller Alberto Tudela Apache-2.0 diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index aa6f01f515e..69090836a83 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.3.8 + 1.3.9 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml index cf7284ae895..3c493eb3956 100644 --- a/nav2_loopback_sim/package.xml +++ b/nav2_loopback_sim/package.xml @@ -2,7 +2,7 @@ nav2_loopback_sim - 1.3.8 + 1.3.9 A loopback simulator to replace physics simulation steve macenski Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index cef39c8ff0b..1e3c86ac807 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.3.8 + 1.3.9 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index ff6a5cf9d3f..ffb38c614d3 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.3.8 + 1.3.9 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 9837d19ce0c..d19b6a2c698 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -48,6 +48,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/ReloadDockDatabase.srv" "srv/SetRouteGraph.srv" "srv/DynamicEdges.srv" + "srv/Toggle.srv" "action/AssistedTeleop.action" "action/BackUp.action" "action/ComputePathToPose.action" diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 80c7a843858..9b72215a26c 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.3.8 + 1.3.9 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_msgs/srv/Toggle.srv b/nav2_msgs/srv/Toggle.srv new file mode 100644 index 00000000000..c4ed478ca6b --- /dev/null +++ b/nav2_msgs/srv/Toggle.srv @@ -0,0 +1,4 @@ +bool enable +--- +bool success +string message diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 150f31d3133..68214fe8497 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.3.8 + 1.3.9 Nav2 NavFn planner Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index c8819e56cff..c6b609146ed 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.3.8 + 1.3.9 Nav2 planner server package Steve Macenski Apache-2.0 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index c0eb744d00c..680140a41ee 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -436,9 +436,17 @@ void PlannerServer::computePlanThroughPoses() throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path"); } - // Concatenate paths together - concat_path.poses.insert( - concat_path.poses.end(), curr_path.poses.begin(), curr_path.poses.end()); + // Concatenate paths together, but skip the first pose of subsequent paths + // to avoid duplicating the connection point + if (i == 0) { + // First path: add all poses + concat_path.poses.insert( + concat_path.poses.end(), curr_path.poses.begin(), curr_path.poses.end()); + } else if (curr_path.poses.size() > 1) { + // Subsequent paths: skip the first pose to avoid duplication + concat_path.poses.insert( + concat_path.poses.end(), curr_path.poses.begin() + 1, curr_path.poses.end()); + } concat_path.header = curr_path.header; } diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index a0d06dc8813..6a9a67235a3 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.3.8 + 1.3.9 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 74b93713bb9..5fb5ef6e70c 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.3.8 + 1.3.9 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 77938e7060b..562f00d908f 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.3.8 + 1.3.9 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 94f207c831c..27094b720e5 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.3.8 + 1.3.9 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index c96c3faea9e..49c822d419c 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.3.8 + 1.3.9 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index 4216c22e094..ebdad2590cb 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -49,23 +49,21 @@ void LatticeMotionTable::initMotionModel( unsigned int & size_x_in, SearchInfo & search_info) { - size_x = size_x_in; - - if (current_lattice_filepath == search_info.lattice_filepath) { - return; - } - size_x = size_x_in; change_penalty = search_info.change_penalty; non_straight_penalty = search_info.non_straight_penalty; cost_penalty = search_info.cost_penalty; reverse_penalty = search_info.reverse_penalty; travel_distance_reward = 1.0f - search_info.retrospective_penalty; - current_lattice_filepath = search_info.lattice_filepath; allow_reverse_expansion = search_info.allow_reverse_expansion; rotation_penalty = search_info.rotation_penalty; min_turning_radius = search_info.minimum_turning_radius; + if (current_lattice_filepath == search_info.lattice_filepath) { + return; + } + current_lattice_filepath = search_info.lattice_filepath; + // Get the metadata about this minimum control set lattice_metadata = getLatticeMetadata(current_lattice_filepath); std::ifstream latticeFile(current_lattice_filepath); @@ -231,7 +229,8 @@ bool NodeLattice::isNodeValid( // Check primitive end pose // Convert grid quantization of primitives to radians, then collision checker quantization static const double bin_size = 2.0 * M_PI / collision_checker->getPrecomputedAngles().size(); - const double & angle = motion_table.getAngleFromBin(this->pose.theta) / bin_size; + const double angle = std::fmod(motion_table.getAngleFromBin(this->pose.theta), + 2.0 * M_PI) / bin_size; if (collision_checker->inCollision( this->pose.x, this->pose.y, angle /*bin in collision checker*/, traverse_unknown)) { @@ -270,7 +269,7 @@ bool NodeLattice::isNodeValid( if (is_backwards) { prim_pose._theta = std::fmod(it->_theta + M_PI, 2.0 * M_PI); } else { - prim_pose._theta = it->_theta; + prim_pose._theta = std::fmod(it->_theta, 2.0 * M_PI); } if (collision_checker->inCollision( prim_pose._x, diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index f6dcabd896d..125703ec9bd 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.3.8 + 1.3.9 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 06f4a2c9de2..dcaef774415 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.3.8 + 1.3.9 A sets of system-level tests for Nav2 usually involving full robot simulation Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 09f7a006625..287271abf94 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.3.8 + 1.3.9 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 209fbbda06a..bae6a99ed99 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.3.8 + 1.3.9 Nav2 utilities Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 5e4f4dd609d..07290b92df8 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.3.8 + 1.3.9 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index ef3d7f79126..fd5c3a62ec7 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.3.8 + 1.3.9 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/package.xml b/nav2_waypoint_follower/package.xml index 4e993e3a611..8678005b90b 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.3.8 + 1.3.9 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 7ee1cd993bc..c36cac1789f 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.3.8 + 1.3.9 ROS2 Navigation Stack