diff --git a/.circleci/config.yml b/.circleci/config.yml index d8e0554b13d..a2e609dd69b 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -532,7 +532,7 @@ jobs: _parameters: release_parameters: &release_parameters - packages_skip_regex: "nav2_system_tests" + packages_skip_regex: "'(nav2_system_tests|nav2_smac_planner|nav2_mppi_controller|nav2_route|nav2_rviz_plugins|nav2_rotation_shim_controller|nav2_waypoint_follower|nav2_smoother|opennav_docking|nav2_behaviors|nav2_bringup|navigation2)'" workflows: version: 2 diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index d7af79e860b..f80c6369789 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.4.0 + 1.4.1

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_amcl/src/motion_model/differential_motion_model.cpp b/nav2_amcl/src/motion_model/differential_motion_model.cpp index afac92bb894..5bfbe5ebc6d 100644 --- a/nav2_amcl/src/motion_model/differential_motion_model.cpp +++ b/nav2_amcl/src/motion_model/differential_motion_model.cpp @@ -19,6 +19,7 @@ * */ +#include #include #include "nav2_amcl/angleutils.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index db5f938a9fe..c67c4442a2f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -98,6 +98,12 @@ class BehaviorTreeEngine */ void resetGrootMonitor(); + /** + * @brief Function to register a BT from an XML file + * @param file_path Path to BT XML file + */ + void registerTreeFromFile(const std::string & file_path); + /** * @brief Function to explicitly reset all BT nodes to initial state * @param tree Tree to halt diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 3ce2f816e88..7af8b8eca0a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -55,6 +55,7 @@ class BtActionServer const std::string & action_name, const std::vector & plugin_lib_names, const std::string & default_bt_xml_filename, + const std::vector & search_directories, OnGoalReceivedCallback on_goal_received_callback, OnLoopCallback on_loop_callback, OnPreemptCallback on_preempt_callback, @@ -104,7 +105,8 @@ class BtActionServer * @return bool true if the resulting BT correspond to the one in bt_xml_filename. false * if something went wrong, and previous BT is maintained */ - bool loadBehaviorTree(const std::string & bt_xml_filename = ""); + bool loadBehaviorTree( + const std::string & bt_xml_filename = ""); /** * @brief Getter function for BT Blackboard @@ -247,6 +249,7 @@ class BtActionServer // The XML file that contains the Behavior Tree to create std::string current_bt_xml_filename_; std::string default_bt_xml_filename_; + std::vector search_directories_; // The wrapper class for the BT functionality std::unique_ptr bt_; @@ -285,7 +288,7 @@ class BtActionServer bool always_reload_bt_xml_ = false; // Parameters for Groot2 monitoring - bool enable_groot_monitoring_ = true; + bool enable_groot_monitoring_ = false; int groot_server_port_ = 1667; // User-provided callbacks diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 61271650b3c..bf9ecf203eb 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -39,12 +39,14 @@ BtActionServer::BtActionServer( const std::string & action_name, const std::vector & plugin_lib_names, const std::string & default_bt_xml_filename, + const std::vector & search_directories, OnGoalReceivedCallback on_goal_received_callback, OnLoopCallback on_loop_callback, OnPreemptCallback on_preempt_callback, OnCompletionCallback on_completion_callback) : action_name_(action_name), default_bt_xml_filename_(default_bt_xml_filename), + search_directories_(search_directories), plugin_lib_names_(plugin_lib_names), node_(parent), on_goal_received_callback_(on_goal_received_callback), @@ -245,6 +247,8 @@ void BtActionServer::setGrootMonitoring(const bool enable, const unsign template bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filename) { + namespace fs = std::filesystem; + // Empty filename is default for backward compatibility auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename; @@ -254,19 +258,38 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena return true; } - // if a new tree is created, than the Groot2 Publisher must be destroyed + // Reset any existing Groot2 monitoring bt_->resetGrootMonitor(); - // Read the input BT XML from the specified file into a string std::ifstream xml_file(filename); - if (!xml_file.good()) { setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, - "Couldn't open input XML file: " + filename); + "Couldn't open BT XML file: " + filename); return false; } - // Create the Behavior Tree from the XML input + const auto canonical_main_bt = fs::canonical(filename); + + // Register all XML behavior Subtrees found in the given directories + for (const auto & directory : search_directories_) { + try { + for (const auto & entry : fs::directory_iterator(directory)) { + if (entry.path().extension() == ".xml") { + // Skip registering the main tree file + if (fs::equivalent(fs::canonical(entry.path()), canonical_main_bt)) { + continue; + } + bt_->registerTreeFromFile(entry.path().string()); + } + } + } catch (const std::exception & e) { + setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, + "Exception reading behavior tree directory: " + std::string(e.what())); + return false; + } + } + + // Try to load the main BT tree try { tree_ = bt_->createTreeFromFile(filename, blackboard_); for (auto & subtree : tree_.subtrees) { @@ -280,15 +303,15 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena } } catch (const std::exception & e) { setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, - std::string("Exception when loading BT: ") + e.what()); + std::string("Exception when creating BT tree from file: ") + e.what()); return false; } + // Optional logging and monitoring topic_logger_ = std::make_unique(client_node_, tree_); current_bt_xml_filename_ = filename; - // Enable monitoring with Groot2 if (enable_groot_monitoring_) { bt_->addGrootMonitoring(&tree_, groot_server_port_); RCLCPP_DEBUG( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp index 6d258552a7f..3bd6c7c5c54 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp @@ -78,6 +78,11 @@ class ComputeAndTrackRouteAction : public BtActionNode feedback) override; + /** + * @brief Function to set all feedbacks and output ports to be null values + */ + void resetFeedbackAndOutputPorts(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports @@ -98,14 +103,36 @@ class ComputeAndTrackRouteAction : public BtActionNode( "use_poses", false, "Whether to use poses or IDs for start and goal"), - BT::OutputPort("execution_duration", + BT::OutputPort( + "execution_duration", "Time taken to compute and track route"), BT::OutputPort( "error_code_id", "The compute route error code"), BT::OutputPort( "error_msg", "The compute route error msg"), + BT::OutputPort( + "last_node_id", + "ID of the previous node"), + BT::OutputPort( + "next_node_id", + "ID of the next node"), + BT::OutputPort( + "current_edge_id", + "ID of current edge"), + BT::OutputPort( + "route", + "List of RouteNodes to go from start to end"), + BT::OutputPort( + "path", + "Path created by ComputeAndTrackRoute node"), + BT::OutputPort( + "rerouted", + "Whether the plan has rerouted"), }); } + +protected: + Action::Feedback feedback_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp index 729271a8c5a..c877dbda77c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp @@ -29,7 +29,7 @@ class WouldAPlannerRecoveryHelp : public AreErrorCodesPresent using Action = nav2_msgs::action::ComputePathToPose; using ActionResult = Action::Result; using ThroughAction = nav2_msgs::action::ComputePathThroughPoses; - using ThroughActionResult = Action::Result; + using ThroughActionResult = ThroughAction::Result; public: WouldAPlannerRecoveryHelp( diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 4439393afa4..51da3ca6de3 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -128,6 +128,12 @@ Duration to compute route "Compute route to pose error code" "Compute route to pose error message" + "ID of the previous node" + "ID of the next node" + "ID of the current edge" + "List of RouteNodes to go from start to end" + "Path created by ComputeAndTrackRoute node" + "Whether the plan has rerouted" diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 31da48c1868..dbbf6fe8b77 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.4.0 + 1.4.1 Nav2 behavior tree wrappers, nodes, and utilities Michael Jeronimo Carlos Orduno diff --git a/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp b/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp index eb2edc19b76..bddc186149c 100644 --- a/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp @@ -26,6 +26,14 @@ ComputeAndTrackRouteAction::ComputeAndTrackRouteAction( const BT::NodeConfiguration & conf) : BtActionNode(xml_tag_name, action_name, conf) { + nav_msgs::msg::Path empty_path; + nav2_msgs::msg::Route empty_route; + feedback_.last_node_id = 0; + feedback_.next_node_id = 0; + feedback_.current_edge_id = 0; + feedback_.route = empty_route; + feedback_.path = empty_path; + feedback_.rerouted = false; } void ComputeAndTrackRouteAction::on_tick() @@ -52,6 +60,7 @@ void ComputeAndTrackRouteAction::on_tick() BT::NodeStatus ComputeAndTrackRouteAction::on_success() { + resetFeedbackAndOutputPorts(); setOutput("execution_duration", result_.result->execution_duration); setOutput("error_code_id", ActionResult::NONE); setOutput("error_msg", ""); @@ -60,6 +69,7 @@ BT::NodeStatus ComputeAndTrackRouteAction::on_success() BT::NodeStatus ComputeAndTrackRouteAction::on_aborted() { + resetFeedbackAndOutputPorts(); setOutput("execution_duration", builtin_interfaces::msg::Duration()); setOutput("error_code_id", result_.result->error_code); setOutput("error_msg", result_.result->error_msg); @@ -68,6 +78,7 @@ BT::NodeStatus ComputeAndTrackRouteAction::on_aborted() BT::NodeStatus ComputeAndTrackRouteAction::on_cancelled() { + resetFeedbackAndOutputPorts(); // Set empty error code, action was cancelled setOutput("execution_duration", builtin_interfaces::msg::Duration()); setOutput("error_code_id", ActionResult::NONE); @@ -82,7 +93,7 @@ void ComputeAndTrackRouteAction::on_timeout() } void ComputeAndTrackRouteAction::on_wait_for_result( - std::shared_ptr/*feedback*/) + std::shared_ptr feedback) { // Check for request updates to the goal bool use_poses = false, use_start = false; @@ -129,6 +140,34 @@ void ComputeAndTrackRouteAction::on_wait_for_result( if (goal_updated_) { on_tick(); } + + if (feedback) { + feedback_ = *feedback; + setOutput("last_node_id", feedback_.last_node_id); + setOutput("next_node_id", feedback_.next_node_id); + setOutput("current_edge_id", feedback_.current_edge_id); + setOutput("route", feedback_.route); + setOutput("path", feedback_.path); + setOutput("rerouted", feedback_.rerouted); + } +} + +void ComputeAndTrackRouteAction::resetFeedbackAndOutputPorts() +{ + nav_msgs::msg::Path empty_path; + nav2_msgs::msg::Route empty_route; + feedback_.last_node_id = 0; + feedback_.next_node_id = 0; + feedback_.current_edge_id = 0; + feedback_.route = empty_route; + feedback_.path = empty_path; + feedback_.rerouted = false; + setOutput("last_node_id", feedback_.last_node_id); + setOutput("next_node_id", feedback_.next_node_id); + setOutput("current_edge_id", feedback_.current_edge_id); + setOutput("route", feedback_.route); + setOutput("path", feedback_.path); + setOutput("rerouted", feedback_.rerouted); } } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 782a9660a8c..8cc80dfa6f4 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -101,6 +101,13 @@ BehaviorTreeEngine::createTreeFromFile( return factory_.createTreeFromFile(file_path, blackboard); } +/// @brief Register a tree from an XML file and return the tree +void BehaviorTreeEngine::registerTreeFromFile( + const std::string & file_path) +{ + factory_.registerBehaviorTreeFromFile(file_path); +} + void BehaviorTreeEngine::addGrootMonitoring( BT::Tree * tree, diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 5c983cb3554..8e292bcc5af 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.4.0 + 1.4.1 Nav2 behavior server Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index dc5ebc7bb19..2999e6bcd24 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.4.0 + 1.4.1 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index cfbcdc858bf..22aa3e86345 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -58,6 +58,8 @@ bt_navigator: plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" enable_groot_monitoring: false groot_server_port: 1669 + bt_search_directories: + - $(find-pkg-share nav2_bt_navigator)/behavior_trees # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index d7a8e903bc3..60d78a03e11 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.4.0 + 1.4.1 Nav2 BT Navigator Server Michael Jeronimo Apache-2.0 diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index a28e1c59586..25bf28da4b1 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -92,7 +92,6 @@ bool NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) { auto bt_xml_filename = goal->behavior_tree; - if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, "Error loading XML file: " + bt_xml_filename + ". Navigation canceled."); diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index a421db4c3f6..e42c2745900 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -99,7 +99,6 @@ bool NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) { auto bt_xml_filename = goal->behavior_tree; - if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, std::string("Error loading XML file: ") + bt_xml_filename + ". Navigation canceled."); diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index bb166d9bbf4..0cba6e5331a 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.4.0 + 1.4.1 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 7aa6a3925eb..ac5307df117 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.4.0 + 1.4.1 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 09f4d27015c..bbf6ca2ed8f 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.4.0 + 1.4.1 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 1532aecbf07..86751b1cfcd 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.4.0 + 1.4.1 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp index 513aab335d3..eacceb7c603 100644 --- a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp +++ b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp @@ -201,12 +201,22 @@ class BehaviorTreeNavigator : public NavigatorBase // get the default behavior tree for this navigator std::string default_bt_xml_filename = getDefaultBTFilepath(parent_node); + if (!node->has_parameter("bt_search_directories")) { + node->declare_parameter( + "bt_search_directories", + std::vector{ament_index_cpp::get_package_share_directory( + "nav2_bt_navigator") + "/behavior_trees"} + ); + } + auto search_directories = node->get_parameter("bt_search_directories").as_string_array(); + // Create the Behavior Tree Action Server for this navigator bt_action_server_ = std::make_unique>( node, getName(), plugin_lib_names, default_bt_xml_filename, + search_directories, std::bind(&BehaviorTreeNavigator::onGoalReceived, this, std::placeholders::_1), std::bind(&BehaviorTreeNavigator::onLoop, this), std::bind(&BehaviorTreeNavigator::onPreempt, this, std::placeholders::_1), diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 044eb215676..0bad5ca9384 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.4.0 + 1.4.1 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index 307385bd4a8..c18dc757c45 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -153,6 +153,15 @@ class StaticLayer : public CostmapLayer */ unsigned char interpretValue(unsigned char value); + /** + * @brief Check if two double values are equal within a given epsilon + * @param a First double value + * @param b Second double value + * @param epsilon The tolerance for equality check + * @return True if the values are equal within the tolerance, false otherwise + */ + bool isEqual(double a, double b, double epsilon); + /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 9a7e1146cb8..a96b8c48058 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.4.0 + 1.4.1 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/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index 6a5d83dd583..9931b7081bd 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -219,7 +219,7 @@ unsigned char CostmapFilter::getMaskCost( { const unsigned int index = my * filter_mask->info.width + mx; - const char data = filter_mask->data[index]; + const signed char data = filter_mask->data[index]; if (data == nav2_util::OCC_GRID_UNKNOWN) { return NO_INFORMATION; } else { diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index c1717507bec..8ba0820c1b7 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -46,6 +46,7 @@ #include "pluginlib/class_list_macros.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" #include "nav2_costmap_2d/costmap_math.hpp" +#include "nav2_util/node_utils.hpp" #include "rclcpp/version.h" PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::ObstacleLayer, nav2_costmap_2d::Layer) @@ -100,6 +101,8 @@ void ObstacleLayer::onInitialize() node->get_parameter("track_unknown_space", track_unknown_space); node->get_parameter("transform_tolerance", transform_tolerance); node->get_parameter(name_ + "." + "observation_sources", topics_string); + double tf_filter_tolerance = nav2_util::declare_or_get_parameter(node, name_ + "." + + "tf_filter_tolerance", 0.05); int combination_method_param{}; node->get_parameter(name_ + "." + "combination_method", combination_method_param); @@ -268,7 +271,8 @@ void ObstacleLayer::onInitialize() observation_subscribers_.push_back(sub); observation_notifiers_.push_back(filter); - observation_notifiers_.back()->setTolerance(rclcpp::Duration::from_seconds(0.05)); + observation_notifiers_.back()->setTolerance(rclcpp::Duration::from_seconds( + tf_filter_tolerance)); } else { std::shared_ptrgetCostmap(); if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x || master->getSizeInCellsY() != size_y || - master->getResolution() != new_map.info.resolution || - master->getOriginX() != new_map.info.origin.position.x || - master->getOriginY() != new_map.info.origin.position.y || + !isEqual(master->getResolution(), new_map.info.resolution, EPSILON) || + !isEqual(master->getOriginX(), new_map.info.origin.position.x, EPSILON) || + !isEqual(master->getOriginY(), new_map.info.origin.position.y, EPSILON) || !layered_costmap_->isSizeLocked())) { // Update the size of the layered costmap (and all layers, including this one) @@ -208,9 +210,9 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) new_map.info.origin.position.y, true); } else if (size_x_ != size_x || size_y_ != size_y || // NOLINT - resolution_ != new_map.info.resolution || - origin_x_ != new_map.info.origin.position.x || - origin_y_ != new_map.info.origin.position.y) + !isEqual(resolution_, new_map.info.resolution, EPSILON) || + !isEqual(origin_x_, new_map.info.origin.position.x, EPSILON) || + !isEqual(origin_y_, new_map.info.origin.position.y, EPSILON)) { // only update the size of the costmap stored locally in this layer RCLCPP_INFO( @@ -471,6 +473,18 @@ StaticLayer::updateCosts( current_ = true; } +/** + * @brief Check if two floating point numbers are equal within a given epsilon + * @param a First number + * @param b Second number + * @param epsilon Tolerance for equality check + * @return True if numbers are equal within the tolerance, false otherwise + */ +bool StaticLayer::isEqual(double a, double b, double epsilon) +{ + return std::abs(a - b) < epsilon; +} + /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index af1ac0a016f..042308afc66 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -64,6 +64,7 @@ void VoxelLayer::onInitialize() declareParameter("enabled", rclcpp::ParameterValue(true)); declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true)); + declareParameter("min_obstacle_height", rclcpp::ParameterValue(0.0)); declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0)); declareParameter("z_voxels", rclcpp::ParameterValue(10)); declareParameter("origin_z", rclcpp::ParameterValue(0.0)); @@ -80,6 +81,7 @@ void VoxelLayer::onInitialize() node->get_parameter(name_ + "." + "enabled", enabled_); node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); + node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_); node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_); node->get_parameter(name_ + "." + "z_voxels", size_z_); node->get_parameter(name_ + "." + "origin_z", origin_z_); @@ -195,6 +197,11 @@ void VoxelLayer::updateBounds( sensor_msgs::PointCloud2ConstIterator iter_z(cloud, "z"); for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + // if the obstacle is too low, we won't add it + if (*iter_z < min_obstacle_height_) { + continue; + } + // if the obstacle is too high or too far away from the robot we won't add it if (*iter_z > max_obstacle_height_) { continue; @@ -344,31 +351,47 @@ void VoxelLayer::raytraceFreespace( double b = wpy - oy; double c = wpz - oz; double t = 1.0; + bool wp_outside = false; // we can only raytrace to a maximum z height if (wpz > map_end_z) { // we know we want the vector's z value to be max_z t = std::max(0.0, std::min(t, (map_end_z - 0.01 - oz) / c)); + wp_outside = true; } else if (wpz < origin_z_) { // and we can only raytrace down to the floor // we know we want the vector's z value to be 0.0 t = std::min(t, (origin_z_ - oz) / c); + wp_outside = true; } // the minimum value to raytrace from is the origin if (wpx < origin_x_) { t = std::min(t, (origin_x_ - ox) / a); + wp_outside = true; } if (wpy < origin_y_) { t = std::min(t, (origin_y_ - oy) / b); + wp_outside = true; } // the maximum value to raytrace to is the end of the map if (wpx > map_end_x) { t = std::min(t, (map_end_x - ox) / a); + wp_outside = true; } if (wpy > map_end_y) { t = std::min(t, (map_end_y - oy) / b); + wp_outside = true; + } + + constexpr double wp_epsilon = 1e-5; + if (wp_outside) { + if (t > 0.0) { + t -= wp_epsilon; + } else if (t < 0.0) { + t += wp_epsilon; + } } wpx = ox + a * t; @@ -501,7 +524,9 @@ VoxelLayer::dynamicParametersCallback( } if (param_type == ParameterType::PARAMETER_DOUBLE) { - if (param_name == name_ + "." + "max_obstacle_height") { + if (param_name == name_ + "." + "min_obstacle_height") { + min_obstacle_height_ = parameter.as_double(); + } else if (param_name == name_ + "." + "max_obstacle_height") { max_obstacle_height_ = parameter.as_double(); } else if (param_name == name_ + "." + "origin_z") { origin_z_ = parameter.as_double(); diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml index 5e8b0070525..ef88c965745 100644 --- a/nav2_docking/opennav_docking/package.xml +++ b/nav2_docking/opennav_docking/package.xml @@ -2,7 +2,7 @@ opennav_docking - 1.4.0 + 1.4.1 A Task Server for robot charger docking Steve Macenski Apache-2.0 diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index c852d7d8d78..34c86094383 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -341,7 +341,7 @@ void DockingServer::dockRobot() } catch (opennav_docking_core::DockingException & e) { if (++num_retries_ > max_retries_) { RCLCPP_ERROR(get_logger(), "Failed to dock, all retries have been used"); - throw e; + throw; } RCLCPP_WARN(get_logger(), "Docking failed, will retry: %s", e.what()); } diff --git a/nav2_docking/opennav_docking_bt/package.xml b/nav2_docking/opennav_docking_bt/package.xml index d2071203bfe..806ca624fd1 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.4.0 + 1.4.1 A set of BT nodes and XMLs for docking Steve Macenski Apache-2.0 diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/docking_exceptions.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/docking_exceptions.hpp index c840c2bdde6..f8862a9889c 100644 --- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/docking_exceptions.hpp +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/docking_exceptions.hpp @@ -15,6 +15,7 @@ #ifndef OPENNAV_DOCKING_CORE__DOCKING_EXCEPTIONS_HPP_ #define OPENNAV_DOCKING_CORE__DOCKING_EXCEPTIONS_HPP_ +#include #include #include diff --git a/nav2_docking/opennav_docking_core/package.xml b/nav2_docking/opennav_docking_core/package.xml index 57a3d15b386..632d5925a2a 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.4.0 + 1.4.1 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 695cebd19b7..adb50b1d2ac 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.4.0 + 1.4.1 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 8b55d813004..08c53320a0b 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.4.0 + 1.4.1 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 a697e486021..96fcf622922 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.4.0 + 1.4.1 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 594c7619a41..22273441ead 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.4.0 + 1.4.1 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 ba51d4d06e1..017617e91be 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.4.0 + 1.4.1 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 7d8363bcfc2..c5d35e6344b 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.4.0 + 1.4.1 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 5f5ed5f97bc..7134e1ad720 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.4.0 + 1.4.1 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 060a6c2f701..ddcc0275379 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.4.0 + 1.4.1 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 be7ee9c4806..42f4c8af4e8 100644 --- a/nav2_graceful_controller/package.xml +++ b/nav2_graceful_controller/package.xml @@ -2,7 +2,7 @@ nav2_graceful_controller - 1.4.0 + 1.4.1 Graceful motion controller Alberto Tudela Apache-2.0 diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index f70a336d141..db03c4d278b 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.4.0 + 1.4.1 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 9d55a6d4837..65689612bac 100644 --- a/nav2_loopback_sim/package.xml +++ b/nav2_loopback_sim/package.xml @@ -2,17 +2,18 @@ nav2_loopback_sim - 1.4.0 + 1.4.1 A loopback simulator to replace physics simulation steve macenski Apache-2.0 - rclpy geometry_msgs + nav2_simple_commander nav_msgs - tf_transformations - tf2_ros python3-transforms3d + rclpy + tf2_ros + tf_transformations ament_copyright ament_flake8 diff --git a/nav2_map_server/include/nav2_map_server/map_io.hpp b/nav2_map_server/include/nav2_map_server/map_io.hpp index 97dc81821c9..5fc34937ee1 100644 --- a/nav2_map_server/include/nav2_map_server/map_io.hpp +++ b/nav2_map_server/include/nav2_map_server/map_io.hpp @@ -98,6 +98,14 @@ bool saveMapToFile( const nav_msgs::msg::OccupancyGrid & map, const SaveParameters & save_parameters); +/** + * @brief to_string_with_precision + * @param value + * @param precision + * @return + */ +std::string to_string_with_precision(double value, int precision); + /** * @brief Expand ~/ to home user dir. * @param yaml_filename Name of input YAML file. diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 9b4d47790b7..73badd52034 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.4.0 + 1.4.1 Refactored map server for ROS2 Navigation diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index e95d192a9af..989f38f1211 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -594,13 +594,15 @@ void tryWriteMapToFile( std::string image_name = mapdatafile.substr(file_name_index + 1); YAML::Emitter e; - e << YAML::Precision(3); + e << YAML::Precision(7); e << YAML::BeginMap; e << YAML::Key << "image" << YAML::Value << image_name; e << YAML::Key << "mode" << YAML::Value << map_mode_to_string(save_parameters.mode); - e << YAML::Key << "resolution" << YAML::Value << map.info.resolution; - e << YAML::Key << "origin" << YAML::Flow << YAML::BeginSeq << map.info.origin.position.x << - map.info.origin.position.y << yaw << YAML::EndSeq; + e << YAML::Key << "resolution" << YAML::Value << to_string_with_precision(map.info.resolution, + 3); + e << YAML::Key << "origin" << YAML::Flow << YAML::BeginSeq << + to_string_with_precision(map.info.origin.position.x, 3) << + to_string_with_precision(map.info.origin.position.y, 3) << yaw << YAML::EndSeq; e << YAML::Key << "negate" << YAML::Value << 0; if (save_parameters.mode == MapMode::Trinary) { @@ -611,8 +613,10 @@ void tryWriteMapToFile( e << YAML::Key << "occupied_thresh" << YAML::Value << 0.65; e << YAML::Key << "free_thresh" << YAML::Value << 0.196; } else { - e << YAML::Key << "occupied_thresh" << YAML::Value << save_parameters.occupied_thresh; - e << YAML::Key << "free_thresh" << YAML::Value << save_parameters.free_thresh; + e << YAML::Key << "occupied_thresh" << YAML::Value << + to_string_with_precision(save_parameters.occupied_thresh, 3); + e << YAML::Key << "free_thresh" << YAML::Value << + to_string_with_precision(save_parameters.free_thresh, 3); } if (!e.good()) { @@ -648,4 +652,12 @@ bool saveMapToFile( return true; } +std::string to_string_with_precision(double value, int precision) +{ + std::ostringstream out; + out << std::fixed << std::setprecision(precision) << value; + + return out.str(); +} + } // namespace nav2_map_server diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 08e7ef8a23e..882681223cb 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.4.0 + 1.4.1 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index b03160cfd6a..6e63361ede1 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -339,6 +339,9 @@ void Optimizer::integrateStateVelocities( auto traj_yaws = trajectory.col(2); const size_t n_size = traj_yaws.size(); + if (n_size == 0) { + return; + } float last_yaw = initial_yaw; for(size_t i = 0; i != n_size; i++) { diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 12a085fbf88..2d4ce574e5e 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.4.0 + 1.4.1 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 94e70c2f61c..e3a386f0638 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.4.0 + 1.4.1 Nav2 NavFn planner Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 68d97a5c0d0..ed1547f39fd 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.4.0 + 1.4.1 Nav2 planner server package Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index f49f9b31420..2caec97ff32 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -91,6 +91,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | | `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | | `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvature calculation (to avoid oscillations at the end of the path) | +| `min_distance_to_obstacle` | The shortest distance at which the robot is allowed to be from an obstacle along its trajectory. Set <= 0.0 to disable. It is limited to maximum distance of lookahead distance selected. | Example fully-described XML with default parameter values: diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index 21edd45558c..3e934ed1f66 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -42,6 +42,7 @@ struct Parameters double min_approach_linear_velocity; double approach_velocity_scaling_dist; double max_allowed_time_to_collision_up_to_carrot; + double min_distance_to_obstacle; bool use_regulated_linear_velocity_scaling; bool use_cost_regulated_linear_velocity_scaling; double cost_scaling_dist; diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 41b2bade1ee..700609c053c 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.4.0 + 1.4.1 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp index ac654fcab76..89a5a2a1bff 100644 --- a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp @@ -92,8 +92,16 @@ bool CollisionChecker::isCollisionImminent( curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); // only forward simulate within time requested + double max_allowed_time_to_collision_check = params_->max_allowed_time_to_collision_up_to_carrot; + if (params_->min_distance_to_obstacle > 0.0) { + max_allowed_time_to_collision_check = std::max( + params_->max_allowed_time_to_collision_up_to_carrot, + params_->min_distance_to_obstacle / std::max(std::abs(linear_vel), + params_->min_approach_linear_velocity) + ); + } int i = 1; - while (i * projection_time < params_->max_allowed_time_to_collision_up_to_carrot) { + while (i * projection_time < max_allowed_time_to_collision_check) { i++; // apply velocity at curr_pose over distance diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index f116768626c..7a704e6cee0 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -61,6 +61,9 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_distance_to_obstacle", + rclcpp::ParameterValue(-1.0)); declare_parameter_if_not_declared( node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( @@ -131,6 +134,9 @@ ParameterHandler::ParameterHandler( node->get_parameter( plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", params_.max_allowed_time_to_collision_up_to_carrot); + node->get_parameter( + plugin_name_ + ".min_distance_to_obstacle", + params_.min_distance_to_obstacle); node->get_parameter( plugin_name_ + ".use_regulated_linear_velocity_scaling", params_.use_regulated_linear_velocity_scaling); @@ -285,6 +291,8 @@ ParameterHandler::updateParametersCallback( params_.curvature_lookahead_dist = parameter.as_double(); } else if (param_name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") { params_.max_allowed_time_to_collision_up_to_carrot = parameter.as_double(); + } else if (param_name == plugin_name_ + ".min_distance_to_obstacle") { + params_.min_distance_to_obstacle = parameter.as_double(); } else if (param_name == plugin_name_ + ".cost_scaling_dist") { params_.cost_scaling_dist = parameter.as_double(); } else if (param_name == plugin_name_ + ".cost_scaling_gain") { diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 2f43f427f2f..5d8bc6a2d51 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -350,6 +350,12 @@ void RegulatedPurePursuitController::rotateToHeading( const double min_feasible_angular_speed = curr_speed.angular.z - params_->max_angular_accel * dt; const double max_feasible_angular_speed = curr_speed.angular.z + params_->max_angular_accel * dt; angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); + + // Check if we need to slow down to avoid overshooting + double max_vel_to_stop = std::sqrt(2 * params_->max_angular_accel * fabs(angle_to_path)); + if (fabs(angular_vel) > max_vel_to_stop) { + angular_vel = sign * max_vel_to_stop; + } } geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersection( diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 9c4dd1cade5..02c5a39485e 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -549,13 +549,13 @@ TEST(RegulatedPurePursuitTest, rotateTests) // basic full speed at a speed ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); EXPECT_EQ(lin_v, 0.0); - EXPECT_EQ(ang_v, 1.8); + EXPECT_EQ(ang_v, 1.6); // hit slow down limit // negative direction angle_to_path = -0.4; curr_speed.angular.z = -1.75; ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); - EXPECT_EQ(ang_v, -1.8); + EXPECT_EQ(ang_v, -1.6); // hit slow down limit // kinematic clamping, no speed, some speed accelerating, some speed decelerating angle_to_path = 0.4; @@ -720,6 +720,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) rclcpp::Parameter("test.rotate_to_heading_angular_vel", 18.0), rclcpp::Parameter("test.min_approach_linear_velocity", 1.0), rclcpp::Parameter("test.max_allowed_time_to_collision_up_to_carrot", 2.0), + rclcpp::Parameter("test.min_distance_to_obstacle", 2.0), rclcpp::Parameter("test.cost_scaling_dist", 2.0), rclcpp::Parameter("test.cost_scaling_gain", 4.0), rclcpp::Parameter("test.regulated_linear_scaling_min_radius", 10.0), @@ -749,6 +750,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) EXPECT_EQ( node->get_parameter( "test.max_allowed_time_to_collision_up_to_carrot").as_double(), 2.0); + EXPECT_EQ(node->get_parameter("test.min_distance_to_obstacle").as_double(), 2.0); EXPECT_EQ(node->get_parameter("test.cost_scaling_dist").as_double(), 2.0); EXPECT_EQ(node->get_parameter("test.cost_scaling_gain").as_double(), 4.0); EXPECT_EQ(node->get_parameter("test.regulated_linear_scaling_min_radius").as_double(), 10.0); diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 87a7f0ec1bd..2e982506050 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.4.0 + 1.4.1 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_route/include/nav2_route/node_spatial_tree.hpp b/nav2_route/include/nav2_route/node_spatial_tree.hpp index ac5347bc0bd..3293b0f2146 100644 --- a/nav2_route/include/nav2_route/node_spatial_tree.hpp +++ b/nav2_route/include/nav2_route/node_spatial_tree.hpp @@ -55,7 +55,9 @@ struct GraphAdaptor }; typedef nanoflann::KDTreeSingleIndexAdaptor< - nanoflann::L2_Simple_Adaptor, GraphAdaptor, DIMENSION> kd_tree_t; + nanoflann::L2_Simple_Adaptor, GraphAdaptor, DIMENSION, + unsigned int> + kd_tree_t; /** * @class nav2_route::NodeSpatialTree diff --git a/nav2_route/package.xml b/nav2_route/package.xml index 1f162385112..692112b4902 100644 --- a/nav2_route/package.xml +++ b/nav2_route/package.xml @@ -2,7 +2,7 @@ nav2_route - 1.4.0 + 1.4.1 A Route Graph planner to compliment the Planner Server Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 5360d4750ce..af4c7dd62dd 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.4.0 + 1.4.1 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_rviz_plugins/src/route_tool.cpp b/nav2_rviz_plugins/src/route_tool.cpp index 767d682887e..7befe4c864a 100644 --- a/nav2_rviz_plugins/src/route_tool.cpp +++ b/nav2_rviz_plugins/src/route_tool.cpp @@ -15,7 +15,6 @@ #include "nav2_rviz_plugins/route_tool.hpp" #include #include -#include #include #include #include "rviz_common/display_context.hpp" diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index e2293165f1b..aab9c1aae1c 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.4.0 + 1.4.1 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 8303ffc165f..2e88dd3e00a 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.4.0 + 1.4.1 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index ffe69aab305..b4b66b84050 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -416,15 +416,17 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( std::to_string(start.pose.position.y) + ") was outside bounds"); } - double orientation_bin = std::round(tf2::getYaw(start.pose.orientation) / _angle_bin_size); - while (orientation_bin < 0.0) { - orientation_bin += static_cast(_angle_quantizations); + double start_orientation_bin = std::round(tf2::getYaw(start.pose.orientation) / _angle_bin_size); + while (start_orientation_bin < 0.0) { + start_orientation_bin += static_cast(_angle_quantizations); } // This is needed to handle precision issues - if (orientation_bin >= static_cast(_angle_quantizations)) { - orientation_bin -= static_cast(_angle_quantizations); + if (start_orientation_bin >= static_cast(_angle_quantizations)) { + start_orientation_bin -= static_cast(_angle_quantizations); } - _a_star->setStart(mx_start, my_start, static_cast(orientation_bin)); + unsigned int start_orientation_bin_int = + static_cast(start_orientation_bin); + _a_star->setStart(mx_start, my_start, start_orientation_bin_int); // Set goal point, in A* bin search coordinates if (!costmap->worldToMapContinuous( @@ -437,15 +439,17 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + std::to_string(goal.pose.position.y) + ") was outside bounds"); } - orientation_bin = std::round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size); - while (orientation_bin < 0.0) { - orientation_bin += static_cast(_angle_quantizations); + double goal_orientation_bin = std::round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size); + while (goal_orientation_bin < 0.0) { + goal_orientation_bin += static_cast(_angle_quantizations); } // This is needed to handle precision issues - if (orientation_bin >= static_cast(_angle_quantizations)) { - orientation_bin -= static_cast(_angle_quantizations); + if (goal_orientation_bin >= static_cast(_angle_quantizations)) { + goal_orientation_bin -= static_cast(_angle_quantizations); } - _a_star->setGoal(mx_goal, my_goal, static_cast(orientation_bin), + unsigned int goal_orientation_bin_int = + static_cast(goal_orientation_bin); + _a_star->setGoal(mx_goal, my_goal, static_cast(goal_orientation_bin_int), _goal_heading_mode, _coarse_search_resolution); // Setup message @@ -462,7 +466,8 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( // Corner case of start and goal being on the same cell if (std::floor(mx_start) == std::floor(mx_goal) && - std::floor(my_start) == std::floor(my_goal)) + std::floor(my_start) == std::floor(my_goal) && + start_orientation_bin_int == goal_orientation_bin_int) { pose.pose = start.pose; pose.pose.orientation = goal.pose.orientation; diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 759d6b34768..9563fa3b6f1 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -338,9 +338,9 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + std::to_string(start.pose.position.y) + ") was outside bounds"); } - _a_star->setStart( - mx_start, my_start, - NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation))); + unsigned int start_bin = + NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation)); + _a_star->setStart(mx_start, my_start, start_bin); // Set goal point, in A* bin search coordinates if (!_costmap->worldToMapContinuous( @@ -353,9 +353,10 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + std::to_string(goal.pose.position.y) + ") was outside bounds"); } + unsigned int goal_bin = + NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)); _a_star->setGoal( - mx_goal, my_goal, - NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)), + mx_goal, my_goal, goal_bin, _goal_heading_mode, _coarse_search_resolution); // Setup message @@ -372,7 +373,8 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( // Corner case of start and goal being on the same cell if (std::floor(mx_start) == std::floor(mx_goal) && - std::floor(my_start) == std::floor(my_goal)) + std::floor(my_start) == std::floor(my_goal) && + start_bin == goal_bin) { pose.pose = start.pose; pose.pose.orientation = goal.pose.orientation; diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 72daaa67bd2..a716b19734b 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.4.0 + 1.4.1 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 76be2eccfed..93a77f9512c 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -81,7 +81,7 @@ bool SimpleSmoother::smooth( std::lock_guard lock(*(costmap->getMutex())); for (unsigned int i = 0; i != path_segments.size(); i++) { - if (path_segments[i].end - path_segments[i].start > 9) { + if (path_segments[i].end - path_segments[i].start > 3) { // Populate path segment curr_path_segment.poses.clear(); std::copy( diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 7a8261b63d7..d8987fe6e34 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.4.0 + 1.4.1 A sets of system-level tests for Nav2 usually involving full robot simulation Carlos Orduno Apache-2.0 diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 52c19fefa7b..280fc7fb0ac 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -70,55 +70,80 @@ class BehaviorTreeHandler } } - bool loadBehaviorTree(const std::string & filename) + BT::Blackboard::Ptr setBlackboardVariables() { - // Read the input BT XML from the specified file into a string - std::ifstream xml_file(filename); + // Create and populate the blackboard + blackboard = BT::Blackboard::create(); + blackboard->set("node", node_); + blackboard->set("server_timeout", std::chrono::milliseconds(20)); + blackboard->set("bt_loop_duration", std::chrono::milliseconds(10)); + blackboard->set("wait_for_service_timeout", + std::chrono::milliseconds(1000)); + blackboard->set("tf_buffer", tf_); + blackboard->set("initial_pose_received", false); + blackboard->set("number_recoveries", 0); + blackboard->set("odom_smoother", odom_smoother_); + + // Create dummy goal + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.header.frame_id = "map"; + blackboard->set("goal", goal); + return blackboard; + } + bool behaviorTreeFileValidation( + const std::string & filename) + { + std::ifstream xml_file(filename); if (!xml_file.good()) { - RCLCPP_ERROR(node_->get_logger(), "Couldn't open input XML file: %s", filename.c_str()); + RCLCPP_ERROR(node_->get_logger(), + "Couldn't open BT XML file: %s", filename.c_str()); return false; } + return true; + } - std::stringstream buffer; - buffer << xml_file.rdbuf(); - xml_file.close(); - std::string xml_string = buffer.str(); - // Create the blackboard that will be shared by all of the nodes in the tree - blackboard = BT::Blackboard::create(); - // Put items on the blackboard - blackboard->set("node", node_); // NOLINT - blackboard->set( - "server_timeout", std::chrono::milliseconds(20)); // NOLINT - blackboard->set( - "bt_loop_duration", std::chrono::milliseconds(10)); // NOLINT - blackboard->set( - "wait_for_service_timeout", std::chrono::milliseconds(1000)); // NOLINT - blackboard->set("tf_buffer", tf_); // NOLINT - blackboard->set("initial_pose_received", false); // NOLINT - blackboard->set("number_recoveries", 0); // NOLINT - blackboard->set("odom_smoother", odom_smoother_); // NOLINT - - // set dummy goal on blackboard - geometry_msgs::msg::PoseStamped goal; - goal.header.stamp = node_->now(); - goal.header.frame_id = "map"; - goal.pose.position.x = 0.0; - goal.pose.position.y = 0.0; - goal.pose.position.z = 0.0; - goal.pose.orientation.x = 0.0; - goal.pose.orientation.y = 0.0; - goal.pose.orientation.z = 0.0; - goal.pose.orientation.w = 1.0; + bool loadBehaviorTree( + const std::string & filename, + const std::vector & search_directories) + { + if (!behaviorTreeFileValidation(filename)) { + return false; + } + + namespace fs = std::filesystem; + const auto canonical_main_bt = fs::canonical(filename); + + // Register all XML behavior Subtrees found in the given directories + for (const auto & directory : search_directories) { + try { + for (const auto & entry : fs::directory_iterator(directory)) { + if (entry.path().extension() == ".xml") { + // Skip registering the main tree file + if (fs::equivalent(fs::canonical(entry.path()), canonical_main_bt)) { + continue; + } + factory_.registerBehaviorTreeFromFile(entry.path().string()); + } + } + } catch (const std::exception & e) { + RCLCPP_ERROR(node_->get_logger(), + "Exception reading behavior tree directory: %s", e.what()); + return false; + } + } - blackboard->set("goal", goal); // NOLINT + // Create and populate the blackboard + blackboard = setBlackboardVariables(); - // Create the Behavior Tree from the XML input + // Build the tree from the XML string try { - tree = factory_.createTreeFromText(xml_string, blackboard); + tree = factory_.createTreeFromFile(filename, blackboard); } catch (BT::RuntimeError & exp) { - RCLCPP_ERROR(node_->get_logger(), "%s: %s", filename.c_str(), exp.what()); + RCLCPP_ERROR(node_->get_logger(), "Failed to create BT from %s: %s", filename.c_str(), + exp.what()); return false; } @@ -195,19 +220,75 @@ std::shared_ptr BehaviorTreeTestFixture::bt_handler = nullp TEST_F(BehaviorTreeTestFixture, TestBTXMLFiles) { - std::filesystem::path root = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - root /= "behavior_trees/"; - - if (std::filesystem::exists(root) && std::filesystem::is_directory(root)) { - for (auto const & entry : std::filesystem::recursive_directory_iterator(root)) { - if (std::filesystem::is_regular_file(entry) && entry.path().extension() == ".xml") { - std::cout << entry.path().string() << std::endl; - EXPECT_EQ(bt_handler->loadBehaviorTree(entry.path().string()), true); - } + // Get the BT root directory + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + + ASSERT_TRUE(std::filesystem::exists(root_dir)); + ASSERT_TRUE(std::filesystem::is_directory(root_dir)); + + std::vector search_directories = {root_dir.string()}; + + for (auto const & entry : std::filesystem::recursive_directory_iterator(root_dir)) { + if (entry.is_regular_file() && entry.path().extension() == ".xml") { + std::string main_bt = entry.path().string(); + std::cout << "Testing BT file: " << main_bt << std::endl; + + EXPECT_TRUE(bt_handler->loadBehaviorTree(main_bt, search_directories)) + << "Failed to load: " << main_bt; } } } +TEST_F(BehaviorTreeTestFixture, TestWrongBTFormatXML) +{ + auto write_file = [](const std::string & path, const std::string & content) { + std::ofstream ofs(path); + ofs << content; + }; + + // File paths + std::string valid_subtree = "/tmp/valid_subtree.xml"; + std::string invalid_subtree = "/tmp/invalid_subtree.xml"; + std::string main_file = "/tmp/test_main_tree.xml"; + std::string malformed_main = "/tmp/malformed_main.xml"; + + // Valid subtree + write_file(valid_subtree, + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"); + + // Invalid subtree (malformed XML) + write_file(invalid_subtree, ""); + + // Main tree referencing the valid subtree + write_file(main_file, + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"); + + // Malformed main tree + write_file(malformed_main, ""); + + std::vector search_directories = {"/tmp"}; + + EXPECT_FALSE(bt_handler->loadBehaviorTree(main_file, search_directories)); + EXPECT_FALSE(bt_handler->loadBehaviorTree(malformed_main, search_directories)); + + std::remove(valid_subtree.c_str()); + std::remove(main_file.c_str()); + std::remove(invalid_subtree.c_str()); + std::remove(malformed_main.c_str()); +} + /** * Test scenario: * @@ -217,10 +298,14 @@ TEST_F(BehaviorTreeTestFixture, TestBTXMLFiles) TEST_F(BehaviorTreeTestFixture, TestAllSuccess) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); BT::NodeStatus result = BT::NodeStatus::RUNNING; @@ -264,10 +349,14 @@ TEST_F(BehaviorTreeTestFixture, TestAllSuccess) TEST_F(BehaviorTreeTestFixture, TestAllFailure) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); // Set all action server to fail the first 100 times Ranges failureRange; @@ -320,10 +409,14 @@ TEST_F(BehaviorTreeTestFixture, TestAllFailure) TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); // Set ComputePathToPose and FollowPath action servers to fail for the first action Ranges failureRange; @@ -379,10 +472,14 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries) TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); // Set ComputePathToPose action server to fail for the first action Ranges plannerFailureRange; @@ -477,10 +574,14 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple) TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); // Set FollowPath action server to fail for the first 2 actions Ranges controllerFailureRange; @@ -545,10 +646,14 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex) TEST_F(BehaviorTreeTestFixture, TestRecoverySubtreeGoalUpdated) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); // Set FollowPath action server to fail for the first 2 actions Ranges controllerFailureRange; diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index a8860994253..8ca7d14c808 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.4.0 + 1.4.1 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 97583144f86..6827895d813 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.4.0 + 1.4.1 Nav2 utilities Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index f2a0c2cd61b..2b1b6e99846 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.4.0 + 1.4.1 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 ef9eedd9b42..d21c870b6a7 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.4.0 + 1.4.1 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 409e652d8ff..7a97798371b 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.4.0 + 1.4.1 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 6aa4209831c..0e4dd122c42 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.4.0 + 1.4.1 ROS2 Navigation Stack