diff --git a/.devcontainer/caddy/srv/nav2/index.md b/.devcontainer/caddy/srv/nav2/index.md index 60e2db1bfb..1f8dc3d0fd 100644 --- a/.devcontainer/caddy/srv/nav2/index.md +++ b/.devcontainer/caddy/srv/nav2/index.md @@ -14,9 +14,9 @@ For more related documentation: -- [Nav2 Documentation](https://navigation.ros.org) - - [Development Guides](https://navigation.ros.org/development_guides) - - [Dev Containers](https://navigation.ros.org/development_guides/devcontainer_docs) +- [Nav2 Documentation](https://docs.nav2.org) + - [Development Guides](https://docs.nav2.org/development_guides) + - [Dev Containers](https://docs.nav2.org/development_guides/devcontainer_docs) ## Session Info diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index c1cfa2787f..88c59fe4ed 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -38,7 +38,7 @@ --> #### For Maintainers: -- [ ] Check that any new parameters added are updated in navigation.ros.org +- [ ] Check that any new parameters added are updated in docs.nav2.org - [ ] Check that any significant change is added to the migration guide - [ ] Check that any new features **OR** changes to existing behaviors are reflected in the tuning guide - [ ] Check that any new functions have Doxygen added diff --git a/.github/mergify.yml b/.github/mergify.yml index 1c9acea3c3..a0427e6054 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -83,7 +83,7 @@ pull_request_rules: comment: message: | @{{author}}, please properly fill in PR template in the future. @stevemacenski, use this instead. - - [ ] Check that any new parameters added are updated in navigation.ros.org + - [ ] Check that any new parameters added are updated in docs.nav2.org - [ ] Check that any significant change is added to the migration guide - [ ] Check that any new features **OR** changes to existing behaviors are reflected in the tuning guide - [ ] Check that any new functions have Doxygen added diff --git a/README.md b/README.md index 6df210791b..ab34c01ce3 100644 --- a/README.md +++ b/README.md @@ -1,24 +1,24 @@ # Nav2 -[![GitHub Workflow Status](https://github.com/ros-planning/navigation2/actions/workflows/update_ci_image.yaml/badge.svg)](https://github.com/ros-planning/navigation2/actions/workflows/update_ci_image.yaml) -[![codecov](https://codecov.io/gh/ros-planning/navigation2/branch/main/graph/badge.svg?token=S3iRmypwlg)](https://codecov.io/gh/ros-planning/navigation2) +[![GitHub Workflow Status](https://github.com/ros-navigation/navigation2/actions/workflows/update_ci_image.yaml/badge.svg)](https://github.com/ros-navigation/navigation2/actions/workflows/update_ci_image.yaml) +[![codecov](https://codecov.io/gh/ros-navigation/navigation2/branch/main/graph/badge.svg?token=S3iRmypwlg)](https://codecov.io/gh/ros-navigation/navigation2)

For detailed instructions on how to: -- [Getting Started](https://navigation.ros.org/getting_started/index.html) -- [Concepts](https://navigation.ros.org/concepts/index.html) -- [Build](https://navigation.ros.org/development_guides/build_docs/index.html#build) -- [Install](https://navigation.ros.org/development_guides/build_docs/index.html#install) -- [General Tutorials](https://navigation.ros.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://navigation.ros.org/plugin_tutorials/index.html) -- [Configure](https://navigation.ros.org/configuration/index.html) -- [Navigation Plugins](https://navigation.ros.org/plugins/index.html) -- [Migration Guides](https://navigation.ros.org/migration/index.html) +- [Getting Started](https://docs.nav2.org/getting_started/index.html) +- [Concepts](https://docs.nav2.org/concepts/index.html) +- [Build](https://docs.nav2.org/development_guides/build_docs/index.html#build) +- [Install](https://docs.nav2.org/development_guides/build_docs/index.html#install) +- [General Tutorials](https://docs.nav2.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://docs.nav2.org/plugin_tutorials/index.html) +- [Configure](https://docs.nav2.org/configuration/index.html) +- [Navigation Plugins](https://docs.nav2.org/plugins/index.html) +- [Migration Guides](https://docs.nav2.org/migration/index.html) - [Container Images for Building Nav2](https://github.com/orgs/ros-planning/packages/container/package/navigation2) -- [Contribute](https://navigation.ros.org/development_guides/involvement_docs/index.html) +- [Contribute](https://docs.nav2.org/development_guides/involvement_docs/index.html) -Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate). +Please visit our [documentation site](https://docs.nav2.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate). If you need professional services related to Nav2, please contact Open Navigation at info@opennav.org. @@ -117,7 +117,7 @@ If you use the Regulated Pure Pursuit Controller algorithm or software from this | Service | Humble | Iron | Main | | :---: | :---: | :---: | :---: | | ROS Build Farm | [![Build Status](https://build.ros2.org/job/Hdev__navigation2__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Hdev__navigation2__ubuntu_jammy_amd64/) | [![Build Status](https://build.ros2.org/job/Idev__navigation2__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Idev__navigation2__ubuntu_jammy_amd64/) | N/A | -| Circle CI | N/A | N/A | [![Build Status](https://circleci.com/gh/ros-planning/navigation2/tree/main.svg?style=svg)](https://circleci.com/gh/ros-planning/navigation2/tree/main) | +| Circle CI | N/A | N/A | [![Build Status](https://circleci.com/gh/ros-navigation/navigation2/tree/main.svg?style=svg)](https://circleci.com/gh/ros-navigation/navigation2/tree/main) | | Package | Humble Source | Humble Debian | Iron Source | Iron Debian | diff --git a/nav2_amcl/README.md b/nav2_amcl/README.md index 7215500919..5e8d737a76 100644 --- a/nav2_amcl/README.md +++ b/nav2_amcl/README.md @@ -1,4 +1,4 @@ # AMCL Adaptive Monte Carlo Localization (AMCL) is a probabilistic localization module which estimates the position and orientation (i.e. Pose) of a robot in a given known map using a 2D laser scanner. This is largely a refactored port from ROS 1 without any algorithmic changes. -See the [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-amcl.html) for more details about configurable settings and their meanings. +See the [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-amcl.html) for more details about configurable settings and their meanings. diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index ba5c83447f..764a8f059f 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -524,11 +524,8 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha RCLCPP_INFO(get_logger(), "initialPoseReceived"); - if (msg->header.frame_id == "") { - // This should be removed at some point - RCLCPP_WARN( - get_logger(), - "Received initial pose with empty frame_id. You should always supply a frame_id."); + if (!nav2_util::validateMsg(*msg)) { + RCLCPP_ERROR(get_logger(), "Received initialpose message is malformed. Rejecting."); return; } if (nav2_util::strip_leading_slash(msg->header.frame_id) != global_frame_id_) { diff --git a/nav2_amcl/src/pf/CMakeLists.txt b/nav2_amcl/src/pf/CMakeLists.txt index c6f16e6a7d..3b4b2fa5ca 100644 --- a/nav2_amcl/src/pf/CMakeLists.txt +++ b/nav2_amcl/src/pf/CMakeLists.txt @@ -15,6 +15,7 @@ target_include_directories(pf_lib PRIVATE ../include) if(HAVE_DRAND48) target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48") endif() +target_link_libraries(pf_lib m) install(TARGETS pf_lib diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index 0ade4e0a01..dab5b9a8a0 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -7,9 +7,9 @@ The nav2_behavior_tree module provides: * Navigation-specific behavior tree nodes, and * a generic BehaviorTreeEngine class that simplifies the integration of BT processing into ROS2 nodes for navigation or higher-level autonomy applications. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-bt-xml.html) for additional parameter descriptions and a list of XML nodes made available in this package. Also review the [Nav2 Behavior Tree Explanation](https://navigation.ros.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. A [tutorial](https://navigation.ros.org/plugin_tutorials/docs/writing_new_bt_plugin.html) is also provided to explain how to create a simple BT plugin. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-bt-xml.html) for additional parameter descriptions and a list of XML nodes made available in this package. Also review the [Nav2 Behavior Tree Explanation](https://docs.nav2.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. A [tutorial](https://docs.nav2.org/plugin_tutorials/docs/writing_new_bt_plugin.html) is also provided to explain how to create a simple BT plugin. -See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. +See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. ## The bt_action_node Template and the Behavior Tree Engine diff --git a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp index cb39a24557..eb401f6762 100644 --- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -62,12 +62,11 @@ inline BT::NodeStatus PathLongerOnApproach::tick() getInput("prox_len", prox_len_); getInput("length_factor", length_factor_); - if (!BT::isStatusActive(status())) { - // Reset the starting point since we're starting a new iteration of - // PathLongerOnApproach (moving from IDLE to RUNNING) - first_time_ = true; + if (first_time_ == false) { + if (old_path_.poses.back() != new_path_.poses.back()) { + first_time_ = true; + } } - setStatus(BT::NodeStatus::RUNNING); // Check if the path is updated and valid, compare the old and the new path length, diff --git a/nav2_behaviors/README.md b/nav2_behaviors/README.md index fdc24a26b3..7c87ae91a3 100644 --- a/nav2_behaviors/README.md +++ b/nav2_behaviors/README.md @@ -10,9 +10,9 @@ The only required class a behavior must derive from is the `nav2_core/behavior.h The value of the centralized behavior server is to **share resources** amongst several behaviors that would otherwise be independent nodes. Subscriptions to TF, costmaps, and more can be quite heavy and add non-trivial compute costs to a robot system. By combining these independent behaviors into a single server, they may share these resources while retaining complete independence in execution and interface. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-behavior-server.html) for additional parameter descriptions and a [tutorial about writing behaviors](https://navigation.ros.org/plugin_tutorials/docs/writing_new_behavior_plugin.html). +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-behavior-server.html) for additional parameter descriptions and a [tutorial about writing behaviors](https://docs.nav2.org/plugin_tutorials/docs/writing_new_behavior_plugin.html). -See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. +See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. The `TimedBehavior` template makes use of a [nav2_util::TwistPublisher](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). The `AssistedTeleop` behavior makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). \ No newline at end of file diff --git a/nav2_bringup/README.md b/nav2_bringup/README.md index 16088452cb..da45591352 100644 --- a/nav2_bringup/README.md +++ b/nav2_bringup/README.md @@ -10,7 +10,7 @@ Dynamically composed bringup (based on [ROS2 Composition](https://docs.ros.org/ * Some discussions about performance improvement of composed bringup could be found here: https://discourse.ros.org/t/nav2-composition/22175. -To use, please see the Nav2 [Getting Started Page](https://navigation.ros.org/getting_started/index.html) on our documentation website. Additional [tutorials will help you](https://navigation.ros.org/tutorials/index.html) go from an initial setup in simulation to testing on a hardware robot, using SLAM, and more. +To use, please see the Nav2 [Getting Started Page](https://docs.nav2.org/getting_started/index.html) on our documentation website. Additional [tutorials will help you](https://docs.nav2.org/tutorials/index.html) go from an initial setup in simulation to testing on a hardware robot, using SLAM, and more. Note: * gazebo should be started with both libgazebo_ros_init.so and libgazebo_ros_factory.so to work correctly. diff --git a/nav2_bt_navigator/README.md b/nav2_bt_navigator/README.md index 8dd51a6c6e..48787a9473 100644 --- a/nav2_bt_navigator/README.md +++ b/nav2_bt_navigator/README.md @@ -2,7 +2,7 @@ The BT Navigator (Behavior Tree Navigator) module implements the NavigateToPose and NavigateThroughPoses task interfaces. It is a [Behavior Tree](https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/docs/BT_basics.md)-based implementation of navigation that is intended to allow for flexibility in the navigation task and provide a way to easily specify complex robot behaviors. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-bt-navigator.html) for additional parameter descriptions, as well as the [Nav2 Behavior Tree Explanation](https://navigation.ros.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-bt-navigator.html) for additional parameter descriptions, as well as the [Nav2 Behavior Tree Explanation](https://docs.nav2.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. ## Overview diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 4f81b241b6..eca25192b6 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -59,7 +59,7 @@ The following diagram is showing the high-level design of Collision Monitor modu ### Configuration -Detailed configuration parameters, their description and how to setup a Collision Monitor could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/configuring-collision-monitor.html) and [Using Collision Monitor tutorial](https://navigation.ros.org/tutorials/docs/using_collision_monitor.html) pages. +Detailed configuration parameters, their description and how to setup a Collision Monitor could be found at its [Configuration Guide](https://docs.nav2.org/configuration/packages/configuring-collision-monitor.html) and [Using Collision Monitor tutorial](https://docs.nav2.org/tutorials/docs/using_collision_monitor.html) pages. ### Metrics @@ -94,6 +94,6 @@ The zones around the robot and the data sources are the same as for the Collisio ### Configuration -Detailed configuration parameters, their description and how to setup a Collision Detector could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/collision_monitor/configuring-collision-detector-node.html). +Detailed configuration parameters, their description and how to setup a Collision Detector could be found at its [Configuration Guide](https://docs.nav2.org/configuration/packages/collision_monitor/configuring-collision-detector-node.html). The `CollisionMonitor` node makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). diff --git a/nav2_constrained_smoother/README.md b/nav2_constrained_smoother/README.md index 55482d5dcf..72768ede25 100644 --- a/nav2_constrained_smoother/README.md +++ b/nav2_constrained_smoother/README.md @@ -2,7 +2,7 @@ A smoother plugin for `nav2_smoother` based on the original deprecated smoother in `nav2_smac_planner` by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) and put into operational state by [**RoboTech Vision**](https://robotechvision.com/). Suitable for applications which need planned global path to be pushed away from obstacles and/or for Reeds-Shepp motion models. -See documentation on navigation.ros.org: https://navigation.ros.org/configuration/packages/configuring-constrained-smoother.html +See documentation on docs.nav2.org: https://docs.nav2.org/configuration/packages/configuring-constrained-smoother.html Example of configuration (see indoor_navigation package of this repo for a full launch configuration): @@ -26,13 +26,13 @@ smoother_server: w_cost: 0.015 # weight to steer robot away from collision and cost # Parameters used to improve obstacle avoidance near cusps (forward/reverse movement changes) - # See the [docs page](https://navigation.ros.org/configuration/packages/configuring-constrained-smoother) for further clarification + # See the [docs page](https://docs.nav2.org/configuration/packages/configuring-constrained-smoother) for further clarification w_cost_cusp_multiplier: 3.0 # option to have higher weight during forward/reverse direction change which is often accompanied with dangerous rotations cusp_zone_length: 2.5 # length of the section around cusp in which nodes use w_cost_cusp_multiplier (w_cost rises gradually inside the zone towards the cusp point, whose costmap weight equals w_cost*w_cost_cusp_multiplier) # Points in robot frame to grab costmap values from. Format: [x1, y1, weight1, x2, y2, weight2, ...] # IMPORTANT: Requires much higher number of iterations to actually improve the path. Uncomment only if you really need it (highly elongated/asymmetric robots) - # See the [docs page](https://navigation.ros.org/configuration/packages/configuring-constrained-smoother) for further clarification + # See the [docs page](https://docs.nav2.org/configuration/packages/configuring-constrained-smoother) for further clarification # cost_check_points: [-0.185, 0.0, 1.0] optimizer: diff --git a/nav2_controller/README.md b/nav2_controller/README.md index 98b76c3355..c718a4e56f 100644 --- a/nav2_controller/README.md +++ b/nav2_controller/README.md @@ -4,8 +4,8 @@ The Nav2 Controller is a Task Server in Nav2 that implements the `nav2_msgs::act An execution module implementing the `nav2_msgs::action::FollowPath` action server is responsible for generating command velocities for the robot, given the computed path from the planner module in `nav2_planner`. The nav2_controller package is designed to be loaded with multiple plugins for path execution. The plugins need to implement functions in the virtual base class defined in the `controller` header file in `nav2_core` package. It also contains progress checkers and goal checker plugins to abstract out that logic from specific controller implementations. -See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available controller plugins. +See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available controller plugins. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-controller-server.html) for additional parameter descriptions and a [tutorial about writing controller plugins](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html). +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-controller-server.html) for additional parameter descriptions and a [tutorial about writing controller plugins](https://docs.nav2.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html). The `ControllerServer` makes use of a [nav2_util::TwistPublisher](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). diff --git a/nav2_costmap_2d/README.md b/nav2_costmap_2d/README.md index b2eb9a2b94..d6c60ea915 100644 --- a/nav2_costmap_2d/README.md +++ b/nav2_costmap_2d/README.md @@ -2,9 +2,9 @@ The costmap_2d package is responsible for building a 2D costmap of the environment, consisting of several "layers" of data about the environment. It can be initialized via the map server or a local rolling window and updates the layers by taking observations from sensors. A plugin interface allows for the layers to be combined into the costmap and finally inflated via an inflation radius based on the robot footprint. The nav2 version of the costmap_2d package is mostly a direct ROS2 port of the ROS1 navigation stack version, with minimal noteable changes necessary due to support in ROS2. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-costmaps.html) for additional parameter descriptions for the costmap and its included plugins. The [tutorials](https://navigation.ros.org/tutorials/index.html) and [first-time setup guides](https://navigation.ros.org/setup_guides/index.html) also provide helpful context for working with the costmap 2D package and its layers. A [tutorial](https://navigation.ros.org/plugin_tutorials/docs/writing_new_costmap2d_plugin.html) is also provided to explain how to create costmap plugins. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-costmaps.html) for additional parameter descriptions for the costmap and its included plugins. The [tutorials](https://docs.nav2.org/tutorials/index.html) and [first-time setup guides](https://docs.nav2.org/setup_guides/index.html) also provide helpful context for working with the costmap 2D package and its layers. A [tutorial](https://docs.nav2.org/plugin_tutorials/docs/writing_new_costmap2d_plugin.html) is also provided to explain how to create costmap plugins. -See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. +See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. ## To visualize the voxels in RVIZ: - Make sure `publish_voxel_map` in `voxel_layer` param's scope is set to `True`. @@ -24,4 +24,4 @@ See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) ### Overview -Costmap Filters - is a costmap layer-based instrument which provides an ability to apply to map spatial-dependent raster features named as filter-masks. These features are used in plugin algorithms when filling costmaps in order to allow robots to change their trajectory, behavior or speed when a robot enters/leaves an area marked in a filter masks. Examples of costmap filters include keep-out/safety zones where robots will never enter, speed restriction areas, preferred lanes for robots moving in industries and warehouses. More information about design, architecture of the feature and how it works could be found on Nav2 website: https://navigation.ros.org. +Costmap Filters - is a costmap layer-based instrument which provides an ability to apply to map spatial-dependent raster features named as filter-masks. These features are used in plugin algorithms when filling costmaps in order to allow robots to change their trajectory, behavior or speed when a robot enters/leaves an area marked in a filter masks. Examples of costmap filters include keep-out/safety zones where robots will never enter, speed restriction areas, preferred lanes for robots moving in industries and warehouses. More information about design, architecture of the feature and how it works could be found on Nav2 website: https://docs.nav2.org. diff --git a/nav2_dwb_controller/README.md b/nav2_dwb_controller/README.md index b440f29a82..da572d6332 100644 --- a/nav2_dwb_controller/README.md +++ b/nav2_dwb_controller/README.md @@ -10,7 +10,7 @@ DWB improves on DWA in a few major ways: It is possible to tune DWB to gain both DWA and base local planner behaviors, as well as expansions using new plugins for totally use-case specific behaviors. The current trajectory generator plugins work for omnidirectional and differential drive robots, though an ackermann generator would be trivial to add. The current critic plugins work for both circular and non-circular robots and include many of the cost functions needed to build a path tracking system with various attributes. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-dwb-controller.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-dwb-controller.html) for additional parameter descriptions. ## DWB Plugins diff --git a/nav2_graceful_controller/README.md b/nav2_graceful_controller/README.md index 9cb5a72ec7..5c3f8a39fb 100644 --- a/nav2_graceful_controller/README.md +++ b/nav2_graceful_controller/README.md @@ -1,7 +1,7 @@ # Graceful Motion Controller The graceful motion controller implements a controller based on the works of Jong Jin Park in "Graceful Navigation for Mobile Robots in Dynamic and Uncertain Environments". (2016). In this implementation, a `motion_target` is set at a distance away from the robot that is exponentially stable to generate a smooth trajectory for the robot to follow. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-graceful-motion-controller.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-graceful-motion-controller.html) for additional parameter descriptions. ## Smooth control law The smooth control law is a pose-following kinematic control law that generates a smooth and confortable trajectory for the robot to follow. It is Lyapunov-based feedback control law made of three components: diff --git a/nav2_lifecycle_manager/README.md b/nav2_lifecycle_manager/README.md index 9a0905006c..e462211988 100644 --- a/nav2_lifecycle_manager/README.md +++ b/nav2_lifecycle_manager/README.md @@ -1,7 +1,7 @@ ### Background on lifecycle enabled nodes Using ROS2’s managed/lifecycle nodes feature allows the system startup to ensure that all required nodes have been instantiated correctly before they begin their execution. Using lifecycle nodes also allows nodes to be restarted or replaced on-line. More details about managed nodes can be found on [ROS2 Design website](https://design.ros2.org/articles/node_lifecycle.html). Several nodes in Nav2, such as map_server, planner_server, and controller_server, are lifecycle enabled. These nodes provide the required overrides of the lifecycle functions: ```on_configure()```, ```on_activate()```, ```on_deactivate()```, ```on_cleanup()```, ```on_shutdown()```, and ```on_error()```. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-lifecycle.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-lifecycle.html) for additional parameter descriptions. ### nav2_lifecycle_manager Nav2's lifecycle manager is used to change the states of the lifecycle nodes in order to achieve a controlled _startup_, _shutdown_, _reset_, _pause_, or _resume_ of the navigation stack. The lifecycle manager presents a ```lifecycle_manager/manage_nodes``` service, from which clients can invoke the startup, shutdown, reset, pause, or resume functions. Based on this service request, the lifecycle manager calls the necessary lifecycle services in the lifecycle managed nodes. Currently, the RVIZ panel uses this ```lifecycle_manager/manage_nodes``` service when user presses the buttons on the RVIZ panel (e.g.,startup, reset, shutdown, etc.), but it is meant to be called on bringup through a production system application. diff --git a/nav2_map_server/README.md b/nav2_map_server/README.md index 94a6bd66af..4e811a2427 100644 --- a/nav2_map_server/README.md +++ b/nav2_map_server/README.md @@ -3,7 +3,7 @@ The `Map Server` provides maps to the rest of the Nav2 system using both topic and service interfaces. Map server will expose maps on the node bringup, but can also change maps using a `load_map` service during run-time, as well as save maps using a `save_map` server. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-map-server.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-map-server.html) for additional parameter descriptions. ### Architecture diff --git a/nav2_navfn_planner/README.md b/nav2_navfn_planner/README.md index ad980d1765..8b4a07e051 100644 --- a/nav2_navfn_planner/README.md +++ b/nav2_navfn_planner/README.md @@ -4,4 +4,4 @@ The NavfnPlanner is a global planner plugin for the Nav2 Planner server. It impl The `global_planner` package from ROS (1) is a refactor on NavFn to make it more easily understandable, but it lacks in run-time performance and introduces suboptimal behaviors. As NavFn has been extremely stable for about 10 years at the time of porting, the maintainers felt no compelling reason to port over another, largely equivalent (but poorer functioning) planner. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-navfn.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-navfn.html) for additional parameter descriptions. diff --git a/nav2_planner/README.md b/nav2_planner/README.md index 3a17c825d4..302fed50db 100644 --- a/nav2_planner/README.md +++ b/nav2_planner/README.md @@ -4,6 +4,6 @@ The Nav2 planner is a Task Server in Nav2 that implements the `nav2_behavior_tre A planning module implementing the `nav2_behavior_tree::ComputePathToPose` interface is responsible for generating a feasible path given start and end robot poses. It loads a map of potential planner plugins to do the path generation in different user-defined situations. -See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. +See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-planner-server.html) for additional parameter descriptions and a [tutorial about writing planner plugins](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2planner_plugin.html). +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-planner-server.html) for additional parameter descriptions and a [tutorial about writing planner plugins](https://docs.nav2.org/plugin_tutorials/docs/writing_new_nav2planner_plugin.html). diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 57b820b16d..acdd0eb1ed 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -2,7 +2,7 @@ This is a controller (local trajectory planner) that implements a variant on the pure pursuit algorithm to track a path. This variant we call the Regulated Pure Pursuit Algorithm, due to its additional regulation terms on collision and linear speed. It also implements the basics behind the Adaptive Pure Pursuit algorithm to vary lookahead distances by current speed. It was developed by [Shrijit Singh](https://www.linkedin.com/in/shrijitsingh99/) and [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/) as part of the Nav2 working group. -Code based on a simplified version of this controller is referenced in the [Writing a New Nav2 Controller](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html) tutorial. +Code based on a simplified version of this controller is referenced in the [Writing a New Nav2 Controller](https://docs.nav2.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html) tutorial. This plugin implements the `nav2_core::Controller` interface allowing it to be used across the navigation stack as a local trajectory planner in the controller server's action server (`controller_server`). @@ -14,7 +14,7 @@ This controller has been measured to run at well over 1 kHz on a modern intel pr

-See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-regulated-pp.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-regulated-pp.html) for additional parameter descriptions. If you use the Regulated Pure Pursuit Controller algorithm or software from this repository, please cite this work in your papers! diff --git a/nav2_rotation_shim_controller/README.md b/nav2_rotation_shim_controller/README.md index 9129e9af61..8858e20c4d 100644 --- a/nav2_rotation_shim_controller/README.md +++ b/nav2_rotation_shim_controller/README.md @@ -23,7 +23,7 @@ This plugin implements the `nav2_core::Controller` interface allowing it to be u

-See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-rotation-shim-controller.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-rotation-shim-controller.html) for additional parameter descriptions. ## Configuration diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index 339d986ed4..668fd9fe7c 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -10,7 +10,7 @@ This was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41 ## API -See its [API Guide Page](https://navigation.ros.org/commander_api/index.html) for additional parameter descriptions. +See its [API Guide Page](https://docs.nav2.org/commander_api/index.html) for additional parameter descriptions. The methods provided by the basic navigator are shown below, with inputs and expected returns. If a server fails, it may throw an exception or return a `None` object, so please be sure to properly wrap your navigation calls in try/catch and check results for `None` type. diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 78adee940b..ebb25bb52b 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -17,7 +17,7 @@ We have users reporting using this on: - Vertical farming - Solar farms -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-smac-planner.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-smac-planner.html) for additional parameter descriptions. ## Introduction diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index b59fa9d66b..56e5bbccaf 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -49,13 +49,13 @@ class AStarAlgorithm { public: typedef NodeT * NodePtr; - typedef robin_hood::unordered_node_map Graph; + typedef robin_hood::unordered_node_map Graph; typedef std::vector NodeVector; typedef std::pair> NodeElement; typedef typename NodeT::Coordinates Coordinates; typedef typename NodeT::CoordinateVector CoordinateVector; typedef typename NodeVector::iterator NeighborIterator; - typedef std::function NodeGetter; + typedef std::function NodeGetter; /** * @struct nav2_smac_planner::NodeComparator @@ -210,7 +210,7 @@ class AStarAlgorithm * @brief Adds node to graph * @param index Node index to add */ - inline NodePtr addToGraph(const unsigned int & index); + inline NodePtr addToGraph(const uint64_t & index); /** * @brief Check if this node is the goal node diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index 9dc317dd8a..76719d7231 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -35,7 +35,7 @@ class AnalyticExpansion public: typedef NodeT * NodePtr; typedef typename NodeT::Coordinates Coordinates; - typedef std::function NodeGetter; + typedef std::function NodeGetter; /** * @struct nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp index 10a975577a..3ccadefded 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -62,7 +62,7 @@ class Node2D * @brief A constructor for nav2_smac_planner::Node2D * @param index The index of this node for self-reference */ - explicit Node2D(const unsigned int index); + explicit Node2D(const uint64_t index); /** * @brief A destructor for nav2_smac_planner::Node2D @@ -158,7 +158,7 @@ class Node2D * @brief Gets cell index * @return Reference to cell index */ - inline unsigned int getIndex() + inline uint64_t getIndex() { return _index; } @@ -185,10 +185,11 @@ class Node2D * @param width width of costmap * @return index */ - static inline unsigned int getIndex( + static inline uint64_t getIndex( const unsigned int & x, const unsigned int & y, const unsigned int & width) { - return x + y * width; + return static_cast(x) + static_cast(y) * + static_cast(width); } /** @@ -199,7 +200,7 @@ class Node2D * @return coordinates of point */ static inline Coordinates getCoords( - const unsigned int & index, const unsigned int & width, const unsigned int & angles) + const uint64_t & index, const unsigned int & width, const unsigned int & angles) { if (angles != 1) { throw std::runtime_error("Node type Node2D does not have a valid angle quantization."); @@ -213,7 +214,7 @@ class Node2D * @param Index Index of point * @return coordinates of point */ - static inline Coordinates getCoords(const unsigned int & index) + static inline Coordinates getCoords(const uint64_t & index) { const unsigned int & size_x = _neighbors_grid_offsets[3]; return Coordinates(index % size_x, index / size_x); @@ -253,7 +254,8 @@ class Node2D * @param neighbors Vector of neighbors to be filled */ void getNeighbors( - std::function & validity_checker, + std::function & validity_checker, GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors); @@ -272,7 +274,7 @@ class Node2D private: float _cell_cost; float _accumulated_cost; - unsigned int _index; + uint64_t _index; bool _was_visited; bool _is_queued; }; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp index 0fbace4360..dfb26c5dda 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp @@ -49,7 +49,7 @@ class NodeBasic * @brief A constructor for nav2_smac_planner::NodeBasic * @param index The index of this node for self-reference */ - explicit NodeBasic(const unsigned int index) + explicit NodeBasic(const uint64_t index) : index(index), graph_node_ptr(nullptr) { @@ -73,7 +73,8 @@ class NodeBasic typename NodeT::Coordinates pose; // Used by NodeHybrid and NodeLattice NodeT * graph_node_ptr; MotionPrimitive * prim_ptr; // Used by NodeLattice - unsigned int index, motion_index; + uint64_t index; + unsigned int motion_index; bool backward; TurnDirection turn_dir; }; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index b6a33a444a..b1e265e139 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -40,7 +40,7 @@ namespace nav2_smac_planner typedef std::vector LookupTable; typedef std::pair TrigValues; -typedef std::pair ObstacleHeuristicElement; +typedef std::pair ObstacleHeuristicElement; struct ObstacleHeuristicComparator { bool operator()(const ObstacleHeuristicElement & a, const ObstacleHeuristicElement & b) const @@ -184,7 +184,7 @@ class NodeHybrid * @brief A constructor for nav2_smac_planner::NodeHybrid * @param index The index of this node for self-reference */ - explicit NodeHybrid(const unsigned int index); + explicit NodeHybrid(const uint64_t index); /** * @brief A destructor for nav2_smac_planner::NodeHybrid @@ -291,7 +291,7 @@ class NodeHybrid * @brief Gets cell index * @return Reference to cell index */ - inline unsigned int getIndex() + inline uint64_t getIndex() { return _index; } @@ -319,11 +319,14 @@ class NodeHybrid * @param angle_quantization Number of theta bins * @return Index */ - static inline unsigned int getIndex( + static inline uint64_t getIndex( const unsigned int & x, const unsigned int & y, const unsigned int & angle, const unsigned int & width, const unsigned int & angle_quantization) { - return angle + x * angle_quantization + y * width * angle_quantization; + return static_cast(angle) + static_cast(x) * + static_cast(angle_quantization) + + static_cast(y) * static_cast(width) * + static_cast(angle_quantization); } /** @@ -333,7 +336,7 @@ class NodeHybrid * @param angle Theta coordinate of point * @return Index */ - static inline unsigned int getIndex( + static inline uint64_t getIndex( const unsigned int & x, const unsigned int & y, const unsigned int & angle) { return getIndex( @@ -349,7 +352,7 @@ class NodeHybrid * @return Coordinates */ static inline Coordinates getCoords( - const unsigned int & index, + const uint64_t & index, const unsigned int & width, const unsigned int & angle_quantization) { return Coordinates( @@ -447,7 +450,8 @@ class NodeHybrid * @param neighbors Vector of neighbors to be filled */ void getNeighbors( - std::function & validity_checker, + std::function & validity_checker, GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors); @@ -478,7 +482,7 @@ class NodeHybrid private: float _cell_cost; float _accumulated_cost; - unsigned int _index; + uint64_t _index; bool _was_visited; unsigned int _motion_primitive_index; TurnDirection _turn_dir; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 99f50d6a92..47363ab9eb 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -132,7 +132,7 @@ class NodeLattice * @brief A constructor for nav2_smac_planner::NodeLattice * @param index The index of this node for self-reference */ - explicit NodeLattice(const unsigned int index); + explicit NodeLattice(const uint64_t index); /** * @brief A destructor for nav2_smac_planner::NodeLattice @@ -229,7 +229,7 @@ class NodeLattice * @brief Gets cell index * @return Reference to cell index */ - inline unsigned int getIndex() + inline uint64_t getIndex() { return _index; } @@ -281,7 +281,7 @@ class NodeLattice * @param angle Theta coordinate of point * @return Index */ - static inline unsigned int getIndex( + static inline uint64_t getIndex( const unsigned int & x, const unsigned int & y, const unsigned int & angle) { // Hybrid-A* and State Lattice share a coordinate system @@ -298,7 +298,7 @@ class NodeLattice * @return Coordinates */ static inline Coordinates getCoords( - const unsigned int & index, + const uint64_t & index, const unsigned int & width, const unsigned int & angle_quantization) { // Hybrid-A* and State Lattice share a coordinate system @@ -396,7 +396,7 @@ class NodeLattice * @param neighbors Vector of neighbors to be filled */ void getNeighbors( - std::function & validity_checker, GridCollisionChecker * collision_checker, const bool & traverse_unknown, @@ -425,7 +425,7 @@ class NodeLattice private: float _cell_cost; float _accumulated_cost; - unsigned int _index; + uint64_t _index; bool _was_visited; MotionPrimitive * _motion_primitive; bool _backwards; diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp index 6baf51f4a4..18c2e1315f 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -26,7 +26,7 @@ namespace nav2_smac_planner { -typedef std::pair NodeHeuristicPair; +typedef std::pair NodeHeuristicPair; /** * @struct nav2_smac_planner::SearchInfo diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 92a89230e9..8c8ec7197f 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -120,7 +120,7 @@ void AStarAlgorithm::setCollisionChecker(GridCollisionChecker * collision template typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( - const unsigned int & index) + const uint64_t & index) { auto iter = _graph.find(index); if (iter != _graph.end()) { @@ -287,9 +287,11 @@ bool AStarAlgorithm::createPath( int closest_distance = std::numeric_limits::max(); // Given an index, return a node ptr reference if its collision-free and valid - const unsigned int max_index = getSizeX() * getSizeY() * getSizeDim3(); + const uint64_t max_index = static_cast(getSizeX()) * + static_cast(getSizeY()) * + static_cast(getSizeDim3()); NodeGetter neighborGetter = - [&, this](const unsigned int & index, NodePtr & neighbor_rtn) -> bool + [&, this](const uint64_t & index, NodePtr & neighbor_rtn) -> bool { if (index >= max_index) { return false; diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 5f5f97d9ec..f2667d3ba8 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -201,7 +201,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion Node2D::_neighbors_grid_offsets; float Node2D::cost_travel_multiplier = 2.0; -Node2D::Node2D(const unsigned int index) +Node2D::Node2D(const uint64_t index) : parent(nullptr), _cell_cost(std::numeric_limits::quiet_NaN()), _accumulated_cost(std::numeric_limits::max()), @@ -108,7 +108,8 @@ void Node2D::initMotionModel( } void Node2D::getNeighbors( - std::function & NeighborGetter, + std::function & NeighborGetter, GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors) @@ -124,9 +125,9 @@ void Node2D::getNeighbors( // 100 100 100 where lower-middle '100' is visited with same cost by both bottom '50' nodes // Therefore, it is valuable to have some low-potential across the entire map // rather than a small inflation around the obstacles - int index; + uint64_t index; NodePtr neighbor; - int node_i = this->getIndex(); + uint64_t node_i = this->getIndex(); const Coordinates parent = getCoords(this->getIndex()); Coordinates child; diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 63f121de3d..f5eec4ec5d 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -330,7 +330,8 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) unsigned int HybridMotionTable::getClosestAngularBin(const double & theta) { - return static_cast(floor(static_cast(theta) / bin_size)); + return static_cast(floor(theta / static_cast(bin_size))) % + num_angle_quantization; } float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) @@ -338,7 +339,7 @@ float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) return bin_idx * bin_size; } -NodeHybrid::NodeHybrid(const unsigned int index) +NodeHybrid::NodeHybrid(const uint64_t index) : parent(nullptr), pose(0.0f, 0.0f, 0.0f), _cell_cost(std::numeric_limits::quiet_NaN()), @@ -468,7 +469,7 @@ void NodeHybrid::initMotionModel( } inline float distanceHeuristic2D( - const unsigned int idx, const unsigned int size_x, + const uint64_t idx, const unsigned int size_x, const unsigned int target_x, const unsigned int target_y) { int dx = static_cast(idx % size_x) - static_cast(target_x); @@ -611,8 +612,8 @@ float NodeHybrid::getObstacleHeuristic( const int size_x_int = static_cast(size_x); const float sqrt2 = sqrtf(2.0f); float c_cost, cost, travel_cost, new_cost, existing_cost; - unsigned int idx, mx, my; - unsigned int new_idx = 0; + unsigned int mx, my; + unsigned int idx, new_idx = 0; const std::vector neighborhood = {1, -1, // left right size_x_int, -size_x_int, // up down @@ -855,12 +856,13 @@ void NodeHybrid::precomputeDistanceHeuristic( } void NodeHybrid::getNeighbors( - std::function & NeighborGetter, + std::function & NeighborGetter, GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors) { - unsigned int index = 0; + uint64_t index = 0; NodePtr neighbor = nullptr; Coordinates initial_node_coords; const MotionPoses motion_projections = motion_table.getProjections(this); diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index c3303e0a11..c68a4e60e1 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -180,7 +180,7 @@ float & LatticeMotionTable::getAngleFromBin(const unsigned int & bin_idx) return lattice_metadata.heading_angles[bin_idx]; } -NodeLattice::NodeLattice(const unsigned int index) +NodeLattice::NodeLattice(const uint64_t index) : parent(nullptr), pose(0.0f, 0.0f, 0.0f), _cell_cost(std::numeric_limits::quiet_NaN()), @@ -469,12 +469,13 @@ void NodeLattice::precomputeDistanceHeuristic( } void NodeLattice::getNeighbors( - std::function & NeighborGetter, + std::function & NeighborGetter, GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors) { - unsigned int index = 0; + uint64_t index = 0; float angle; bool backwards = false; NodePtr neighbor = nullptr; diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index 3671594855..1326fb0213 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -142,8 +142,10 @@ TEST(Node2DTest, test_node_2d_neighbors) unsigned char cost = static_cast(1); nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(1); node->setCost(cost); - std::function neighborGetter = - [&, this](const unsigned int & index, nav2_smac_planner::Node2D * & neighbor_rtn) -> bool + std::function neighborGetter = + [&, this](const uint64_t & index, + nav2_smac_planner::Node2D * & neighbor_rtn) -> bool { return false; }; diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index f6d917c08f..856f37c0a1 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -13,6 +13,7 @@ // limitations under the License. Reserved. #include +#include #include #include #include @@ -360,8 +361,10 @@ TEST(NodeHybridTest, test_node_reeds_neighbors) std::make_unique(costmap_ros, 72, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); nav2_smac_planner::NodeHybrid * node = new nav2_smac_planner::NodeHybrid(49); - std::function neighborGetter = - [&, this](const unsigned int & index, nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool + std::function neighborGetter = + [&, this](const uint64_t & index, + nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool { // because we don't return a real object return false; @@ -382,6 +385,7 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test) { motion_table.bin_size = 3.1415926; + motion_table.num_angle_quantization = 2; double test_theta = 3.1415926; unsigned int expected_angular_bin = 1; unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); @@ -390,17 +394,28 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test) { motion_table.bin_size = M_PI; + motion_table.num_angle_quantization = 2; double test_theta = M_PI; - unsigned int expected_angular_bin = 1; + unsigned int expected_angular_bin = 0; unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); EXPECT_EQ(expected_angular_bin, calculated_angular_bin); } { motion_table.bin_size = M_PI; + motion_table.num_angle_quantization = 2; float test_theta = M_PI; unsigned int expected_angular_bin = 1; unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); EXPECT_EQ(expected_angular_bin, calculated_angular_bin); } + + { + motion_table.bin_size = 0.0872664675; + motion_table.num_angle_quantization = 72; + double test_theta = 6.28318526567925; + unsigned int expected_angular_bin = 71; + unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); + EXPECT_EQ(expected_angular_bin, calculated_angular_bin); + } } diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp index b3eadab5f9..2355ac9a84 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -303,8 +303,10 @@ TEST(NodeLatticeTest, test_get_neighbors) std::make_unique(costmap_ros, 72, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); - std::function neighborGetter = - [&, this](const unsigned int & index, nav2_smac_planner::NodeLattice * & neighbor_rtn) -> bool + std::function neighborGetter = + [&, this](const uint64_t & index, + nav2_smac_planner::NodeLattice * & neighbor_rtn) -> bool { // because we don't return a real object return false; diff --git a/nav2_smoother/README.md b/nav2_smoother/README.md index c612dba43c..04498a0034 100644 --- a/nav2_smoother/README.md +++ b/nav2_smoother/README.md @@ -4,8 +4,8 @@ The Nav2 smoother is a Task Server in Nav2 that implements the `nav2_behavior_tr A smoothing module implementing the `nav2_behavior_tree::SmoothPath` interface is responsible for improving path smoothness and/or quality, typically given an unsmoothed path from the planner module in `nav2_planner`. It loads a map of potential smoother plugins to do the path smoothing in different user-defined situations. -See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available smoother plugins. +See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available smoother plugins. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-smoother-server.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-smoother-server.html) for additional parameter descriptions. This package contains the Simple Smoother and Savitzky-Golay Smoother plugins. diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 831beebbb3..0ac1fa9b7a 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -1,7 +1,7 @@ # Nav2 Theta Star Planner The Theta Star Planner is a global planning plugin meant to be used with the Nav2 Planner Server. The `nav2_theta_star_planner` implements a highly optimized version of the Theta\* Planner (specifically the [Lazy Theta\* P variant](http://idm-lab.org/bib/abstracts/papers/aaai10b.pdf)) meant to plan any-angle paths using A\*. The planner supports differential-drive and omni-directional robots. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-thetastar.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-thetastar.html) for additional parameter descriptions. ## Features - The planner uses A\* search along with line of sight (LOS) checks to form any-angle paths thus avoiding zig-zag paths that may be present in the usual implementation of A\* diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index 6da348c5f1..7f1ef07157 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -205,6 +205,7 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode // Connection to tell that server is still up std::unique_ptr bond_{nullptr}; + double bond_heartbeat_period; }; } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/validate_messages.hpp b/nav2_util/include/nav2_util/validate_messages.hpp index 7ce942be42..58594f1ecd 100644 --- a/nav2_util/include/nav2_util/validate_messages.hpp +++ b/nav2_util/include/nav2_util/validate_messages.hpp @@ -21,6 +21,7 @@ #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" // @brief Validation Check @@ -55,6 +56,20 @@ bool validateMsg(const double & num) return true; } +template +bool validateMsg(const std::array & msg) +{ + /* @brief value check for double-array + * like the field `covariance` used in the msg-type: + * geometry_msgs::msg::PoseWithCovarianceStamped + */ + for (const auto & element : msg) { + if (!validateMsg(element)) {return false;} + } + + return true; +} + const int NSEC_PER_SEC = 1e9; // 1 second = 1e9 nanosecond bool validateMsg(const builtin_interfaces::msg::Time & msg) { @@ -111,6 +126,23 @@ bool validateMsg(const geometry_msgs::msg::Pose & msg) return true; } +bool validateMsg(const geometry_msgs::msg::PoseWithCovariance & msg) +{ + // check sub-type + if (!validateMsg(msg.pose)) {return false;} + if (!validateMsg(msg.covariance)) {return false;} + + return true; +} + +bool validateMsg(const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +{ + // check sub-type + if (!validateMsg(msg.header)) {return false;} + if (!validateMsg(msg.pose)) {return false;} + return true; +} + // Function to verify map meta information bool validateMsg(const nav_msgs::msg::MapMetaData & msg) diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp index e09ae54b5d..5976d098a8 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/nav2_util/src/lifecycle_node.cpp @@ -19,6 +19,7 @@ #include #include "lifecycle_msgs/msg/state.hpp" +#include "nav2_util/node_utils.hpp" namespace nav2_util { @@ -35,6 +36,10 @@ LifecycleNode::LifecycleNode( rclcpp::Parameter( bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true)); + nav2_util::declare_parameter_if_not_declared( + this, "bond_heartbeat_period", rclcpp::ParameterValue(0.1)); + this->get_parameter("bond_heartbeat_period", bond_heartbeat_period); + printLifecycleNodeNotification(); register_rcl_preshutdown_callback(); @@ -55,16 +60,18 @@ LifecycleNode::~LifecycleNode() void LifecycleNode::createBond() { - RCLCPP_INFO(get_logger(), "Creating bond (%s) to lifecycle manager.", this->get_name()); + if (bond_heartbeat_period > 0.0) { + RCLCPP_INFO(get_logger(), "Creating bond (%s) to lifecycle manager.", this->get_name()); - bond_ = std::make_unique( - std::string("bond"), - this->get_name(), - shared_from_this()); + bond_ = std::make_unique( + std::string("bond"), + this->get_name(), + shared_from_this()); - bond_->setHeartbeatPeriod(0.10); - bond_->setHeartbeatTimeout(4.0); - bond_->start(); + bond_->setHeartbeatPeriod(bond_heartbeat_period); + bond_->setHeartbeatTimeout(4.0); + bond_->start(); + } } void LifecycleNode::runCleanups() @@ -110,10 +117,12 @@ void LifecycleNode::register_rcl_preshutdown_callback() void LifecycleNode::destroyBond() { - RCLCPP_INFO(get_logger(), "Destroying bond (%s) to lifecycle manager.", this->get_name()); + if (bond_heartbeat_period > 0.0) { + RCLCPP_INFO(get_logger(), "Destroying bond (%s) to lifecycle manager.", this->get_name()); - if (bond_) { - bond_.reset(); + if (bond_) { + bond_.reset(); + } } } diff --git a/nav2_util/test/test_validation_messages.cpp b/nav2_util/test/test_validation_messages.cpp index 060ec11ede..a7d8ed8904 100644 --- a/nav2_util/test/test_validation_messages.cpp +++ b/nav2_util/test/test_validation_messages.cpp @@ -244,4 +244,125 @@ TEST(ValidateMessagesTest, OccupancyGridCheck) { EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); } +TEST(ValidateMessagesTest, PoseWithCovarianceCheck) { + // Valid message + geometry_msgs::msg::PoseWithCovariance validate_msg; + validate_msg.covariance[0] = 0.25; + // assign other covariance values... + validate_msg.covariance[35] = 0.06853891909122467; + + validate_msg.pose.position.x = 0.50010401010515571; + validate_msg.pose.position.y = 1.7468730211257935; + validate_msg.pose.position.z = 0.0; + + validate_msg.pose.orientation.x = 0.9440542194053062; + validate_msg.pose.orientation.y = 0.0; + validate_msg.pose.orientation.z = 0.0; + validate_msg.pose.orientation.w = -0.32979028309372299; + + EXPECT_TRUE(nav2_util::validateMsg(validate_msg)); + + // Invalid messages + geometry_msgs::msg::PoseWithCovariance invalidate_msg1; + invalidate_msg1.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg1.covariance[7] = NAN; + invalidate_msg1.covariance[9] = NAN; + invalidate_msg1.covariance[35] = 0.06853891909122467; + + invalidate_msg1.pose.position.x = 0.50010401010515571; + invalidate_msg1.pose.position.y = 1.7468730211257935; + invalidate_msg1.pose.position.z = 0.0; + + invalidate_msg1.pose.orientation.x = 0.9440542194053062; + invalidate_msg1.pose.orientation.y = 0.0; + invalidate_msg1.pose.orientation.z = 0.0; + invalidate_msg1.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg1)); + + geometry_msgs::msg::PoseWithCovariance invalidate_msg2; + invalidate_msg2.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg2.covariance[35] = 0.06853891909122467; + + invalidate_msg2.pose.position.x = NAN; + invalidate_msg2.pose.position.y = 1.7468730211257935; + invalidate_msg2.pose.position.z = 0.0; + + invalidate_msg2.pose.orientation.x = 0.9440542194053062; + invalidate_msg2.pose.orientation.y = 0.0; + invalidate_msg2.pose.orientation.z = 0.0; + invalidate_msg2.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg2)); +} + +TEST(ValidateMessagesTest, PoseWithCovarianceStampedCheck) { + // Valid message + geometry_msgs::msg::PoseWithCovarianceStamped validate_msg; + validate_msg.header.frame_id = "map"; + validate_msg.header.stamp.sec = 1711029956; + validate_msg.header.stamp.nanosec = 146734875; + + validate_msg.pose.covariance[0] = 0.25; + // assign other covariance values... + validate_msg.pose.covariance[35] = 0.06853891909122467; + + validate_msg.pose.pose.position.x = 0.50010401010515571; + validate_msg.pose.pose.position.y = 1.7468730211257935; + validate_msg.pose.pose.position.z = 0.0; + + validate_msg.pose.pose.orientation.x = 0.9440542194053062; + validate_msg.pose.pose.orientation.y = 0.0; + validate_msg.pose.pose.orientation.z = 0.0; + validate_msg.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_TRUE(nav2_util::validateMsg(validate_msg)); + + // Invalid messages + geometry_msgs::msg::PoseWithCovarianceStamped invalidate_msg1; + invalidate_msg1.header.frame_id = "map"; + invalidate_msg1.header.stamp.sec = 1711029956; + invalidate_msg1.header.stamp.nanosec = 146734875; + + invalidate_msg1.pose.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg1.pose.covariance[7] = NAN; + invalidate_msg1.pose.covariance[9] = NAN; + invalidate_msg1.pose.covariance[35] = 0.06853891909122467; + + invalidate_msg1.pose.pose.position.x = 0.50010401010515571; + invalidate_msg1.pose.pose.position.y = 1.7468730211257935; + invalidate_msg1.pose.pose.position.z = 0.0; + + invalidate_msg1.pose.pose.orientation.x = 0.9440542194053062; + invalidate_msg1.pose.pose.orientation.y = 0.0; + invalidate_msg1.pose.pose.orientation.z = 0.0; + invalidate_msg1.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg1)); + + geometry_msgs::msg::PoseWithCovarianceStamped invalidate_msg2; + invalidate_msg2.header.frame_id = ""; + invalidate_msg2.header.stamp.sec = 1711029956; + invalidate_msg2.header.stamp.nanosec = 146734875; + + invalidate_msg2.pose.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg2.pose.covariance[35] = 0.06853891909122467; + + invalidate_msg2.pose.pose.position.x = 0.50010401010515571; + invalidate_msg2.pose.pose.position.y = 1.7468730211257935; + invalidate_msg2.pose.pose.position.z = 0.0; + + invalidate_msg2.pose.pose.orientation.x = 0.9440542194053062; + invalidate_msg2.pose.pose.orientation.y = 0.0; + invalidate_msg2.pose.pose.orientation.z = 0.0; + invalidate_msg2.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg2)); +} + + // Add more test cases for other validateMsg functions if needed diff --git a/nav2_velocity_smoother/README.md b/nav2_velocity_smoother/README.md index a9554997e2..579b3a6737 100644 --- a/nav2_velocity_smoother/README.md +++ b/nav2_velocity_smoother/README.md @@ -5,7 +5,7 @@ The aim of this package is to implement velocity, acceleration, and deadband smo It supports differential drive and omnidirectional robot platforms primarily, but is applicable to ackermann as well with some interpretations of ``Twist``. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-velocity-smoother.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-velocity-smoother.html) for additional parameter descriptions. ## Features diff --git a/nav2_waypoint_follower/README.md b/nav2_waypoint_follower/README.md index 5d6a049857..822314fbf3 100644 --- a/nav2_waypoint_follower/README.md +++ b/nav2_waypoint_follower/README.md @@ -2,7 +2,7 @@ The Nav2 waypoint follower is an example application of how to use the navigation action to complete some sort of orchestrated task. In this example, that task is to take a given set of waypoints and navigate to a set of positions in the order provided in the action request. The last waypoint in the waypoint array is the final position. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-waypoint-follower.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-waypoint-follower.html) for additional parameter descriptions. The package exposes the `follow_waypoints` action server of type `nav2_msgs/FollowWaypoints`. It is given an array of waypoints to visit, gives feedback about the current index of waypoint it is processing, and returns a list of waypoints it was unable to complete.