diff --git a/.circleci/config.yml b/.circleci/config.yml index d6766347816..edb996eb1f7 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v36\ + - "<< parameters.key >>-v38\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v36\ + - "<< parameters.key >>-v38\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v36\ + key: "<< parameters.key >>-v38\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ @@ -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/.github/workflows/update_ci_image.yaml b/.github/workflows/update_ci_image.yaml index 0f88b356f83..ef39facf09c 100644 --- a/.github/workflows/update_ci_image.yaml +++ b/.github/workflows/update_ci_image.yaml @@ -30,7 +30,7 @@ jobs: if: github.event_name == 'push' run: | echo "trigger=true" >> $GITHUB_OUTPUT - echo "no_cache=false" >> $GITHUB_OUTPUT + echo "no_cache=true" >> $GITHUB_OUTPUT check_ci_image: name: Check CI Image if: github.event_name == 'schedule' diff --git a/README.md b/README.md index 4ef84c7c30f..48a40d00e21 100644 --- a/README.md +++ b/README.md @@ -8,16 +8,16 @@

For detailed instructions on how to: -- [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) +- [Concepts](https://docs.nav2.org/concepts/index.html) and [Getting Started](https://docs.nav2.org/getting_started/index.html) +- [First Time Setup Guide](https://docs.nav2.org/setup_guides/index.html) +- [ROS Distribution Statuses](https://docs.nav2.org/#distributions) +- [Build & Install](https://docs.nav2.org/development_guides/build_docs/index.html#build) and [Docker Containers](https://github.com/orgs/ros-navigation/packages/container/package/navigation2) - [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) +- [Configuration Guide](https://docs.nav2.org/configuration/index.html) - [Navigation Plugins](https://docs.nav2.org/plugins/index.html) -- [ROSCon Talks](https://docs.nav2.org/about/roscon.html) +- [API Docs](https://api.nav2.org/) +- [ROSCon Talks](https://docs.nav2.org/about/roscon.html) and [Citations](https://docs.nav2.org/citations.html) - [Migration Guides](https://docs.nav2.org/migration/index.html) -- [Container Images for Building Nav2](https://github.com/orgs/ros-navigation/packages/container/package/navigation2) - [Contribute](https://docs.nav2.org/development_guides/involvement_docs/index.html) Please visit our [documentation site](https://docs.nav2.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-uj428p0x-jKx8U7OzK1IOWp5TnDS2rA) (if this link does not work, please contact maintainers to reactivate). diff --git a/nav2_amcl/CMakeLists.txt b/nav2_amcl/CMakeLists.txt index aa6cf1a4ed7..4813b3cdd9b 100644 --- a/nav2_amcl/CMakeLists.txt +++ b/nav2_amcl/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_amcl) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(message_filters REQUIRED) find_package(nav_msgs REQUIRED) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 0be08c39db6..65787e82b00 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -44,9 +44,9 @@ #include "sensor_msgs/msg/laser_scan.hpp" #include "nav2_ros_common/service_server.hpp" #include "std_srvs/srv/empty.hpp" -#include "tf2_ros/message_filter.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/message_filter.hpp" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/transform_listener.hpp" #define NEW_UNIFORM_SAMPLING 1 diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index c1a2cbc2cea..69ac2d324b5 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -24,6 +24,7 @@ ament_cmake nav2_common + backward_ros geometry_msgs message_filters nav_msgs diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 7fa70df770f..b3b21631a59 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -38,11 +38,11 @@ #include "tf2/utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/LinearMath/Transform.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/message_filter.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/message_filter.hpp" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_amcl/portable_utils.hpp" #include "nav2_ros_common/validate_messages.hpp" @@ -58,126 +58,6 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options) : nav2::LifecycleNode("amcl", "", options) { RCLCPP_INFO(get_logger(), "Creating"); - - declare_parameter( - "alpha1", rclcpp::ParameterValue(0.2)); - - declare_parameter( - "alpha2", rclcpp::ParameterValue(0.2)); - - declare_parameter( - "alpha3", rclcpp::ParameterValue(0.2)); - - declare_parameter( - "alpha4", rclcpp::ParameterValue(0.2)); - - declare_parameter( - "alpha5", rclcpp::ParameterValue(0.2)); - - declare_parameter( - "base_frame_id", rclcpp::ParameterValue(std::string("base_footprint"))); - - declare_parameter("beam_skip_distance", rclcpp::ParameterValue(0.5)); - declare_parameter("beam_skip_error_threshold", rclcpp::ParameterValue(0.9)); - declare_parameter("beam_skip_threshold", rclcpp::ParameterValue(0.3)); - declare_parameter("do_beamskip", rclcpp::ParameterValue(false)); - - declare_parameter( - "global_frame_id", rclcpp::ParameterValue(std::string("map"))); - - declare_parameter( - "lambda_short", rclcpp::ParameterValue(0.1)); - - declare_parameter( - "laser_likelihood_max_dist", rclcpp::ParameterValue(2.0)); - - declare_parameter( - "laser_max_range", rclcpp::ParameterValue(100.0)); - - declare_parameter( - "laser_min_range", rclcpp::ParameterValue(-1.0)); - - declare_parameter( - "laser_model_type", rclcpp::ParameterValue(std::string("likelihood_field"))); - - declare_parameter( - "set_initial_pose", rclcpp::ParameterValue(false)); - - declare_parameter( - "initial_pose.x", rclcpp::ParameterValue(0.0)); - - declare_parameter( - "initial_pose.y", rclcpp::ParameterValue(0.0)); - - declare_parameter( - "initial_pose.z", rclcpp::ParameterValue(0.0)); - - declare_parameter( - "initial_pose.yaw", rclcpp::ParameterValue(0.0)); - - declare_parameter( - "max_beams", rclcpp::ParameterValue(60)); - - declare_parameter( - "max_particles", rclcpp::ParameterValue(2000)); - - declare_parameter( - "min_particles", rclcpp::ParameterValue(500)); - - declare_parameter( - "odom_frame_id", rclcpp::ParameterValue(std::string("odom"))); - - declare_parameter("pf_err", rclcpp::ParameterValue(0.05)); - declare_parameter("pf_z", rclcpp::ParameterValue(0.99)); - - declare_parameter( - "recovery_alpha_fast", rclcpp::ParameterValue(0.0)); - - declare_parameter( - "recovery_alpha_slow", rclcpp::ParameterValue(0.0)); - - declare_parameter( - "resample_interval", rclcpp::ParameterValue(1)); - - declare_parameter("robot_model_type", - rclcpp::ParameterValue("nav2_amcl::DifferentialMotionModel")); - - declare_parameter( - "save_pose_rate", rclcpp::ParameterValue(0.5)); - - declare_parameter("sigma_hit", rclcpp::ParameterValue(0.2)); - - declare_parameter( - "tf_broadcast", rclcpp::ParameterValue(true)); - - declare_parameter( - "transform_tolerance", rclcpp::ParameterValue(1.0)); - - declare_parameter( - "update_min_a", rclcpp::ParameterValue(0.2)); - - declare_parameter( - "update_min_d", rclcpp::ParameterValue(0.25)); - - declare_parameter("z_hit", rclcpp::ParameterValue(0.5)); - declare_parameter("z_max", rclcpp::ParameterValue(0.05)); - declare_parameter("z_rand", rclcpp::ParameterValue(0.5)); - declare_parameter("z_short", rclcpp::ParameterValue(0.05)); - - declare_parameter( - "always_reset_initial_pose", rclcpp::ParameterValue(false)); - - declare_parameter( - "scan_topic", rclcpp::ParameterValue("scan")); - - declare_parameter( - "map_topic", rclcpp::ParameterValue("map")); - - declare_parameter( - "first_map_only", rclcpp::ParameterValue(false)); - - declare_parameter( - "freespace_downsampling", rclcpp::ParameterValue(false)); } AmclNode::~AmclNode() @@ -1013,52 +893,54 @@ AmclNode::initParameters() double save_pose_rate; double tmp_tol; - get_parameter("alpha1", alpha1_); - get_parameter("alpha2", alpha2_); - get_parameter("alpha3", alpha3_); - get_parameter("alpha4", alpha4_); - get_parameter("alpha5", alpha5_); - get_parameter("base_frame_id", base_frame_id_); - get_parameter("beam_skip_distance", beam_skip_distance_); - get_parameter("beam_skip_error_threshold", beam_skip_error_threshold_); - get_parameter("beam_skip_threshold", beam_skip_threshold_); - get_parameter("do_beamskip", do_beamskip_); - get_parameter("global_frame_id", global_frame_id_); - get_parameter("lambda_short", lambda_short_); - get_parameter("laser_likelihood_max_dist", laser_likelihood_max_dist_); - get_parameter("laser_max_range", laser_max_range_); - get_parameter("laser_min_range", laser_min_range_); - get_parameter("laser_model_type", sensor_model_type_); - get_parameter("set_initial_pose", set_initial_pose_); - get_parameter("initial_pose.x", initial_pose_x_); - get_parameter("initial_pose.y", initial_pose_y_); - get_parameter("initial_pose.z", initial_pose_z_); - get_parameter("initial_pose.yaw", initial_pose_yaw_); - get_parameter("max_beams", max_beams_); - get_parameter("max_particles", max_particles_); - get_parameter("min_particles", min_particles_); - get_parameter("odom_frame_id", odom_frame_id_); - get_parameter("pf_err", pf_err_); - get_parameter("pf_z", pf_z_); - get_parameter("recovery_alpha_fast", alpha_fast_); - get_parameter("recovery_alpha_slow", alpha_slow_); - get_parameter("resample_interval", resample_interval_); - get_parameter("robot_model_type", robot_model_type_); - get_parameter("save_pose_rate", save_pose_rate); - get_parameter("sigma_hit", sigma_hit_); - get_parameter("tf_broadcast", tf_broadcast_); - get_parameter("transform_tolerance", tmp_tol); - get_parameter("update_min_a", a_thresh_); - get_parameter("update_min_d", d_thresh_); - get_parameter("z_hit", z_hit_); - get_parameter("z_max", z_max_); - get_parameter("z_rand", z_rand_); - get_parameter("z_short", z_short_); - get_parameter("first_map_only", first_map_only_); - get_parameter("always_reset_initial_pose", always_reset_initial_pose_); - get_parameter("scan_topic", scan_topic_); - get_parameter("map_topic", map_topic_); - get_parameter("freespace_downsampling", freespace_downsampling_); + alpha1_ = this->declare_or_get_parameter("alpha1", 0.2); + alpha2_ = this->declare_or_get_parameter("alpha2", 0.2); + alpha3_ = this->declare_or_get_parameter("alpha3", 0.2); + alpha4_ = this->declare_or_get_parameter("alpha4", 0.2); + alpha5_ = this->declare_or_get_parameter("alpha5", 0.2); + base_frame_id_ = this->declare_or_get_parameter("base_frame_id", std::string{"base_footprint"}); + beam_skip_distance_ = this->declare_or_get_parameter("beam_skip_distance", 0.5); + beam_skip_error_threshold_ = this->declare_or_get_parameter("beam_skip_error_threshold", 0.9); + beam_skip_threshold_ = this->declare_or_get_parameter("beam_skip_threshold", 0.3); + do_beamskip_ = this->declare_or_get_parameter("do_beamskip", false); + global_frame_id_ = this->declare_or_get_parameter("global_frame_id", std::string{"map"}); + lambda_short_ = this->declare_or_get_parameter("lambda_short", 0.1); + laser_likelihood_max_dist_ = this->declare_or_get_parameter("laser_likelihood_max_dist", 2.0); + laser_max_range_ = this->declare_or_get_parameter("laser_max_range", 100.0); + laser_min_range_ = this->declare_or_get_parameter("laser_min_range", -1.0); + sensor_model_type_ = this->declare_or_get_parameter( + "laser_model_type", std::string{"likelihood_field"}); + set_initial_pose_ = this->declare_or_get_parameter("set_initial_pose", false); + initial_pose_x_ = this->declare_or_get_parameter("initial_pose.x", 0.0); + initial_pose_y_ = this->declare_or_get_parameter("initial_pose.y", 0.0); + initial_pose_z_ = this->declare_or_get_parameter("initial_pose.z", 0.0); + initial_pose_yaw_ = this->declare_or_get_parameter("initial_pose.yaw", 0.0); + max_beams_ = this->declare_or_get_parameter("max_beams", 60); + max_particles_ = this->declare_or_get_parameter("max_particles", 2000); + min_particles_ = this->declare_or_get_parameter("min_particles", 500); + odom_frame_id_ = this->declare_or_get_parameter("odom_frame_id", std::string{"odom"}); + pf_err_ = this->declare_or_get_parameter("pf_err", 0.05); + pf_z_ = this->declare_or_get_parameter("pf_z", 0.99); + alpha_fast_ = this->declare_or_get_parameter("recovery_alpha_fast", 0.0); + alpha_slow_ = this->declare_or_get_parameter("recovery_alpha_slow", 0.0); + resample_interval_ = this->declare_or_get_parameter("resample_interval", 1); + robot_model_type_ = this->declare_or_get_parameter( + "robot_model_type", std::string{"nav2_amcl::DifferentialMotionModel"}); + save_pose_rate = this->declare_or_get_parameter("save_pose_rate", 0.5); + sigma_hit_ = this->declare_or_get_parameter("sigma_hit", 0.2); + tf_broadcast_ = this->declare_or_get_parameter("tf_broadcast", true); + tmp_tol = this->declare_or_get_parameter("transform_tolerance", 1.0); + a_thresh_ = this->declare_or_get_parameter("update_min_a", 0.2); + d_thresh_ = this->declare_or_get_parameter("update_min_d", 0.25); + z_hit_ = this->declare_or_get_parameter("z_hit", 0.5); + z_max_ = this->declare_or_get_parameter("z_max", 0.05); + z_rand_ = this->declare_or_get_parameter("z_rand", 0.5); + z_short_ = this->declare_or_get_parameter("z_short", 0.05); + first_map_only_ = this->declare_or_get_parameter("first_map_only", false); + always_reset_initial_pose_ = this->declare_or_get_parameter("always_reset_initial_pose", false); + scan_topic_ = this->declare_or_get_parameter("scan_topic", std::string{"scan"}); + map_topic_ = this->declare_or_get_parameter("map_topic", std::string{"map"}); + freespace_downsampling_ = this->declare_or_get_parameter("freespace_downsampling", false); save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate); transform_tolerance_ = tf2::durationFromSec(tmp_tol); @@ -1457,7 +1339,7 @@ AmclNode::initTransforms() get_node_timers_interface(), callback_group_); tf_buffer_->setCreateTimerInterface(timer_interface); - tf_listener_ = std::make_shared(*tf_buffer_); + tf_listener_ = std::make_shared(*tf_buffer_, this, true); tf_broadcaster_ = std::make_shared(shared_from_this()); sent_first_transform_ = false; diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 03339b19cdc..de7d0f6237f 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_behavior_tree CXX) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(action_msgs REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(geometry_msgs REQUIRED) @@ -218,6 +219,12 @@ list(APPEND plugin_libs nav2_nonblocking_sequence_bt_node) add_library(nav2_round_robin_node_bt_node SHARED plugins/control/round_robin_node.cpp) list(APPEND plugin_libs nav2_round_robin_node_bt_node) +add_library(nav2_pause_resume_controller_bt_node SHARED plugins/control/pause_resume_controller.cpp) +list(APPEND plugin_libs nav2_pause_resume_controller_bt_node) + +add_library(nav2_persistent_sequence_bt_node SHARED plugins/control/persistent_sequence.cpp) +list(APPEND plugin_libs nav2_persistent_sequence_bt_node) + add_library(nav2_single_trigger_bt_node SHARED plugins/decorator/single_trigger_node.cpp) list(APPEND plugin_libs nav2_single_trigger_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp index 48a4e407d1b..b699e5b6089 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -19,7 +19,7 @@ #include #include "nav2_msgs/action/compute_path_through_poses.hpp" -#include "nav_msgs/msg/path.h" +#include "nav_msgs/msg/path.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" #include "nav2_ros_common/lifecycle_node.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp index 626dd2cf591..ca2d623cbff 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp @@ -18,7 +18,7 @@ #include #include "nav2_msgs/action/compute_path_to_pose.hpp" -#include "nav_msgs/msg/path.h" +#include "nav_msgs/msg/path.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" #include "nav2_ros_common/lifecycle_node.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_route_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_route_action.hpp index c54d0f73b83..7fbbfdf2741 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_route_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_route_action.hpp @@ -18,7 +18,7 @@ #include #include "nav2_msgs/action/compute_route.hpp" -#include "nav_msgs/msg/path.h" +#include "nav_msgs/msg/path.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" #include "nav2_ros_common/lifecycle_node.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp index d757f5665c9..44ec643146f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp @@ -22,7 +22,7 @@ #include "behaviortree_cpp/action_node.h" #include "behaviortree_cpp/json_export.h" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/path.h" +#include "nav_msgs/msg/path.hpp" #include "nav2_behavior_tree/bt_utils.hpp" #include "nav2_behavior_tree/json_utils.hpp" #include "nav2_util/geometry_utils.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp index d49e99077f0..381f3f47c19 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp @@ -19,7 +19,7 @@ #include #include "nav2_msgs/action/smooth_path.hpp" -#include "nav_msgs/msg/path.h" +#include "nav_msgs/msg/path.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" #include "nav2_ros_common/lifecycle_node.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp index 6ce8a76fd64..9ffc5aa773f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp @@ -25,7 +25,7 @@ #include "nav_msgs/msg/path.hpp" #include "nav2_behavior_tree/bt_utils.hpp" #include "nav2_behavior_tree/json_utils.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_ros_common/lifecycle_node.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp index db615c06181..eb099f329b0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp @@ -20,7 +20,7 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "behaviortree_cpp/condition_node.h" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp index abcf722b872..c75f8b78d98 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp @@ -23,7 +23,7 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp index aeebfcb3634..db3c56f24b7 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp @@ -23,7 +23,7 @@ #include "behaviortree_cpp/json_export.h" #include "nav2_behavior_tree/bt_utils.hpp" #include "nav2_behavior_tree/json_utils.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp index 41a86ee813d..be1bcd6ee18 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp @@ -21,7 +21,7 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "behaviortree_cpp/condition_node.h" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp new file mode 100644 index 00000000000..fa5bfa35f2f --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp @@ -0,0 +1,153 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_ + +// Other includes +#include +#include +#include + +// ROS includes +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/callback_group.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "behaviortree_cpp/control_node.h" +#include "nav2_ros_common/service_server.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" + +// Interface definitions +#include "std_srvs/srv/trigger.hpp" + + +namespace nav2_behavior_tree +{ + +using Trigger = std_srvs::srv::Trigger; + +enum state_t {RESUMED, PAUSED, PAUSE_REQUESTED, ON_PAUSE, RESUME_REQUESTED, ON_RESUME}; +const std::map state_names = { + {RESUMED, "RESUMED"}, + {PAUSED, "PAUSED"}, + {PAUSE_REQUESTED, "PAUSE_REQUESTED"}, + {ON_PAUSE, "ON_PAUSE"}, + {RESUME_REQUESTED, "RESUME_REQUESTED"}, + {ON_RESUME, "ON_RESUME"} +}; +const std::map child_indices = { + {RESUMED, 0}, + {PAUSED, 1}, + {ON_PAUSE, 2}, + {ON_RESUME, 3} +}; + +/* @brief Controlled through service calls to pause and resume the execution of the tree + * It has one mandatory child for the RESUMED, and three optional for the PAUSED state, + * the ON_PAUSE event and the ON_RESUME event. + * It has two input ports: + * - pause_service_name: name of the service to pause + * - resume_service_name: name of the service to resume + * + * Usage: + * + * + * + * + * + * + * + * + * + * + * The controller starts in RESUMED state, and ticks it until it returns success. + * When the pause service is called, ON_PAUSE is ticked until completion, + * then the controller switches to PAUSED state. + * When the resume service is called, ON_RESUME is ticked until completion, + * then the controller switches back to RESUMED state. + * + * The controller only returns success when the RESUMED child returns success. + * The controller returns failure if any child returns failure. + * In any other case, it returns running. + */ + + +class PauseResumeController : public BT::ControlNode +{ +public: + //! @brief Constructor + PauseResumeController( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + //! @brief Reset state and go to Idle + void halt() override; + + //! @brief Handle transitions if requested and tick child related to the actual state + BT::NodeStatus tick() override; + + //! @brief Declare ports + static BT::PortsList providedPorts() + { + return { + BT::InputPort( + "pause_service_name", + "Name of the service to pause"), + BT::InputPort( + "resume_service_name", + "Name of the service to resume"), + }; + } + + [[nodiscard]] inline state_t getState() const + { + return state_; + } + +private: + //! @brief Set state to PAUSE_REQUESTED + void pauseServiceCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response); + + //! @brief Set state to RESUME_REQUESTED + void resumeServiceCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response); + + /** @brief Switch to the next state based on the current state + * + * PAUSE_REQUESTED -> ON_PAUSE + * ON_PAUSE -> PAUSED + * + * RESUME_REQUESTED -> ON_RESUME + * ON_RESUME -> RESUMED + * + * Do nothing if in end state + */ + void switchToNextState(); + + rclcpp::Logger logger_{rclcpp::get_logger("PauseResumeController")}; + rclcpp::CallbackGroup::SharedPtr cb_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + nav2::ServiceServer::SharedPtr pause_srv_; + nav2::ServiceServer::SharedPtr resume_srv_; + state_t state_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/persistent_sequence.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/persistent_sequence.hpp new file mode 100644 index 00000000000..79d494eb8e6 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/persistent_sequence.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PERSISTENT_SEQUENCE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PERSISTENT_SEQUENCE_HPP_ + +#include +#include "behaviortree_cpp/control_node.h" +#include "behaviortree_cpp/bt_factory.h" + +namespace nav2_behavior_tree +{ +/** + * @brief The PersistentSequenceNode is similar to the SequenceNode, but it + * stores the index of the last running child in the blackboard (key: "current_child_idx"), + * and it does not reset the index when it got halted. + * It used to tick children in an ordered sequence. If any child returns RUNNING, previous + * children will NOT be ticked again. + * + * - If all the children return SUCCESS, this node returns SUCCESS. + * + * - If a child returns RUNNING, this node returns RUNNING. + * Loop is NOT restarted, the same running child will be ticked again. + * + * - If a child returns FAILURE, stop the loop and return FAILURE. + * Restart the loop only if (reset_on_failure == true) + * + */ +class PersistentSequenceNode : public BT::ControlNode +{ +public: + PersistentSequenceNode(const std::string & name, const BT::NodeConfiguration & conf); + + ~PersistentSequenceNode() override = default; + + //! @brief Declare ports + static BT::PortsList providedPorts() + { + return { + BT::BidirectionalPort("current_child_idx", "The index of the current child"), + }; + } + +private: + BT::NodeStatus tick() override; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PERSISTENT_SEQUENCE_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp index 64e4a7c4a75..782e89a8f89 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp @@ -20,7 +20,7 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "behaviortree_cpp/decorator_node.h" #include "nav2_behavior_tree/bt_utils.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp index e418c5846ad..57ca3d4ffc3 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp @@ -22,9 +22,9 @@ #include "behaviortree_cpp/loggers/abstract_logger.h" #include "rclcpp/rclcpp.hpp" #include "nav2_msgs/msg/behavior_tree_log.hpp" -#include "nav2_msgs/msg/behavior_tree_status_change.h" +#include "nav2_msgs/msg/behavior_tree_status_change.hpp" #include "tf2/time.hpp" -#include "tf2_ros/buffer_interface.h" +#include "tf2_ros/buffer_interface.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index a9798c0c64c..230ce02f70c 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -404,6 +404,16 @@ + + + Service to call to pause + Service to call to resume + + + + Index of the child to execute + + Rate diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index e931df421e2..69bcb1466f7 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -12,6 +12,7 @@ ament_cmake action_msgs + backward_ros behaviortree_cpp geometry_msgs nav2_common diff --git a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp index bf8e58b9184..7cae783bb9f 100644 --- a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp +++ b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp @@ -25,8 +25,8 @@ #include "nav_msgs/msg/path.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/LinearMath/Quaternion.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp" diff --git a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp index b377d47c585..914a2be547a 100644 --- a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp @@ -18,7 +18,7 @@ #include "rclcpp/rclcpp.hpp" #include "tf2/time.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_behavior_tree/plugins/condition/transform_available_condition.hpp" diff --git a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp new file mode 100644 index 00000000000..60b269fa2e0 --- /dev/null +++ b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp @@ -0,0 +1,188 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Other includes +#include + +// ROS includes +#include "nav2_behavior_tree/plugins/control/pause_resume_controller.hpp" + +// Other includes +#include "behaviortree_cpp/bt_factory.h" + +namespace nav2_behavior_tree +{ + +using namespace std::placeholders; + +PauseResumeController::PauseResumeController( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf) +: BT::ControlNode(xml_tag_name, conf) +{ + auto node = config().blackboard->get("node"); + state_ = RESUMED; + + // Create a separate cb group with a separate executor to spin + cb_group_ = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + + executor_ = + std::make_shared(); + + executor_->add_callback_group(cb_group_, node->get_node_base_interface()); + + std::string pause_service_name; + getInput("pause_service_name", pause_service_name); + pause_srv_ = node->create_service( + pause_service_name, + std::bind(&PauseResumeController::pauseServiceCallback, this, _1, _2, _3), + cb_group_); + + std::string resume_service_name; + getInput("resume_service_name", resume_service_name); + resume_srv_ = node->create_service( + resume_service_name, + std::bind(&PauseResumeController::resumeServiceCallback, this, _1, _2, _3), + cb_group_); +} + +BT::NodeStatus PauseResumeController::tick() +{ + unsigned int children_count = children_nodes_.size(); + if (children_count < 1 || children_count > 4) { + throw std::runtime_error( + "PauseNode must have at least one and at most four children " + "(currently has " + std::to_string(children_count) + ")"); + } + + if (status() == BT::NodeStatus::IDLE) { + state_ = RESUMED; + } + setStatus(BT::NodeStatus::RUNNING); + + executor_->spin_some(); + + // If pause / resume requested, reset children and switch to transient child + if (state_ == PAUSE_REQUESTED || state_ == RESUME_REQUESTED) { + resetChildren(); + switchToNextState(); + } + + // Return RUNNING and do nothing if specific child is not used + const uint16_t child_idx = child_indices.at(state_); + if (children_nodes_.size() <= child_idx) { + switchToNextState(); + return BT::NodeStatus::RUNNING; + } + + // If child is used, tick it + const BT::NodeStatus child_status = + children_nodes_[child_indices.at(state_)]->executeTick(); + + switch (child_status) { + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; + case BT::NodeStatus::SUCCESS: + case BT::NodeStatus::SKIPPED: + if (state_ == RESUMED) { + // Resumed child returned SUCCESS, we're done + return BT::NodeStatus::SUCCESS; + } + switchToNextState(); + // If any branch other than RESUMED returned SUCCESS, keep ticking + return BT::NodeStatus::RUNNING; + case BT::NodeStatus::FAILURE: + RCLCPP_ERROR( + logger_, "%s child returned FAILURE", state_names.at(state_).c_str()); + return BT::NodeStatus::FAILURE; + default: + throw std::runtime_error("A child node must never return IDLE"); + } +} + +void PauseResumeController::switchToNextState() +{ + static const std::map next_states = { + {PAUSE_REQUESTED, ON_PAUSE}, + {ON_PAUSE, PAUSED}, + {RESUME_REQUESTED, ON_RESUME}, + {ON_RESUME, RESUMED} + }; + + if (state_ == PAUSED || state_ == RESUMED) { + // No next state, do nothing + return; + } + + state_ = next_states.at(state_); + RCLCPP_INFO(logger_, "Switched to state: %s", state_names.at(state_).c_str()); +} + +void PauseResumeController::pauseServiceCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response) +{ + if (state_ != PAUSED) { + RCLCPP_INFO(logger_, "Received pause request"); + response->success = true; + state_ = PAUSE_REQUESTED; + return; + } + + std::string warn_message = "PauseResumeController BT node already in state PAUSED"; + RCLCPP_WARN(logger_, "%s", warn_message.c_str()); + response->success = false; + response->message = warn_message; +} + +void PauseResumeController::resumeServiceCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response) +{ + if (state_ == PAUSED) { + RCLCPP_INFO(logger_, "Received resume request"); + response->success = true; + state_ = RESUME_REQUESTED; + return; + } + + std::string warn_message = "PauseResumeController BT node not in state PAUSED"; + RCLCPP_WARN(logger_, "%s", warn_message.c_str()); + response->success = false; + response->message = warn_message; +} + +void PauseResumeController::halt() +{ + state_ = RESUMED; + ControlNode::halt(); +} + +} // namespace nav2_behavior_tree + +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory.registerBuilder( + "PauseResumeController", builder); +} diff --git a/nav2_behavior_tree/plugins/control/persistent_sequence.cpp b/nav2_behavior_tree/plugins/control/persistent_sequence.cpp new file mode 100644 index 00000000000..1886deb3d5c --- /dev/null +++ b/nav2_behavior_tree/plugins/control/persistent_sequence.cpp @@ -0,0 +1,84 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_behavior_tree/plugins/control/persistent_sequence.hpp" +#include "behaviortree_cpp/action_node.h" +#include "behaviortree_cpp/bt_factory.h" + +namespace nav2_behavior_tree +{ + +PersistentSequenceNode::PersistentSequenceNode( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ControlNode::ControlNode(name, conf) {} + +BT::NodeStatus PersistentSequenceNode::tick() +{ + const int children_count = children_nodes_.size(); + + int current_child_idx; + getInput("current_child_idx", current_child_idx); + + setStatus(BT::NodeStatus::RUNNING); + + while (current_child_idx < children_count) { + TreeNode * current_child_node = children_nodes_[current_child_idx]; + const BT::NodeStatus child_status = current_child_node->executeTick(); + + switch (child_status) { + case BT::NodeStatus::RUNNING: + return child_status; + + case BT::NodeStatus::FAILURE: + // Reset on failure + resetChildren(); + current_child_idx = 0; + setOutput("current_child_idx", 0); + return child_status; + + case BT::NodeStatus::SUCCESS: + case BT::NodeStatus::SKIPPED: + // Skip the child node + current_child_idx++; + setOutput("current_child_idx", current_child_idx); + break; + + case BT::NodeStatus::IDLE: + throw std::runtime_error("A child node must never return IDLE"); + } // end switch + } // end while loop + + // The entire while loop completed. This means that all the children returned SUCCESS. + if (current_child_idx >= children_count) { + resetChildren(); + setOutput("current_child_idx", 0); + } + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory.registerBuilder( + "PersistentSequence", builder); +} diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp index 0fdd47e7ffb..b560cddf2e7 100644 --- a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp @@ -21,7 +21,7 @@ #include "nav2_util/robot_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "behaviortree_cpp/decorator_node.h" diff --git a/nav2_behavior_tree/test/plugins/control/CMakeLists.txt b/nav2_behavior_tree/test/plugins/control/CMakeLists.txt index 1a8b0cf781a..a625bdad792 100644 --- a/nav2_behavior_tree/test/plugins/control/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/control/CMakeLists.txt @@ -5,3 +5,8 @@ plugin_add_test(test_control_pipeline_sequence test_pipeline_sequence.cpp nav2_p plugin_add_test(test_control_round_robin_node test_round_robin_node.cpp nav2_round_robin_node_bt_node) plugin_add_test(test_control_nonblocking_sequence test_nonblocking_sequence.cpp nav2_nonblocking_sequence_bt_node) + +plugin_add_test(test_control_pause_resume_controller + test_pause_resume_controller.cpp nav2_pause_resume_controller_bt_node) + +plugin_add_test(test_persistent_sequence test_persistent_sequence.cpp nav2_persistent_sequence_bt_node) diff --git a/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp b/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp new file mode 100644 index 00000000000..a546bcfb565 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp @@ -0,0 +1,253 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "utils/test_behavior_tree_fixture.hpp" +#include "utils/test_dummy_tree_node.hpp" +#include "utils/get_node_from_tree.hpp" +#include "nav2_behavior_tree/plugins/control/pause_resume_controller.hpp" +#include "std_srvs/srv/trigger.hpp" + +class PauseResumeControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + static void SetUpTestCase() + { + nav2_behavior_tree::BehaviorTreeTestFixture::SetUpTestCase(); + + ASSERT_TRUE(node_); + pause_client_ = node_->create_client("pause", true); + resume_client_ = node_->create_client("resume", true); + + ASSERT_TRUE(factory_); + factory_->registerNodeType("PauseResumeController"); + factory_->registerNodeType("DummyNode"); + } + + static void TearDownTestCase() + { + tree_.reset(); + pause_client_.reset(); + resume_client_.reset(); + } + + static void CheckServiceResult( + const nav2::ServiceClient::SharedPtr & client, + std::shared_future> future, + bool success = true) + { + const auto res = client->spin_until_complete(future, std::chrono::seconds(1)); + ASSERT_EQ(res, rclcpp::FutureReturnCode::SUCCESS); + ASSERT_EQ(future.wait_for(std::chrono::seconds(0)), std::future_status::ready); + EXPECT_EQ(future.get()->success, success); + } + +protected: + static std::shared_ptr tree_; + static nav2::ServiceClient::SharedPtr pause_client_; + static nav2::ServiceClient::SharedPtr resume_client_; +}; + +std::shared_ptr PauseResumeControllerTestFixture::tree_ = nullptr; +nav2::ServiceClient::SharedPtr +PauseResumeControllerTestFixture::pause_client_ = nullptr; +nav2::ServiceClient::SharedPtr +PauseResumeControllerTestFixture::resume_client_ = nullptr; + +TEST_F(PauseResumeControllerTestFixture, test_incorrect_num_children) +{ + // create tree with incorrect number of children + std::string xml_txt = + R"( + + + + + + )"; + EXPECT_THROW( + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard), + BT::RuntimeError); +} + +TEST_F(PauseResumeControllerTestFixture, test_unused_children) +{ + // create tree with only RESUMED child + std::string xml_txt = + R"( + + + + + + + )"; + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // get pause_resume_controller so we can check state + auto pause_bt_node = + nav2_behavior_tree::get_node_from_tree(tree_); + ASSERT_NE(pause_bt_node, nullptr); + using state_t = nav2_behavior_tree::state_t; + + // get dummy nodes so we can change their status + auto resumed_child = + nav2_behavior_tree::get_node_from_tree(tree_, 0); + ASSERT_NE(resumed_child, nullptr); + resumed_child->changeStatus(BT::NodeStatus::RUNNING); + + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + + // Call pause service, expect RUNNING and PAUSED + auto req = std::make_shared(); + auto future = pause_client_->async_call(req); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + CheckServiceResult(pause_client_, future); + + // Tick again, expect RUNNING and PAUSED + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + + // Call resume service, expect RUNNING and ON_RESUME + future = resume_client_->async_call(req); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + CheckServiceResult(resume_client_, future); + + // Tick again, expect RUNNING and RESUMED + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); +} + +TEST_F(PauseResumeControllerTestFixture, test_behavior) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + + + + )"; + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // get pause_resume_controller so we can check state + auto pause_bt_node = + nav2_behavior_tree::get_node_from_tree(tree_); + ASSERT_NE(pause_bt_node, nullptr); + using state_t = nav2_behavior_tree::state_t; + + // get dummy nodes so we can change their status + auto resumed_child = + nav2_behavior_tree::get_node_from_tree(tree_, 0); + auto paused_child = + nav2_behavior_tree::get_node_from_tree(tree_, 1); + auto on_pause_child = + nav2_behavior_tree::get_node_from_tree(tree_, 2); + auto on_resume_child = + nav2_behavior_tree::get_node_from_tree(tree_, 3); + ASSERT_NE(resumed_child, nullptr); + ASSERT_NE(paused_child, nullptr); + ASSERT_NE(on_pause_child, nullptr); + ASSERT_NE(on_resume_child, nullptr); + + resumed_child->changeStatus(BT::NodeStatus::RUNNING); + paused_child->changeStatus(BT::NodeStatus::RUNNING); + on_pause_child->changeStatus(BT::NodeStatus::RUNNING); + on_resume_child->changeStatus(BT::NodeStatus::RUNNING); + + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + + // Call pause service, set ON_PAUSE child to RUNNING, expect RUNNING and ON_PAUSE + auto req = std::make_shared(); + auto future = pause_client_->async_call(req); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::ON_PAUSE); + CheckServiceResult(pause_client_, future); + + // Change ON_PAUSE child to SUCCESS, expect RUNNING and PAUSED + on_pause_child->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + + // Set PAUSED child to SUCCESS, expect RUNNING and PAUSED + // (should keep ticking unless RESUMED branch succeeds) + paused_child->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + + // Set PAUSED child to SKIPPED, expect RUNNING and PAUSED + paused_child->changeStatus(BT::NodeStatus::SKIPPED); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + + // Call pause service again, expect RUNNING and PAUSED + future = pause_client_->async_call(req); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + CheckServiceResult(pause_client_, future, false); + + // Call resume service, change ON_RESUME child to FAILURE, expect FAILURE + future = resume_client_->async_call(req); + on_resume_child->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::FAILURE); + CheckServiceResult(resume_client_, future); + + // Halt the tree, expect RUNNING and RESUMED + tree_->haltTree(); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + + // Set resumed child to SUCCESS, expect SUCCESS and RESUMED + resumed_child->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + + // Call resume service again, expect RUNNING and RESUMED + future = resume_client_->async_call(req); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + CheckServiceResult(resume_client_, future, false); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/control/test_persistent_sequence.cpp b/nav2_behavior_tree/test/plugins/control/test_persistent_sequence.cpp new file mode 100644 index 00000000000..ebf6aa0d6b8 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/control/test_persistent_sequence.cpp @@ -0,0 +1,157 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "utils/test_behavior_tree_fixture.hpp" +#include "utils/test_dummy_tree_node.hpp" +#include "utils/get_node_from_tree.hpp" +#include "nav2_behavior_tree/plugins/control/persistent_sequence.hpp" + +class PersistentSequenceTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + static void SetUpTestCase() + { + factory_ = std::make_shared(); + config_ = new BT::NodeConfiguration(); + config_->blackboard = BT::Blackboard::create(); + config_->blackboard->set("seq_child_idx", 0); + + factory_->registerNodeType( + "PersistentSequence"); + + // Register dummy node for testing + factory_->registerNodeType("DummyNode"); + } + + static void TearDownTestCase() + { + if (config_) { + delete config_; + config_ = nullptr; + } + tree_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +BT::NodeConfiguration * PersistentSequenceTestFixture::config_ = nullptr; +std::shared_ptr +PersistentSequenceTestFixture::factory_ = nullptr; +std::shared_ptr PersistentSequenceTestFixture::tree_ = nullptr; + +TEST_F(PersistentSequenceTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + )"; + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + config_->blackboard->set("seq_child_idx", 0); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); +} + +TEST_F(PersistentSequenceTestFixture, test_behavior) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + + + )"; + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // get dummy nodes so we can change their status + auto child0 = + nav2_behavior_tree::get_node_from_tree(tree_, 0); + auto child1 = + nav2_behavior_tree::get_node_from_tree(tree_, 1); + auto child2 = + nav2_behavior_tree::get_node_from_tree(tree_, 2); + ASSERT_NE(child0, nullptr); + ASSERT_NE(child1, nullptr); + ASSERT_NE(child2, nullptr); + child0->changeStatus(BT::NodeStatus::RUNNING); + child1->changeStatus(BT::NodeStatus::RUNNING); + child2->changeStatus(BT::NodeStatus::RUNNING); + + // Set child index to 0 and child0 to RUNNING, expect RUNNING and idx = 0 + config_->blackboard->set("seq_child_idx", 0); + child0->changeStatus(BT::NodeStatus::RUNNING); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(config_->blackboard->get("seq_child_idx"), 0); + + // Set child0 to SUCCESS, expect SUCCESS and idx = 1 + child0->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(config_->blackboard->get("seq_child_idx"), 1); + + // Set child1 to FAILURE, expect FAILURE and idx = 0 + child1->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_EQ(config_->blackboard->get("seq_child_idx"), 0); + + // Set idx to 1, child0 to FAILURE, child1 to SKIPPED and child2 to RUNNING, + // expect RUNNING and idx = 2 + config_->blackboard->set("seq_child_idx", 1); + child0->changeStatus(BT::NodeStatus::FAILURE); + child1->changeStatus(BT::NodeStatus::SKIPPED); + child2->changeStatus(BT::NodeStatus::RUNNING); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(config_->blackboard->get("seq_child_idx"), 2); + + // Set child2 to SUCCESS, expect SUCCESS and idx = 0 + child2->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(config_->blackboard->get("seq_child_idx"), 0); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/utils/get_node_from_tree.hpp b/nav2_behavior_tree/test/utils/get_node_from_tree.hpp new file mode 100644 index 00000000000..5ea720872d4 --- /dev/null +++ b/nav2_behavior_tree/test/utils/get_node_from_tree.hpp @@ -0,0 +1,66 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILS__GET_NODE_FROM_TREE_HPP_ +#define UTILS__GET_NODE_FROM_TREE_HPP_ + +#include +#include + +#include "behaviortree_cpp/bt_factory.h" +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief Get node from tree by type and index, casted to NodeT type, const casted away + * Returns node if it was found, nullptr otherwise. + */ +template +NodeT * get_node_from_tree( + const std::shared_ptr tree, + const size_t index = 0) +{ + static rclcpp::Logger logger = rclcpp::get_logger("nav2_behavior_tree::get_node_from_tree"); + + std::vector nodes = tree->getNodesByPath("*"); + if (nodes.empty()) { + RCLCPP_ERROR(logger, "No nodes of given type found"); + return nullptr; + } + if (nodes.size() <= index) { + RCLCPP_ERROR(logger, "Out of bounds (found %zu < %zu nodes)", nodes.size(), index); + return nullptr; + } + + const NodeT * const_bt_node = + dynamic_cast(nodes[index]); + if (const_bt_node == nullptr) { + RCLCPP_ERROR(logger, "Failed to cast node to given type"); + return nullptr; + } + + NodeT * bt_node = const_cast(const_bt_node); + if (bt_node == nullptr) { + RCLCPP_ERROR(logger, "Failed to cast away const from node"); + return nullptr; + } + + return bt_node; +} + +} // namespace nav2_behavior_tree + +#endif // UTILS__GET_NODE_FROM_TREE_HPP_ diff --git a/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp b/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp index dcfa52927ac..b4a64d7c78f 100644 --- a/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp +++ b/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp @@ -18,6 +18,7 @@ #include #include +#include namespace nav2_behavior_tree { @@ -29,17 +30,20 @@ namespace nav2_behavior_tree class DummyNode : public BT::ActionNodeBase { public: - DummyNode() + DummyNode( + const std::string & /*xml_tag_name*/ = "dummy", + const BT::NodeConfiguration & /*conf*/ = BT::NodeConfiguration()) : BT::ActionNodeBase("dummy", {}) { } void changeStatus(BT::NodeStatus status) { - if (status == BT::NodeStatus::IDLE) { + requested_status = status; + if (requested_status == BT::NodeStatus::IDLE) { resetStatus(); } else { - setStatus(status); + setStatus(requested_status); } } @@ -50,12 +54,26 @@ class DummyNode : public BT::ActionNodeBase BT::NodeStatus tick() override { + if (requested_status == BT::NodeStatus::IDLE) { + resetStatus(); + } else { + setStatus(requested_status); + } return status(); } void halt() override { + resetStatus(); } + + static BT::PortsList providedPorts() + { + return {}; + } + +protected: + BT::NodeStatus requested_status = BT::NodeStatus::IDLE; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/test/utils/test_transform_handler.hpp b/nav2_behavior_tree/test/utils/test_transform_handler.hpp index 19cfcdfa222..cf272d132cd 100644 --- a/nav2_behavior_tree/test/utils/test_transform_handler.hpp +++ b/nav2_behavior_tree/test/utils/test_transform_handler.hpp @@ -29,9 +29,9 @@ #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_msgs/msg/tf_message.hpp" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/buffer.hpp" using namespace std::chrono_literals; // NOLINT using namespace std::chrono; // NOLINT diff --git a/nav2_behaviors/CMakeLists.txt b/nav2_behaviors/CMakeLists.txt index c67f9af628b..bd37a097c2f 100644 --- a/nav2_behaviors/CMakeLists.txt +++ b/nav2_behaviors/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_behaviors) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) diff --git a/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp b/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp index 7301cef3836..82b55c0786f 100644 --- a/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp +++ b/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp @@ -19,8 +19,8 @@ #include #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" #include "nav2_core/behavior.hpp" diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 70d0f30fa30..22db8701700 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -26,8 +26,8 @@ #include #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" #include "geometry_msgs/msg/twist.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/twist_publisher.hpp" diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index c3739a3461e..04f20f80782 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -11,6 +11,7 @@ ament_cmake nav2_common + backward_ros geometry_msgs nav2_core nav2_costmap_2d diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index 54ace7bac0a..0f4db4b3bf6 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -87,7 +87,7 @@ BehaviorServer::on_configure(const rclcpp_lifecycle::State & state) get_node_base_interface(), get_node_timers_interface()); tf_->setCreateTimerInterface(timer_interface); - transform_listener_ = std::make_shared(*tf_); + transform_listener_ = std::make_shared(*tf_, this, true); behavior_types_.resize(behavior_ids_.size()); if (!loadBehaviorPlugins()) { diff --git a/nav2_bringup/CMakeLists.txt b/nav2_bringup/CMakeLists.txt index 5537046d246..b0999ee4eb8 100644 --- a/nav2_bringup/CMakeLists.txt +++ b/nav2_bringup/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_bringup) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(nav2_common REQUIRED) find_package(navigation2 REQUIRED) diff --git a/nav2_bringup/launch/tb4_loopback_simulation_launch.py b/nav2_bringup/launch/tb4_loopback_simulation_launch.py index 715a6730c8c..e5c70e2c9ed 100644 --- a/nav2_bringup/launch/tb4_loopback_simulation_launch.py +++ b/nav2_bringup/launch/tb4_loopback_simulation_launch.py @@ -141,6 +141,8 @@ def generate_launch_description() -> LaunchDescription: 'autostart': autostart, 'use_composition': use_composition, 'use_respawn': use_respawn, + 'use_keepout_zones': 'False', # Keepout zones not used in loopback simulation + 'use_speed_zones': 'False', # Speed zones not used in loopback simulation 'use_localization': 'False', # Don't use SLAM, AMCL }.items(), ) @@ -158,8 +160,9 @@ def generate_launch_description() -> LaunchDescription: package='tf2_ros', executable='static_transform_publisher', arguments=[ - '0.0', '0.0', '0.0', '0', '0', '0', - 'base_footprint', 'base_link'] + '--x', '0.0', '--y', '0.0', '--z', '0.0', + '--roll', '0', '--pitch', '0', '--yaw', '0', + '--frame-id', 'base_footprint', '--child-frame-id', 'base_link'] ) configured_params = ParameterFile( diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index dc5ebc7bb19..d17f705cd1e 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -12,9 +12,11 @@ ament_cmake nav2_common + backward_ros navigation2 launch_ros + backward_ros diff_drive_controller joint_state_broadcaster launch_ros diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index 2fa895dd8b8..83fa44fab7f 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/nav2_bt_navigator/CMakeLists.txt @@ -3,6 +3,7 @@ project(nav2_bt_navigator) find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_behavior_tree REQUIRED) find_package(nav2_common REQUIRED) diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 8a25e5a2c8d..adea1aba20f 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -23,9 +23,9 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_util/odometry_utils.hpp" #include "rclcpp_action/rclcpp_action.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_core/behavior_tree_navigator.hpp" #include "pluginlib/class_loader.hpp" diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 825e8f9eec3..62e5b7307ea 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -11,6 +11,7 @@ nav2_common ament_index_cpp + backward_ros geometry_msgs nav2_behavior_tree nav2_core diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index a3e95776b72..95d3c7dcaa7 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -38,19 +38,6 @@ BtNavigator::BtNavigator(rclcpp::NodeOptions options) class_loader_("nav2_core", "nav2_core::NavigatorBase") { RCLCPP_INFO(get_logger(), "Creating"); - - declare_parameter_if_not_declared( - this, "plugin_lib_names", rclcpp::ParameterValue(std::vector{})); - declare_parameter_if_not_declared( - this, "transform_tolerance", rclcpp::ParameterValue(0.1)); - declare_parameter_if_not_declared( - this, "global_frame", rclcpp::ParameterValue(std::string("map"))); - declare_parameter_if_not_declared( - this, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link"))); - declare_parameter_if_not_declared( - this, "odom_topic", rclcpp::ParameterValue(std::string("odom"))); - declare_parameter_if_not_declared( - this, "filter_duration", rclcpp::ParameterValue(0.3)); } BtNavigator::~BtNavigator() @@ -67,19 +54,20 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & state) get_node_base_interface(), get_node_timers_interface()); tf_->setCreateTimerInterface(timer_interface); tf_->setUsingDedicatedThread(true); - tf_listener_ = std::make_shared(*tf_, this, false); + tf_listener_ = std::make_shared(*tf_, this, true); - global_frame_ = get_parameter("global_frame").as_string(); - robot_frame_ = get_parameter("robot_base_frame").as_string(); - transform_tolerance_ = get_parameter("transform_tolerance").as_double(); - odom_topic_ = get_parameter("odom_topic").as_string(); - filter_duration_ = get_parameter("filter_duration").as_double(); + global_frame_ = this->declare_or_get_parameter("global_frame", std::string("map")); + robot_frame_ = this->declare_or_get_parameter("robot_base_frame", std::string("base_link")); + transform_tolerance_ = this->declare_or_get_parameter("transform_tolerance", 0.1); + odom_topic_ = this->declare_or_get_parameter("odom_topic", std::string("odom")); + filter_duration_ = this->declare_or_get_parameter("filter_duration", 0.3); // Libraries to pull plugins (BT Nodes) from std::vector plugin_lib_names; plugin_lib_names = nav2_util::split(nav2::details::BT_BUILTIN_PLUGINS, ';'); - auto user_defined_plugins = get_parameter("plugin_lib_names").as_string_array(); + auto user_defined_plugins = + this->declare_or_get_parameter("plugin_lib_names", std::vector{}); // append user_defined_plugins to plugin_lib_names plugin_lib_names.insert( plugin_lib_names.end(), user_defined_plugins.begin(), @@ -106,10 +94,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & state) }; std::vector navigator_ids; - declare_parameter_if_not_declared( - node, "navigators", - rclcpp::ParameterValue(default_navigator_ids)); - get_parameter("navigators", navigator_ids); + navigator_ids = this->declare_or_get_parameter("navigators", default_navigator_ids); if (navigator_ids == default_navigator_ids) { for (size_t i = 0; i < default_navigator_ids.size(); ++i) { declare_parameter_if_not_declared( diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 4dfd57a3eef..1127c3ab2cd 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -30,39 +30,25 @@ NavigateThroughPosesNavigator::configure( start_time_ = rclcpp::Time(0); auto node = parent_node.lock(); - if (!node->has_parameter("goals_blackboard_id")) { - node->declare_parameter("goals_blackboard_id", std::string("goals")); - } - - goals_blackboard_id_ = node->get_parameter("goals_blackboard_id").as_string(); - - if (!node->has_parameter("path_blackboard_id")) { - node->declare_parameter("path_blackboard_id", std::string("path")); - } - - path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string(); - - if (!node->has_parameter("waypoint_statuses_blackboard_id")) { - node->declare_parameter("waypoint_statuses_blackboard_id", std::string("waypoint_statuses")); - } - + goals_blackboard_id_ = + node->declare_or_get_parameter("goals_blackboard_id", std::string("goals")); + path_blackboard_id_ = + node->declare_or_get_parameter("path_blackboard_id", std::string("path")); waypoint_statuses_blackboard_id_ = - node->get_parameter("waypoint_statuses_blackboard_id").as_string(); + node->declare_or_get_parameter("waypoint_statuses_blackboard_id", + std::string("waypoint_statuses")); // Odometry smoother object for getting current speed odom_smoother_ = odom_smoother; - if (!node->has_parameter(getName() + ".enable_groot_monitoring")) { - node->declare_parameter(getName() + ".enable_groot_monitoring", false); - } - - if (!node->has_parameter(getName() + ".groot_server_port")) { - node->declare_parameter(getName() + ".groot_server_port", 1669); - } + bool enable_groot_monitoring = + node->declare_or_get_parameter(getName() + ".enable_groot_monitoring", false); + int groot_server_port = + node->declare_or_get_parameter(getName() + ".groot_server_port", 1669); bt_action_server_->setGrootMonitoring( - node->get_parameter(getName() + ".enable_groot_monitoring").as_bool(), - node->get_parameter(getName() + ".groot_server_port").as_int()); + enable_groot_monitoring, + groot_server_port); return true; } @@ -71,19 +57,14 @@ std::string NavigateThroughPosesNavigator::getDefaultBTFilepath( nav2::LifecycleNode::WeakPtr parent_node) { - std::string default_bt_xml_filename; auto node = parent_node.lock(); + std::string pkg_share_dir = + ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - if (!node->has_parameter("default_nav_through_poses_bt_xml")) { - std::string pkg_share_dir = - ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - node->declare_parameter( - "default_nav_through_poses_bt_xml", - pkg_share_dir + - "/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml"); - } - - node->get_parameter("default_nav_through_poses_bt_xml", default_bt_xml_filename); + auto default_bt_xml_filename = node->declare_or_get_parameter( + "default_nav_through_poses_bt_xml", + pkg_share_dir + + "/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml"); return default_bt_xml_filename; } diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 5d2218000f7..03d77451e16 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -29,17 +29,10 @@ NavigateToPoseNavigator::configure( start_time_ = rclcpp::Time(0); auto node = parent_node.lock(); - if (!node->has_parameter("goal_blackboard_id")) { - node->declare_parameter("goal_blackboard_id", std::string("goal")); - } - - goal_blackboard_id_ = node->get_parameter("goal_blackboard_id").as_string(); - - if (!node->has_parameter("path_blackboard_id")) { - node->declare_parameter("path_blackboard_id", std::string("path")); - } - - path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string(); + goal_blackboard_id_ = + node->declare_or_get_parameter("goal_blackboard_id", std::string("goal")); + path_blackboard_id_ = + node->declare_or_get_parameter("path_blackboard_id", std::string("path")); // Odometry smoother object for getting current speed odom_smoother_ = odom_smoother; @@ -50,17 +43,14 @@ NavigateToPoseNavigator::configure( "goal_pose", std::bind(&NavigateToPoseNavigator::onGoalPoseReceived, this, std::placeholders::_1)); - if (!node->has_parameter(getName() + ".enable_groot_monitoring")) { - node->declare_parameter(getName() + ".enable_groot_monitoring", false); - } - - if (!node->has_parameter(getName() + ".groot_server_port")) { - node->declare_parameter(getName() + ".groot_server_port", 1667); - } + bool enable_groot_monitoring = + node->declare_or_get_parameter(getName() + ".enable_groot_monitoring", false); + int groot_server_port = + node->declare_or_get_parameter(getName() + ".groot_server_port", 1669); bt_action_server_->setGrootMonitoring( - node->get_parameter(getName() + ".enable_groot_monitoring").as_bool(), - node->get_parameter(getName() + ".groot_server_port").as_int()); + enable_groot_monitoring, + groot_server_port); return true; } @@ -69,19 +59,14 @@ std::string NavigateToPoseNavigator::getDefaultBTFilepath( nav2::LifecycleNode::WeakPtr parent_node) { - std::string default_bt_xml_filename; auto node = parent_node.lock(); + std::string pkg_share_dir = + ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - if (!node->has_parameter("default_nav_to_pose_bt_xml")) { - std::string pkg_share_dir = - ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - node->declare_parameter( - "default_nav_to_pose_bt_xml", - pkg_share_dir + - "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"); - } - - node->get_parameter("default_nav_to_pose_bt_xml", default_bt_xml_filename); + auto default_bt_xml_filename = node->declare_or_get_parameter( + "default_nav_to_pose_bt_xml", + pkg_share_dir + + "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"); return default_bt_xml_filename; } diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index a7c22e59a09..e67c8ea7719 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_collision_monitor) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_costmap_2d REQUIRED) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index 59b807f562e..450e97c8078 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -23,8 +23,8 @@ #include "rclcpp/rclcpp.hpp" #include "tf2/time.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_msgs/msg/collision_detector_state.hpp" diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index d73804cb827..c53540089d7 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -26,8 +26,8 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "tf2/time.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_util/twist_publisher.hpp" diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index 997031b825f..8a46d2b65c5 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -102,6 +102,8 @@ class PointCloud : public Source // Minimum and maximum height of PointCloud projected to 2D space double min_height_, max_height_; + // Minimum range from sensor origin to filter out close points + double min_range_; /// @brief Latest data obtained from pointcloud sensor_msgs::msg::PointCloud2::ConstSharedPtr data_; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 12d64259993..3191de1727e 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -24,7 +24,7 @@ #include "geometry_msgs/msg/polygon_stamped.hpp" #include "tf2/time.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_costmap_2d/footprint_subscriber.hpp" diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 312c13ea577..4e91dab13c7 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -22,7 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "tf2/time.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_ros_common/lifecycle_node.hpp" diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp index 55f894d87c5..8056973ddf9 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp @@ -24,7 +24,7 @@ #include "nav2_collision_monitor/types.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" namespace nav2_collision_monitor { diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 47ffe9a6d60..6a8e5ace75b 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -11,6 +11,7 @@ ament_cmake nav2_common + backward_ros geometry_msgs nav2_util nav2_costmap_2d diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 739d0d12207..91023afae4f 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -19,7 +19,7 @@ #include #include -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_ros_common/node_utils.hpp" @@ -50,7 +50,7 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & state) this->get_node_base_interface(), this->get_node_timers_interface()); tf_buffer_->setCreateTimerInterface(timer_interface); - tf_listener_ = std::make_shared(*tf_buffer_); + tf_listener_ = std::make_shared(*tf_buffer_, this, true); state_pub_ = this->create_publisher( "collision_detector_state"); diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index f85bdf4a866..d7ec6131220 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -18,7 +18,7 @@ #include #include -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -52,7 +52,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state) this->get_node_base_interface(), this->get_node_timers_interface()); tf_buffer_->setCreateTimerInterface(timer_interface); - tf_listener_ = std::make_shared(*tf_buffer_); + tf_listener_ = std::make_shared(*tf_buffer_, this, true); std::string cmd_vel_in_topic; std::string cmd_vel_out_topic; diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 998001508bb..590eacb3cb8 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -96,6 +96,13 @@ bool PointCloud::getData( for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { // Transform point coordinates from source frame -> to base frame tf2::Vector3 p_v3_s(*iter_x, *iter_y, *iter_z); + + // Check range from sensor origin before transformation + double range = p_v3_s.length(); + if (range < min_range_) { + continue; + } + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; // Refill data array @@ -121,6 +128,9 @@ void PointCloud::getParameters(std::string & source_topic) nav2::declare_parameter_if_not_declared( node, source_name_ + ".max_height", rclcpp::ParameterValue(0.5)); max_height_ = node->get_parameter(source_name_ + ".max_height").as_double(); + nav2::declare_parameter_if_not_declared( + node, source_name_ + ".min_range", rclcpp::ParameterValue(0.0)); + min_range_ = node->get_parameter(source_name_ + ".min_range").as_double(); } void PointCloud::dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) @@ -145,6 +155,8 @@ PointCloud::dynamicParametersCallback( min_height_ = parameter.as_double(); } else if (param_name == source_name_ + "." + "max_height") { max_height_ = parameter.as_double(); + } else if (param_name == source_name_ + "." + "min_range") { + min_range_ = parameter.as_double(); } } else if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { if (param_name == source_name_ + "." + "enabled") { diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 1bd2eb0ca0c..4bbd37bc4e9 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -34,7 +34,7 @@ #include "geometry_msgs/msg/polygon_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/collision_detector_node.hpp" diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index d64f0bd71e8..ea0213f468c 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -34,7 +34,7 @@ #include "geometry_msgs/msg/polygon_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/collision_monitor_node.hpp" diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index 52eff3c650b..86f393febfa 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -27,9 +27,9 @@ #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/polygon.hpp" diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index 7ed6a7aa561..034cdd40d96 100644 --- a/nav2_collision_monitor/test/sources_test.cpp +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -30,9 +30,9 @@ #include "sensor_msgs/msg/range.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/scan.hpp" @@ -142,6 +142,59 @@ class TestNode : public nav2::LifecycleNode pointcloud_pub_->publish(std::move(msg)); } + void publishPointCloudForMinRange(const rclcpp::Time & stamp) + { + pointcloud_pub_ = this->create_publisher( + POINTCLOUD_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + pointcloud_pub_->on_activate(); + + std::unique_ptr msg = + std::make_unique(); + sensor_msgs::PointCloud2Modifier modifier(*msg); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + modifier.setPointCloud2Fields( + 3, "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32); + modifier.resize(4); + + sensor_msgs::PointCloud2Iterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*msg, "z"); + + // Point 0: Very close to origin (0.05, 0.0, 0.15) - range ≈ 0.158, + // should be filtered + *iter_x = 0.05; + *iter_y = 0.0; + *iter_z = 0.15; + ++iter_x; ++iter_y; ++iter_z; + + // Point 1: Close to origin (0.15, 0.0, 0.15) - range ≈ 0.212, + // should pass with min_range=0.16 + *iter_x = 0.15; + *iter_y = 0.0; + *iter_z = 0.15; + ++iter_x; ++iter_y; ++iter_z; + + // Point 2: At min_range threshold (0.2, 0.0, 0.15) - range ≈ 0.25, + // should pass with min_range=0.16 + *iter_x = 0.2; + *iter_y = 0.0; + *iter_z = 0.15; + ++iter_x; ++iter_y; ++iter_z; + + // Point 3: Beyond min_range (0.5, 0.0, 0.15) - range ≈ 0.522, + // should pass + *iter_x = 0.5; + *iter_y = 0.0; + *iter_z = 0.15; + + pointcloud_pub_->publish(std::move(msg)); + } + void publishRange(const rclcpp::Time & stamp, const double range) { range_pub_ = this->create_publisher( @@ -301,6 +354,8 @@ class Tester : public ::testing::Test public: Tester(); ~Tester(); + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; protected: // Data sources creation routine @@ -324,10 +379,6 @@ class Tester : public ::testing::Test std::shared_ptr pointcloud_; std::shared_ptr range_; std::shared_ptr polygon_; - -private: - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; }; // Tester Tester::Tester() @@ -746,6 +797,62 @@ TEST_F(Tester, testIgnoreTimeShift) checkPolygon(data); } +TEST_F(Tester, testPointCloudMinRange) +{ + rclcpp::Time curr_time = test_node_->now(); + + // Create PointCloud object with min_range = 0.2 + test_node_->declare_parameter( + std::string(POINTCLOUD_NAME) + ".topic", rclcpp::ParameterValue(POINTCLOUD_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POINTCLOUD_NAME) + ".topic", POINTCLOUD_TOPIC)); + test_node_->declare_parameter( + std::string(POINTCLOUD_NAME) + ".min_height", rclcpp::ParameterValue(0.1)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POINTCLOUD_NAME) + ".min_height", 0.1)); + test_node_->declare_parameter( + std::string(POINTCLOUD_NAME) + ".max_height", rclcpp::ParameterValue(1.0)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POINTCLOUD_NAME) + ".max_height", 1.0)); + test_node_->declare_parameter( + std::string(POINTCLOUD_NAME) + ".min_range", rclcpp::ParameterValue(0.16)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POINTCLOUD_NAME) + ".min_range", 0.16)); + + pointcloud_ = std::make_shared( + test_node_, POINTCLOUD_NAME, tf_buffer_, + BASE_FRAME_ID, GLOBAL_FRAME_ID, + TRANSFORM_TOLERANCE, DATA_TIMEOUT, true); + pointcloud_->configure(); + + sendTransforms(curr_time); + + // Publish pointcloud data with points at various ranges + test_node_->publishPointCloudForMinRange(curr_time); + + // Wait until pointcloud receives the data + ASSERT_TRUE(waitPointCloud(500ms)); + + // Check PointCloud data - should only contain points with range >= min_range (0.16) + std::vector data; + pointcloud_->getData(curr_time, data); + + // Should have 3 points: first point (range ≈ 0.158) filtered out, remaining 3 pass + ASSERT_EQ(data.size(), 3u); + + // Point 1: (0.15 + 0.1, 0.0 + 0.1) = (0.25, 0.1) - range ≈ 0.212, passes + EXPECT_NEAR(data[0].x, 0.25, EPSILON); + EXPECT_NEAR(data[0].y, 0.1, EPSILON); + + // Point 2: (0.2 + 0.1, 0.0 + 0.1) = (0.3, 0.1) - range ≈ 0.25, passes + EXPECT_NEAR(data[1].x, 0.3, EPSILON); + EXPECT_NEAR(data[1].y, 0.1, EPSILON); + + // Point 3: (0.5 + 0.1, 0.0 + 0.1) = (0.6, 0.1) - range ≈ 0.522, passes + EXPECT_NEAR(data[2].x, 0.6, EPSILON); + EXPECT_NEAR(data[2].y, 0.1, EPSILON); +} + int main(int argc, char ** argv) { // Initialize the system diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index d13fc873340..88024dd723b 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -27,9 +27,9 @@ #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/polygon.hpp" diff --git a/nav2_common/CMakeLists.txt b/nav2_common/CMakeLists.txt index 8504b8b23de..f0f1e22c32c 100644 --- a/nav2_common/CMakeLists.txt +++ b/nav2_common/CMakeLists.txt @@ -4,6 +4,7 @@ project(nav2_common NONE) find_package(ament_cmake_core REQUIRED) find_package(ament_cmake_python REQUIRED) +find_package(backward_ros REQUIRED) ament_python_install_package(nav2_common) diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 7aa6a3925eb..12bbf129ccd 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -7,6 +7,7 @@ Carl Delsey Apache-2.0 + backward_ros launch launch_ros osrf_pycommon diff --git a/nav2_constrained_smoother/CMakeLists.txt b/nav2_constrained_smoother/CMakeLists.txt index 31e83e77fe5..c4613f52496 100644 --- a/nav2_constrained_smoother/CMakeLists.txt +++ b/nav2_constrained_smoother/CMakeLists.txt @@ -4,6 +4,7 @@ project(nav2_constrained_smoother) set(CMAKE_BUILD_TYPE Release) # significant Ceres optimization speedup find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(Ceres REQUIRED COMPONENTS SuiteSparse) find_package(Eigen3 REQUIRED) find_package(geometry_msgs REQUIRED) diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index 11fb47b1b06..eb5b60ae9b4 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -12,6 +12,7 @@ eigen nav2_common + backward_ros geometry_msgs libceres-dev nav2_core diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index b9285fa9240..6e4e3d4bc90 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -23,7 +23,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" #include "nav2_costmap_2d/footprint_collision_checker.hpp" diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index a100f96a214..93885ef7efc 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -3,6 +3,7 @@ project(nav2_controller) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(nav2_core REQUIRED) @@ -10,8 +11,6 @@ find_package(nav2_common REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) -find_package(nav_2d_msgs REQUIRED) -find_package(nav_2d_utils REQUIRED) find_package(pluginlib REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(rclcpp REQUIRED) @@ -41,9 +40,6 @@ target_link_libraries(${library_name} PUBLIC ${nav2_msgs_TARGETS} nav2_util::nav2_util_core nav2_ros_common::nav2_ros_common - ${nav_2d_msgs_TARGETS} - nav_2d_utils::conversions - nav_2d_utils::tf_help pluginlib::pluginlib rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle @@ -85,7 +81,6 @@ target_link_libraries(simple_progress_checker PUBLIC ) target_link_libraries(simple_progress_checker PRIVATE nav2_util::nav2_util_core - nav_2d_utils::conversions pluginlib::pluginlib ) @@ -106,7 +101,6 @@ target_link_libraries(pose_progress_checker PUBLIC target_link_libraries(pose_progress_checker PRIVATE angles::angles nav2_util::nav2_util_core - nav_2d_utils::conversions pluginlib::pluginlib ) @@ -217,8 +211,6 @@ ament_export_dependencies( nav2_costmap_2d nav2_msgs nav2_util - nav_2d_msgs - nav_2d_utils pluginlib rclcpp rclcpp_lifecycle diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index a112a3c9d66..b68c8e5bb94 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -26,13 +26,13 @@ #include "nav2_core/progress_checker.hpp" #include "nav2_core/goal_checker.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" #include "nav2_msgs/action/follow_path.hpp" #include "nav2_msgs/msg/speed_limit.hpp" -#include "nav_2d_utils/odom_subscriber.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_ros_common/simple_action_server.hpp" #include "nav2_util/robot_utils.hpp" +#include "nav2_util/odometry_utils.hpp" #include "nav2_util/twist_publisher.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" @@ -208,12 +208,12 @@ class ControllerServer : public nav2::LifecycleNode * @param Twist The current Twist from odometry * @return Twist Twist after thresholds applied */ - nav_2d_msgs::msg::Twist2D getThresholdedTwist(const nav_2d_msgs::msg::Twist2D & twist) + geometry_msgs::msg::Twist getThresholdedTwist(const geometry_msgs::msg::Twist & twist) { - nav_2d_msgs::msg::Twist2D twist_thresh; - twist_thresh.x = getThresholdedVelocity(twist.x, min_x_velocity_threshold_); - twist_thresh.y = getThresholdedVelocity(twist.y, min_y_velocity_threshold_); - twist_thresh.theta = getThresholdedVelocity(twist.theta, min_theta_velocity_threshold_); + geometry_msgs::msg::Twist twist_thresh; + twist_thresh.linear.x = getThresholdedVelocity(twist.linear.x, min_x_velocity_threshold_); + twist_thresh.linear.y = getThresholdedVelocity(twist.linear.y, min_y_velocity_threshold_); + twist_thresh.angular.z = getThresholdedVelocity(twist.angular.z, min_theta_velocity_threshold_); return twist_thresh; } @@ -233,7 +233,7 @@ class ControllerServer : public nav2::LifecycleNode std::unique_ptr costmap_thread_; // Publishers and subscribers - std::unique_ptr odom_sub_; + std::unique_ptr odom_sub_; std::unique_ptr vel_publisher_; nav2::Subscription::SharedPtr speed_limit_sub_; diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 16875164c0d..9360c10d05f 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -11,14 +11,13 @@ nav2_common angles + backward_ros geometry_msgs lifecycle_msgs nav2_core nav2_costmap_2d nav2_msgs nav2_util - nav_2d_msgs - nav_2d_utils pluginlib rcl_interfaces rclcpp diff --git a/nav2_controller/plugins/pose_progress_checker.cpp b/nav2_controller/plugins/pose_progress_checker.cpp index 453d20992e8..a891104cac0 100644 --- a/nav2_controller/plugins/pose_progress_checker.cpp +++ b/nav2_controller/plugins/pose_progress_checker.cpp @@ -18,7 +18,6 @@ #include #include #include "angles/angles.h" -#include "nav_2d_utils/conversions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" #include "nav2_ros_common/node_utils.hpp" diff --git a/nav2_controller/plugins/simple_progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp index 2e219cdeee8..df3cb77df69 100644 --- a/nav2_controller/plugins/simple_progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -17,7 +17,6 @@ #include #include #include -#include "nav_2d_utils/conversions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" #include "nav2_ros_common/node_utils.hpp" diff --git a/nav2_controller/plugins/test/CMakeLists.txt b/nav2_controller/plugins/test/CMakeLists.txt index cd8048513d4..ba0ce9eb2d5 100644 --- a/nav2_controller/plugins/test/CMakeLists.txt +++ b/nav2_controller/plugins/test/CMakeLists.txt @@ -1,5 +1,5 @@ ament_add_gtest(pctest progress_checker.cpp) -target_link_libraries(pctest simple_progress_checker pose_progress_checker nav_2d_utils::conversions) +target_link_libraries(pctest simple_progress_checker pose_progress_checker) ament_add_gtest(gctest goal_checker.cpp) -target_link_libraries(gctest simple_goal_checker stopped_goal_checker nav_2d_utils::conversions) +target_link_libraries(gctest simple_goal_checker stopped_goal_checker) diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index 4730aeb3a04..1aba1ba80bb 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -38,7 +38,6 @@ #include "gtest/gtest.h" #include "nav2_controller/plugins/simple_goal_checker.hpp" #include "nav2_controller/plugins/stopped_goal_checker.hpp" -#include "nav_2d_utils/conversions.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "eigen3/Eigen/Geometry" diff --git a/nav2_controller/plugins/test/progress_checker.cpp b/nav2_controller/plugins/test/progress_checker.cpp index 0791b9e9c27..293daf13a25 100644 --- a/nav2_controller/plugins/test/progress_checker.cpp +++ b/nav2_controller/plugins/test/progress_checker.cpp @@ -38,7 +38,6 @@ #include "gtest/gtest.h" #include "nav2_controller/plugins/simple_progress_checker.hpp" #include "nav2_controller/plugins/pose_progress_checker.hpp" -#include "nav_2d_utils/conversions.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_util/geometry_utils.hpp" diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 26187087ec1..66b62e667e2 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -21,8 +21,6 @@ #include "lifecycle_msgs/msg/state.hpp" #include "nav2_core/controller_exceptions.hpp" -#include "nav_2d_utils/conversions.hpp" -#include "nav_2d_utils/tf_help.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_controller/controller_server.hpp" @@ -65,6 +63,9 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) declare_parameter("publish_zero_velocity", rclcpp::ParameterValue(true)); declare_parameter("costmap_update_timeout", 0.30); // 300ms + declare_parameter("odom_topic", rclcpp::ParameterValue("odom")); + declare_parameter("odom_duration", rclcpp::ParameterValue(0.3)); + // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( "local_costmap", std::string{get_namespace()}, @@ -125,8 +126,11 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_); RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_); - std::string speed_limit_topic; + std::string speed_limit_topic, odom_topic; get_parameter("speed_limit_topic", speed_limit_topic); + get_parameter("odom_topic", odom_topic); + double odom_duration; + get_parameter("odom_duration", odom_duration); get_parameter("failure_tolerance", failure_tolerance_); get_parameter("use_realtime_priority", use_realtime_priority_); get_parameter("publish_zero_velocity", publish_zero_velocity_); @@ -219,7 +223,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) get_logger(), "Controller Server has %s controllers available.", controller_ids_concat_.c_str()); - odom_sub_ = std::make_unique(node); + odom_sub_ = std::make_unique(node, odom_duration, odom_topic); vel_publisher_ = std::make_unique(node, "cmd_vel"); double costmap_update_timeout_dbl; @@ -624,7 +628,7 @@ void ControllerServer::computeAndPublishVelocity() throw nav2_core::FailedToMakeProgress("Failed to make progress"); } - nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); + geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist()); geometry_msgs::msg::TwistStamped cmd_vel_2d; @@ -632,7 +636,7 @@ void ControllerServer::computeAndPublishVelocity() cmd_vel_2d = controllers_[current_controller_]->computeVelocityCommands( pose, - nav_2d_utils::twist2Dto3D(twist), + twist, goal_checkers_[current_goal_checker_].get()); last_valid_cmd_time_ = now(); cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID(); @@ -665,10 +669,9 @@ void ControllerServer::computeAndPublishVelocity() // Find the closest pose to current pose on global path geometry_msgs::msg::PoseStamped robot_pose_in_path_frame; - rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance())); - if (!nav_2d_utils::transformPose( - costmap_ros_->getTfBuffer(), current_path_.header.frame_id, pose, - robot_pose_in_path_frame, tolerance)) + if (!nav2_util::transformPoseInTargetFrame( + pose, robot_pose_in_path_frame, *costmap_ros_->getTfBuffer(), + current_path_.header.frame_id, costmap_ros_->getTransformTolerance())) { throw nav2_core::ControllerTFError("Failed to transform robot pose to path frame"); } @@ -793,14 +796,12 @@ bool ControllerServer::isGoalReached() return false; } - nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); - geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(twist); + geometry_msgs::msg::Twist velocity = getThresholdedTwist(odom_sub_->getRawTwist()); geometry_msgs::msg::PoseStamped transformed_end_pose; - rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance())); - nav_2d_utils::transformPose( - costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(), - end_pose_, transformed_end_pose, tolerance); + nav2_util::transformPoseInTargetFrame( + end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(), + costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance()); return goal_checkers_[current_goal_checker_]->isGoalReached( pose.pose, transformed_end_pose.pose, diff --git a/nav2_core/CMakeLists.txt b/nav2_core/CMakeLists.txt index 1d0e6b724f5..586410c3488 100644 --- a/nav2_core/CMakeLists.txt +++ b/nav2_core/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_core) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_behavior_tree REQUIRED) find_package(nav2_common REQUIRED) diff --git a/nav2_core/include/nav2_core/behavior.hpp b/nav2_core/include/nav2_core/behavior.hpp index b2e00e8cf53..7a58d3aa2a0 100644 --- a/nav2_core/include/nav2_core/behavior.hpp +++ b/nav2_core/include/nav2_core/behavior.hpp @@ -20,7 +20,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_costmap_2d/costmap_topic_collision_checker.hpp" namespace nav2_core diff --git a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp index 87824c7fd31..fda64cec974 100644 --- a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp +++ b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp @@ -21,7 +21,7 @@ #include #include "nav2_util/odometry_utils.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "rclcpp/rclcpp.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "pluginlib/class_loader.hpp" diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index 261eec22436..d24e84d6680 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -41,7 +41,7 @@ #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" #include "pluginlib/class_loader.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" diff --git a/nav2_core/include/nav2_core/global_planner.hpp b/nav2_core/include/nav2_core/global_planner.hpp index bcf65da6d4f..fa63d2e519e 100644 --- a/nav2_core/include/nav2_core/global_planner.hpp +++ b/nav2_core/include/nav2_core/global_planner.hpp @@ -18,7 +18,7 @@ #include #include #include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_ros_common/lifecycle_node.hpp" diff --git a/nav2_core/include/nav2_core/smoother.hpp b/nav2_core/include/nav2_core/smoother.hpp index 7705106177d..deb4a6107ec 100644 --- a/nav2_core/include/nav2_core/smoother.hpp +++ b/nav2_core/include/nav2_core/smoother.hpp @@ -20,8 +20,8 @@ #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_costmap_2d/footprint_subscriber.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "pluginlib/class_loader.hpp" #include "nav_msgs/msg/path.hpp" diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 044eb215676..aa95ad053fd 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -11,6 +11,7 @@ ament_cmake nav2_common + backward_ros geometry_msgs nav2_behavior_tree nav2_costmap_2d diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 221c5595515..851f2428b42 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -3,6 +3,7 @@ project(nav2_costmap_2d) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(laser_geometry REQUIRED) find_package(map_msgs REQUIRED) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 0bcda597046..5b1fa01591b 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -43,8 +43,8 @@ #include #include -#include "geometry_msgs/msg/polygon.h" -#include "geometry_msgs/msg/polygon_stamped.h" +#include "geometry_msgs/msg/polygon.hpp" +#include "geometry_msgs/msg/polygon_stamped.hpp" #include "nav2_costmap_2d/costmap_2d_publisher.hpp" #include "nav2_costmap_2d/footprint.hpp" #include "nav2_costmap_2d/footprint_collision_checker.hpp" @@ -56,8 +56,8 @@ #include "pluginlib/class_loader.hpp" #include "tf2/convert.hpp" #include "tf2/LinearMath/Transform.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "tf2/time.hpp" #include "tf2/transform_datatypes.hpp" #include "nav2_ros_common/service_server.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index 2136db7c856..571df68c9d3 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp @@ -41,7 +41,7 @@ #include #include -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp index c54d586b729..09f0e79c114 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp @@ -43,7 +43,7 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "rclcpp/time.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "tf2_sensor_msgs/tf2_sensor_msgs.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "nav2_costmap_2d/observation.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp index 16113d61b16..5e66e24795a 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -47,7 +47,7 @@ #include "laser_geometry/laser_geometry.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wreorder" -#include "tf2_ros/message_filter.h" +#include "tf2_ros/message_filter.hpp" #pragma GCC diagnostic pop #include "message_filters/subscriber.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp index 2e00a412637..8a9aeb29623 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp @@ -27,7 +27,7 @@ #include "nav2_costmap_2d/costmap_layer.hpp" #include "nav2_costmap_2d/observation_buffer.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" -#include "tf2_ros/message_filter.h" +#include "tf2_ros/message_filter.hpp" #include "pluginlib/class_loader.hpp" using nav2_costmap_2d::LETHAL_OBSTACLE; 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 7da443ebd25..0333f03577b 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 c01e7bb2dd6..c69d467b908 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -20,6 +20,7 @@ nav2_common angles + backward_ros geometry_msgs laser_geometry map_msgs diff --git a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp index 9c8bd890afb..097537d40c6 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp @@ -97,8 +97,8 @@ void BinaryFilter::initializeFilter( base_ = BASE_DEFAULT; multiplier_ = MULTIPLIER_DEFAULT; - // Initialize state as "false" by-default - changeState(default_state_); + // Initialize state binary_state_ which at start its equal to default_state_ + changeState(binary_state_); } void BinaryFilter::filterInfoCallback( @@ -229,8 +229,11 @@ void BinaryFilter::resetFilter() { std::lock_guard guard(*getMutex()); - RCLCPP_INFO(logger_, "BinaryFilter: Resetting the filter to default state"); - changeState(default_state_); + // Publishing new BinaryState ib reset + std::unique_ptr msg = + std::make_unique(); + msg->data = binary_state_; + binary_state_pub_->publish(std::move(msg)); filter_info_sub_.reset(); mask_sub_.reset(); diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index e4dbe277723..94aa69f87f8 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -47,6 +47,8 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_ros_common/validate_messages.hpp" +#define EPSILON 1e-5 + PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer) using nav2_costmap_2d::NO_INFORMATION; @@ -190,9 +192,9 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) Costmap2D * master = layered_costmap_->getCostmap(); 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) @@ -206,9 +208,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( @@ -469,6 +471,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 33bc0fbe7e6..278ef40e7a5 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_); @@ -193,6 +195,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; @@ -515,7 +522,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_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 49dd1885fea..f43373d286b 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -48,7 +48,7 @@ #include "nav2_util/execution_timer.hpp" #include "nav2_ros_common/node_utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_util/robot_utils.hpp" #include "rcl_interfaces/msg/set_parameters_result.hpp" diff --git a/nav2_costmap_2d/test/integration/costmap_tester.cpp b/nav2_costmap_2d/test/integration/costmap_tester.cpp index c7d44e6e70c..8569cae54e3 100644 --- a/nav2_costmap_2d/test/integration/costmap_tester.cpp +++ b/nav2_costmap_2d/test/integration/costmap_tester.cpp @@ -38,7 +38,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/cost_values.hpp" diff --git a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp index a543dda02ae..94e0f72ea77 100644 --- a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp +++ b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp @@ -19,7 +19,7 @@ #include "rclcpp/rclcpp.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" class DynParamTestNode { diff --git a/nav2_costmap_2d/test/integration/footprint_tests.cpp b/nav2_costmap_2d/test/integration/footprint_tests.cpp index 2f527b5faac..0bde175ce0a 100644 --- a/nav2_costmap_2d/test/integration/footprint_tests.cpp +++ b/nav2_costmap_2d/test/integration/footprint_tests.cpp @@ -41,7 +41,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" #include "nav2_costmap_2d/footprint.hpp" class FootprintTestNode diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index c0616176647..a365cdc9a7d 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -19,7 +19,7 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_costmap_2d/cost_values.hpp" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" class CostmapRosLifecycleNode : public nav2::LifecycleNode diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index 1543060ccca..e5b73e4b5cc 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -29,10 +29,10 @@ #include "nav2_util/robot_utils.hpp" #include "nav2_ros_common/node_utils.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/transform_broadcaster.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" #include "tf2/utils.hpp" diff --git a/nav2_costmap_2d/test/unit/binary_filter_test.cpp b/nav2_costmap_2d/test/unit/binary_filter_test.cpp index bd85faa903d..e0061e91e88 100644 --- a/nav2_costmap_2d/test/unit/binary_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/binary_filter_test.cpp @@ -24,9 +24,9 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/occ_grid_values.hpp" #include "nav2_costmap_2d/cost_values.hpp" @@ -236,6 +236,7 @@ class TestNode : public ::testing::Test std::shared_ptr master_grid_; bool default_state_; + bool binary_state_; private: void waitSome(const std::chrono::nanoseconds & duration); @@ -703,12 +704,13 @@ void TestNode::testResetFilter() binary_state = waitBinaryState(); verifyBinaryState(getSign(pose.position.x, pose.position.y, base, multiplier, flip_threshold), binary_state); + binary_state_ = binary_state->data; // Reset binary filter and check its state was reset to default binary_filter_->resetFilter(); binary_state = waitBinaryState(); ASSERT_TRUE(binary_state != nullptr); - ASSERT_EQ(binary_state->data, default_state_); + ASSERT_EQ(binary_state->data, binary_state_); } diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp index c75acf2b7dc..37a5a683299 100644 --- a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp @@ -22,9 +22,9 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_util/occ_grid_values.hpp" #include "nav2_costmap_2d/cost_values.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp index 76e96187924..5d3eebb6db5 100644 --- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp @@ -24,9 +24,9 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_util/occ_grid_values.hpp" #include "nav2_costmap_2d/cost_values.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt index 83680e3eeae..a68bd4677ab 100644 --- a/nav2_docking/opennav_docking/CMakeLists.txt +++ b/nav2_docking/opennav_docking/CMakeLists.txt @@ -3,6 +3,7 @@ project(opennav_docking) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_graceful_controller REQUIRED) diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp index 06c6a7252e0..4538b2e447d 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp @@ -27,14 +27,14 @@ #include "nav2_ros_common/node_utils.hpp" #include "nav2_ros_common/simple_action_server.hpp" #include "nav2_util/twist_publisher.hpp" -#include "nav_2d_utils/odom_subscriber.hpp" +#include "nav2_util/odometry_utils.hpp" #include "opennav_docking/controller.hpp" #include "opennav_docking/utils.hpp" #include "opennav_docking/types.hpp" #include "opennav_docking/dock_database.hpp" #include "opennav_docking/navigator.hpp" #include "opennav_docking_core/charging_dock.hpp" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" namespace opennav_docking { @@ -264,7 +264,7 @@ class DockingServer : public nav2::LifecycleNode rclcpp::Time action_start_time_; std::unique_ptr vel_publisher_; - std::unique_ptr odom_sub_; + std::unique_ptr odom_sub_; typename DockingActionServer::SharedPtr docking_action_server_; typename UndockingActionServer::SharedPtr undocking_action_server_; diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml index ecfa8082379..b65cb91ee56 100644 --- a/nav2_docking/opennav_docking/package.xml +++ b/nav2_docking/opennav_docking/package.xml @@ -11,6 +11,7 @@ nav2_common angles + backward_ros geometry_msgs nav2_graceful_controller nav2_msgs diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index 9048c70a29a..7838158f8af 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/nav2_docking/opennav_docking/src/controller.cpp @@ -19,7 +19,6 @@ #include "opennav_docking/controller.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_ros_common/node_utils.hpp" -#include "nav_2d_utils/conversions.hpp" #include "tf2/utils.hpp" namespace opennav_docking diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 3584af4e9d3..1208ade5521 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -42,6 +42,7 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options) declare_parameter("dock_backwards", rclcpp::PARAMETER_BOOL); declare_parameter("dock_prestaging_tolerance", 0.5); declare_parameter("odom_topic", "odom"); + declare_parameter("odom_duration", 0.3); declare_parameter("rotation_angular_tolerance", 0.05); } @@ -83,7 +84,9 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state) // Create odom subscriber for backward blind docking std::string odom_topic; get_parameter("odom_topic", odom_topic); - odom_sub_ = std::make_unique(node, odom_topic); + double odom_duration; + get_parameter("odom_duration", odom_duration); + odom_sub_ = std::make_unique(node, odom_duration, odom_topic); // Create the action servers for dock / undock docking_action_server_ = node->create_action_server( @@ -118,7 +121,7 @@ DockingServer::on_activate(const rclcpp_lifecycle::State & /*state*/) auto node = shared_from_this(); - tf2_listener_ = std::make_unique(*tf2_buffer_); + tf2_listener_ = std::make_unique(*tf2_buffer_, this, true); dock_db_->activate(); navigator_->activate(); vel_publisher_->on_activate(); @@ -464,7 +467,7 @@ void DockingServer::rotateToDock(const geometry_msgs::msg::PoseStamped & dock_po } auto current_vel = std::make_unique(); - current_vel->twist.angular.z = odom_sub_->getTwist().theta; + current_vel->twist.angular.z = odom_sub_->getRawTwist().angular.z; auto command = std::make_unique(); command->header = robot_pose.header; diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp index f0e982a919b..8708b31428d 100644 --- a/nav2_docking/opennav_docking/test/test_controller.cpp +++ b/nav2_docking/opennav_docking/test/test_controller.cpp @@ -21,7 +21,7 @@ #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_ros_common/node_utils.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" // Testing the controller at high level; the nav2_graceful_controller diff --git a/nav2_docking/opennav_docking_bt/CMakeLists.txt b/nav2_docking/opennav_docking_bt/CMakeLists.txt index 015265a3753..05e67fb713f 100644 --- a/nav2_docking/opennav_docking_bt/CMakeLists.txt +++ b/nav2_docking/opennav_docking_bt/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(opennav_docking_bt) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_behavior_tree REQUIRED) diff --git a/nav2_docking/opennav_docking_bt/package.xml b/nav2_docking/opennav_docking_bt/package.xml index 3b946c8c296..9fc0d3c0692 100644 --- a/nav2_docking/opennav_docking_bt/package.xml +++ b/nav2_docking/opennav_docking_bt/package.xml @@ -10,6 +10,7 @@ ament_cmake nav2_common + backward_ros behaviortree_cpp geometry_msgs nav2_behavior_tree diff --git a/nav2_docking/opennav_docking_core/CMakeLists.txt b/nav2_docking/opennav_docking_core/CMakeLists.txt index 9575861645a..057bebb0b6c 100644 --- a/nav2_docking/opennav_docking_core/CMakeLists.txt +++ b/nav2_docking/opennav_docking_core/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(opennav_docking_core) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(rclcpp_lifecycle REQUIRED) diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp index a923cc98d26..67e599405d7 100644 --- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp @@ -20,7 +20,7 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" namespace opennav_docking_core diff --git a/nav2_docking/opennav_docking_core/package.xml b/nav2_docking/opennav_docking_core/package.xml index 735f89cf24d..285bf274cce 100644 --- a/nav2_docking/opennav_docking_core/package.xml +++ b/nav2_docking/opennav_docking_core/package.xml @@ -10,6 +10,7 @@ ament_cmake nav2_common + backward_ros geometry_msgs rclcpp_lifecycle tf2_ros diff --git a/nav2_dwb_controller/costmap_queue/CMakeLists.txt b/nav2_dwb_controller/costmap_queue/CMakeLists.txt index 28d47ed3dd5..68dbfa64ef9 100644 --- a/nav2_dwb_controller/costmap_queue/CMakeLists.txt +++ b/nav2_dwb_controller/costmap_queue/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(costmap_queue) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_costmap_2d REQUIRED) diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 695cebd19b7..a74b57e69a6 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -3,12 +3,13 @@ costmap_queue 1.4.0 The costmap_queue package - David V. Lu!! + David V. Lu!! BSD-3-Clause ament_cmake nav2_common + backward_ros nav2_costmap_2d ament_cmake_gtest diff --git a/nav2_dwb_controller/dwb_core/CMakeLists.txt b/nav2_dwb_controller/dwb_core/CMakeLists.txt index ba787fb831c..00789a46e3d 100644 --- a/nav2_dwb_controller/dwb_core/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_core/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(dwb_core) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(dwb_msgs REQUIRED) find_package(geometry_msgs REQUIRED) @@ -44,7 +45,6 @@ target_link_libraries(dwb_core PUBLIC nav2_util::nav2_util_core ${nav_2d_msgs_TARGETS} nav_2d_utils::conversions - nav_2d_utils::tf_help ${nav_msgs_TARGETS} pluginlib::pluginlib rclcpp::rclcpp diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index 41120a7c48a..63013bd091e 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -199,7 +199,7 @@ class DWBLocalPlanner : public nav2_core::Controller bool prune_plan_; double prune_distance_; bool debug_trajectory_details_; - rclcpp::Duration transform_tolerance_{0, 0}; + double transform_tolerance_{0}; bool shorten_transformed_plan_; double forward_prune_distance_; diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index ce879804bbc..a76dd8c474c 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -10,6 +10,7 @@ ament_cmake nav2_common + backward_ros builtin_interfaces dwb_msgs geometry_msgs diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index e539c98819a..5c1b47322a6 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -44,7 +44,6 @@ #include "dwb_msgs/msg/critic_score.hpp" #include "nav_2d_msgs/msg/twist2_d.hpp" #include "nav_2d_utils/conversions.hpp" -#include "nav_2d_utils/tf_help.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_ros_common/node_utils.hpp" @@ -112,8 +111,7 @@ void DWBLocalPlanner::configure( std::string traj_generator_name; double transform_tolerance; - node->get_parameter(dwb_plugin_name_ + ".transform_tolerance", transform_tolerance); - transform_tolerance_ = rclcpp::Duration::from_seconds(transform_tolerance); + node->get_parameter(dwb_plugin_name_ + ".transform_tolerance", transform_tolerance_); RCLCPP_INFO(logger_, "Setting transform_tolerance to %f", transform_tolerance); node->get_parameter(dwb_plugin_name_ + ".prune_plan", prune_plan_); @@ -293,9 +291,9 @@ DWBLocalPlanner::prepareGlobalPlan( goal_pose.header.frame_id = global_plan_.header.frame_id; goal_pose.pose = global_plan_.poses.back().pose; - nav_2d_utils::transformPose( - tf_, costmap_ros_->getGlobalFrameID(), goal_pose, - goal_pose, transform_tolerance_); + nav2_util::transformPoseInTargetFrame( + goal_pose, goal_pose, *tf_, + costmap_ros_->getGlobalFrameID(), transform_tolerance_); } nav_2d_msgs::msg::Twist2DStamped @@ -470,9 +468,9 @@ DWBLocalPlanner::transformGlobalPlan( // let's get the pose of the robot in the frame of the plan geometry_msgs::msg::PoseStamped robot_pose; - if (!nav_2d_utils::transformPose( - tf_, global_plan_.header.frame_id, pose, - robot_pose, transform_tolerance_)) + if (!nav2_util::transformPoseInTargetFrame( + pose, robot_pose, *tf_, + global_plan_.header.frame_id, transform_tolerance_)) { throw nav2_core:: ControllerTFError("Unable to transform robot pose into global plan's frame"); @@ -538,9 +536,9 @@ DWBLocalPlanner::transformGlobalPlan( // frame to local auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { geometry_msgs::msg::PoseStamped transformed_pose; - nav_2d_utils::transformPose( - tf_, transformed_plan.header.frame_id, - global_plan_pose, transformed_pose, transform_tolerance_); + nav2_util::transformPoseInTargetFrame( + global_plan_pose, transformed_pose, *tf_, + transformed_plan.header.frame_id, transform_tolerance_); return transformed_pose; }; diff --git a/nav2_dwb_controller/dwb_critics/CMakeLists.txt b/nav2_dwb_controller/dwb_critics/CMakeLists.txt index b1cb464cd5c..ddda0a9087b 100644 --- a/nav2_dwb_controller/dwb_critics/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_critics/CMakeLists.txt @@ -3,6 +3,7 @@ project(dwb_critics) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(costmap_queue REQUIRED) find_package(dwb_core REQUIRED) find_package(dwb_msgs REQUIRED) diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index f6c22047aa6..eb923ca051e 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -3,12 +3,13 @@ dwb_critics 1.4.0 The dwb_critics package - David V. Lu!! + David V. Lu!! BSD-3-Clause ament_cmake nav2_common + backward_ros angles costmap_queue dwb_core diff --git a/nav2_dwb_controller/dwb_critics/src/goal_align.cpp b/nav2_dwb_controller/dwb_critics/src/goal_align.cpp index 1ae85c2e1dd..f8b713c032f 100644 --- a/nav2_dwb_controller/dwb_critics/src/goal_align.cpp +++ b/nav2_dwb_controller/dwb_critics/src/goal_align.cpp @@ -37,7 +37,6 @@ #include #include "dwb_critics/alignment_util.hpp" #include "pluginlib/class_list_macros.hpp" -#include "nav_2d_utils/parameters.hpp" namespace dwb_critics { @@ -52,9 +51,8 @@ void GoalAlignCritic::onInit() throw std::runtime_error{"Failed to lock node"}; } - forward_point_distance_ = nav_2d_utils::searchAndGetParam( - node, - dwb_plugin_name_ + "." + name_ + ".forward_point_distance", 0.325); + forward_point_distance_ = node->declare_or_get_parameter(dwb_plugin_name_ + "." + name_ + + ".forward_point_distance", 0.325); } bool GoalAlignCritic::prepare( diff --git a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp index bad8e40e3ff..170a9764c1f 100644 --- a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp +++ b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp @@ -37,7 +37,6 @@ #include #include #include -#include "nav_2d_utils/parameters.hpp" #include "nav2_ros_common/node_utils.hpp" #include "dwb_core/exceptions.hpp" #include "pluginlib/class_list_macros.hpp" @@ -99,16 +98,13 @@ void OscillationCritic::onInit() clock_ = node->get_clock(); - oscillation_reset_dist_ = nav_2d_utils::searchAndGetParam( - node, - dwb_plugin_name_ + "." + name_ + ".oscillation_reset_dist", 0.05); + oscillation_reset_dist_ = node->declare_or_get_parameter(dwb_plugin_name_ + "." + name_ + + ".oscillation_reset_dist", 0.05); oscillation_reset_dist_sq_ = oscillation_reset_dist_ * oscillation_reset_dist_; - oscillation_reset_angle_ = nav_2d_utils::searchAndGetParam( - node, + oscillation_reset_angle_ = node->declare_or_get_parameter( dwb_plugin_name_ + "." + name_ + ".oscillation_reset_angle", 0.2); oscillation_reset_time_ = rclcpp::Duration::from_seconds( - nav_2d_utils::searchAndGetParam( - node, + node->declare_or_get_parameter( dwb_plugin_name_ + "." + name_ + ".oscillation_reset_time", -1.0)); nav2::declare_parameter_if_not_declared( diff --git a/nav2_dwb_controller/dwb_critics/src/path_align.cpp b/nav2_dwb_controller/dwb_critics/src/path_align.cpp index 6c4b28ea6f5..d91414586dd 100644 --- a/nav2_dwb_controller/dwb_critics/src/path_align.cpp +++ b/nav2_dwb_controller/dwb_critics/src/path_align.cpp @@ -37,7 +37,6 @@ #include #include "dwb_critics/alignment_util.hpp" #include "pluginlib/class_list_macros.hpp" -#include "nav_2d_utils/parameters.hpp" namespace dwb_critics { @@ -52,8 +51,7 @@ void PathAlignCritic::onInit() throw std::runtime_error{"Failed to lock node"}; } - forward_point_distance_ = nav_2d_utils::searchAndGetParam( - node, + forward_point_distance_ = node->declare_or_get_parameter( dwb_plugin_name_ + "." + name_ + ".forward_point_distance", 0.325); } diff --git a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp index f4323188b18..34f2ae95335 100644 --- a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp +++ b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp @@ -35,7 +35,6 @@ #include "dwb_critics/rotate_to_goal.hpp" #include #include -#include "nav_2d_utils/parameters.hpp" #include "dwb_core/exceptions.hpp" #include "pluginlib/class_list_macros.hpp" #include "dwb_core/trajectory_utils.hpp" @@ -58,19 +57,15 @@ void RotateToGoalCritic::onInit() throw std::runtime_error{"Failed to lock node"}; } - xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam( - node, + xy_goal_tolerance_ = node->declare_or_get_parameter( dwb_plugin_name_ + ".xy_goal_tolerance", 0.25); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; - double stopped_xy_velocity = nav_2d_utils::searchAndGetParam( - node, + double stopped_xy_velocity = node->declare_or_get_parameter( dwb_plugin_name_ + ".trans_stopped_velocity", 0.25); stopped_xy_velocity_sq_ = stopped_xy_velocity * stopped_xy_velocity; - slowing_factor_ = nav_2d_utils::searchAndGetParam( - node, + slowing_factor_ = node->declare_or_get_parameter( dwb_plugin_name_ + "." + name_ + ".slowing_factor", 5.0); - lookahead_time_ = nav_2d_utils::searchAndGetParam( - node, + lookahead_time_ = node->declare_or_get_parameter( dwb_plugin_name_ + "." + name_ + ".lookahead_time", -1.0); reset(); } diff --git a/nav2_dwb_controller/dwb_msgs/CMakeLists.txt b/nav2_dwb_controller/dwb_msgs/CMakeLists.txt index 2f34fcdf5ea..574a48c4c6a 100644 --- a/nav2_dwb_controller/dwb_msgs/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_msgs/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required(VERSION 3.5) project(dwb_msgs) +find_package(backward_ros REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_2d_msgs REQUIRED) diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 594c7619a41..9d1b3f47e6b 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -4,11 +4,12 @@ dwb_msgs 1.4.0 Message/Service definitions specifically for the dwb_core - David V. Lu!! + David V. Lu!! BSD-3-Clause ament_cmake + backward_ros builtin_interfaces geometry_msgs nav_2d_msgs diff --git a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt index f725aa2cf94..7d4228ae427 100644 --- a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(dwb_plugins) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(dwb_core REQUIRED) find_package(dwb_msgs REQUIRED) find_package(geometry_msgs REQUIRED) diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index d848c166a1f..f25f724d95a 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -6,12 +6,13 @@ Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core - David V. Lu!! + David V. Lu!! BSD-3-Clause ament_cmake nav2_common + backward_ros dwb_core dwb_msgs geometry_msgs diff --git a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp index dde49133011..c1152bc3f5b 100644 --- a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp @@ -38,7 +38,6 @@ #include #include -#include "nav_2d_utils/parameters.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" diff --git a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp index 24d1c255973..4c096f7e1f1 100644 --- a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp @@ -36,7 +36,6 @@ #include #include #include -#include "nav_2d_utils/parameters.hpp" #include "pluginlib/class_list_macros.hpp" #include "dwb_core/exceptions.hpp" #include "nav2_ros_common/node_utils.hpp" @@ -65,8 +64,7 @@ void LimitedAccelGenerator::initialize( RCLCPP_WARN( rclcpp::get_logger("LimitedAccelGenerator"), "'sim_period' parameter is not set for %s", plugin_name.c_str()); - double controller_frequency = nav_2d_utils::searchAndGetParam( - nh, "controller_frequency", 20.0); + double controller_frequency = nh->declare_or_get_parameter("controller_frequency", 20.0); if (controller_frequency > 0) { acceleration_time_ = 1.0 / controller_frequency; } else { diff --git a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp index 8e68a399f08..a11dd8ece0b 100644 --- a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp @@ -38,7 +38,6 @@ #include #include #include "dwb_plugins/xy_theta_iterator.hpp" -#include "nav_2d_utils/parameters.hpp" #include "pluginlib/class_list_macros.hpp" #include "dwb_core/exceptions.hpp" #include "nav2_ros_common/node_utils.hpp" diff --git a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp index c319bef4a72..01dab35cb7b 100644 --- a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp @@ -38,7 +38,6 @@ #include #include -#include "nav_2d_utils/parameters.hpp" #include "nav2_ros_common/node_utils.hpp" namespace dwb_plugins diff --git a/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt b/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt index e900fd292ca..5c1c8fce07a 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt +++ b/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_dwb_controller) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(nav2_common REQUIRED) nav2_package() diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 7d8363bcfc2..8eea7edacb8 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -11,8 +11,10 @@ Apache-2.0 ament_cmake + backward_ros nav2_common + backward_ros costmap_queue dwb_core dwb_critics diff --git a/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt b/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt index c2838963c9b..155c13cdd65 100644 --- a/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt +++ b/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required(VERSION 3.5) project(nav_2d_msgs) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 2f422cc95da..bd343580140 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -4,13 +4,14 @@ nav_2d_msgs 1.4.0 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose. - David V. Lu!! + David V. Lu!! BSD-3-Clause rosidl_default_runtime ament_cmake + backward_ros geometry_msgs std_msgs rosidl_default_generators diff --git a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt index 8a729af46a9..7aad657a142 100644 --- a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt +++ b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav_2d_utils) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_2d_msgs REQUIRED) find_package(nav_msgs REQUIRED) @@ -55,28 +56,7 @@ target_link_libraries(path_ops PRIVATE ${geometry_msgs_TARGETS} ) -add_library(tf_help SHARED - src/tf_help.cpp -) -target_include_directories(tf_help - PUBLIC - "$" - "$" -) -target_link_libraries(tf_help PUBLIC - conversions - ${geometry_msgs_TARGETS} - ${nav_2d_msgs_TARGETS} - rclcpp::rclcpp - nav2_ros_common::nav2_ros_common - tf2::tf2 - tf2_geometry_msgs::tf2_geometry_msgs - tf2_ros::tf2_ros - rclcpp_lifecycle::rclcpp_lifecycle - rcl_lifecycle::rcl_lifecycle -) - -install(TARGETS conversions path_ops tf_help +install(TARGETS conversions path_ops EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -100,7 +80,7 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include/${PROJECT_NAME}) -ament_export_libraries(conversions path_ops tf_help) +ament_export_libraries(conversions path_ops) ament_export_dependencies( geometry_msgs nav_2d_msgs diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp index 35f3ec2018f..3a30fd03050 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp +++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp @@ -49,10 +49,6 @@ namespace nav_2d_utils { geometry_msgs::msg::Twist twist2Dto3D(const nav_2d_msgs::msg::Twist2D & cmd_vel_2d); nav_2d_msgs::msg::Twist2D twist3Dto2D(const geometry_msgs::msg::Twist & cmd_vel); -geometry_msgs::msg::PoseStamped poseToPoseStamped( - const geometry_msgs::msg::Pose & pose, - const std::string & frame, const rclcpp::Time & stamp); -nav_msgs::msg::Path posesToPath(const std::vector & poses); nav_msgs::msg::Path posesToPath( const std::vector & poses, const std::string & frame, const rclcpp::Time & stamp); diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp deleted file mode 100644 index 2f989e71f2f..00000000000 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp +++ /dev/null @@ -1,100 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_ -#define NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_ - -#include -#include -#include -#include -#include "nav_2d_msgs/msg/twist2_d_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "nav2_ros_common/lifecycle_node.hpp" -#include "rclcpp/rclcpp.hpp" -#include "nav2_ros_common/node_utils.hpp" - -namespace nav_2d_utils -{ - -/** - * @class OdomSubscriber - * Wrapper for some common odometry operations. Subscribes to the topic with a mutex. - */ -class OdomSubscriber -{ -public: - /** - * @brief Constructor that subscribes to an Odometry topic - * - * @param nh NodeHandle for creating subscriber - * @param default_topic Name of the topic that will be loaded of the odom_topic param is not set. - */ - explicit OdomSubscriber( - nav2::LifecycleNode::SharedPtr nh, - std::string default_topic = "odom") - { - nav2::declare_parameter_if_not_declared( - nh, "odom_topic", rclcpp::ParameterValue(default_topic)); - - std::string odom_topic; - nh->get_parameter_or("odom_topic", odom_topic, default_topic); - odom_sub_ = - nh->create_subscription( - odom_topic, - std::bind(&OdomSubscriber::odomCallback, this, std::placeholders::_1)); - } - - inline nav_2d_msgs::msg::Twist2D getTwist() {return odom_vel_.velocity;} - inline nav_2d_msgs::msg::Twist2DStamped getTwistStamped() {return odom_vel_;} - -protected: - void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) - { - // ROS_INFO_ONCE("odom received!"); - std::lock_guard lock(odom_mutex_); - odom_vel_.header = msg->header; - odom_vel_.velocity.x = msg->twist.twist.linear.x; - odom_vel_.velocity.y = msg->twist.twist.linear.y; - odom_vel_.velocity.theta = msg->twist.twist.angular.z; - } - - nav2::Subscription::SharedPtr odom_sub_; - nav_2d_msgs::msg::Twist2DStamped odom_vel_; - std::mutex odom_mutex_; -}; - -} // namespace nav_2d_utils - -#endif // NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_ diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp deleted file mode 100644 index 09b039487e0..00000000000 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp +++ /dev/null @@ -1,76 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef NAV_2D_UTILS__PARAMETERS_HPP_ -#define NAV_2D_UTILS__PARAMETERS_HPP_ - -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "nav2_ros_common/lifecycle_node.hpp" -#include "nav2_ros_common/node_utils.hpp" - -// TODO(crdelsey): Remove when code is re-enabled -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-parameter" -namespace nav_2d_utils -{ - -/** - * @brief Search for a parameter and load it, or use the default value - * - * This templated function shortens a commonly used ROS pattern in which you - * search for a parameter and get its value if it exists, otherwise returning a default value. - * - * @param nh NodeHandle to start the parameter search from - * @param param_name Name of the parameter to search for - * @param default_value Value to return if not found - * @return Value of parameter if found, otherwise the default_value - */ -template -param_t searchAndGetParam( - const nav2::LifecycleNode::SharedPtr & nh, const std::string & param_name, - const param_t & default_value) -{ - nav2::declare_parameter_if_not_declared( - nh, param_name, - rclcpp::ParameterValue(default_value)); - return nh->get_parameter(param_name).get_value(); -} - -} // namespace nav_2d_utils -#pragma GCC diagnostic pop - -#endif // NAV_2D_UTILS__PARAMETERS_HPP_ diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp deleted file mode 100644 index 9bed6cd6810..00000000000 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp +++ /dev/null @@ -1,85 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef NAV_2D_UTILS__TF_HELP_HPP_ -#define NAV_2D_UTILS__TF_HELP_HPP_ - -#include -#include -#include "tf2_ros/buffer.h" -#include "nav_2d_utils/conversions.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - -namespace nav_2d_utils -{ -/** - * @brief Transform a PoseStamped from one frame to another while catching exceptions - * - * Also returns immediately if the frames are equal. - * @param tf Smart pointer to TFListener - * @param frame Frame to transform the pose into - * @param in_pose Pose to transform - * @param out_pose Place to store the resulting transformed pose - * @return True if successful transform - */ -bool transformPose( - const std::shared_ptr tf, - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose, - rclcpp::Duration & transform_tolerance -); - -/** - * @brief Transform a PoseStamped from one frame to another while catching exceptions - * - * Also returns immediately if the frames are equal. - * @param tf Smart pointer to TFListener - * @param frame Frame to transform the pose into - * @param in_pose Pose to transform - * @param out_pose Place to store the resulting transformed pose - * @return True if successful transform - */ -bool transformPose( - const std::shared_ptr tf, - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose, - rclcpp::Duration & transform_tolerance -); - -} // namespace nav_2d_utils - -#endif // NAV_2D_UTILS__TF_HELP_HPP_ diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index 060a6c2f701..9968469eab4 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -4,12 +4,13 @@ nav_2d_utils 1.4.0 A handful of useful utility functions for nav_2d packages. - David V. Lu!! + David V. Lu!! BSD-3-Clause ament_cmake nav2_common + backward_ros geometry_msgs nav_2d_msgs nav_msgs diff --git a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp index 4ec9a73fb99..3629e8bb989 100644 --- a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp +++ b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp @@ -72,34 +72,6 @@ nav_2d_msgs::msg::Twist2D twist3Dto2D(const geometry_msgs::msg::Twist & cmd_vel) return cmd_vel_2d; } - -geometry_msgs::msg::PoseStamped poseToPoseStamped( - const geometry_msgs::msg::Pose & pose_in, - const std::string & frame, const rclcpp::Time & stamp) -{ - geometry_msgs::msg::PoseStamped pose; - pose.header.frame_id = frame; - pose.header.stamp = stamp; - pose.pose = pose_in; - return pose; -} - -nav_msgs::msg::Path posesToPath(const std::vector & poses) -{ - nav_msgs::msg::Path path; - if (poses.empty()) { - return path; - } - path.poses.resize(poses.size()); - path.header.frame_id = poses[0].header.frame_id; - path.header.stamp = poses[0].header.stamp; - for (unsigned int i = 0; i < poses.size(); i++) { - path.poses[i] = poses[i]; - } - return path; -} - - nav_msgs::msg::Path posesToPath( const std::vector & poses, const std::string & frame, const rclcpp::Time & stamp) diff --git a/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp b/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp deleted file mode 100644 index 0916cc6a402..00000000000 --- a/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp +++ /dev/null @@ -1,98 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include "nav_2d_utils/tf_help.hpp" - -namespace nav_2d_utils -{ - -bool transformPose( - const std::shared_ptr tf, - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose, - rclcpp::Duration & transform_tolerance -) -{ - if (in_pose.header.frame_id == frame) { - out_pose = in_pose; - return true; - } - - try { - tf->transform(in_pose, out_pose, frame); - return true; - } catch (tf2::ExtrapolationException & ex) { - auto transform = tf->lookupTransform( - frame, - in_pose.header.frame_id, - tf2::TimePointZero - ); - if ( - (rclcpp::Time(in_pose.header.stamp) - rclcpp::Time(transform.header.stamp)) > - transform_tolerance) - { - RCLCPP_ERROR( - rclcpp::get_logger("tf_help"), - "Transform data too old when converting from %s to %s", - in_pose.header.frame_id.c_str(), - frame.c_str() - ); - RCLCPP_ERROR( - rclcpp::get_logger("tf_help"), - "Data time: %ds %uns, Transform time: %ds %uns", - in_pose.header.stamp.sec, - in_pose.header.stamp.nanosec, - transform.header.stamp.sec, - transform.header.stamp.nanosec - ); - return false; - } else { - tf2::doTransform(in_pose, out_pose, transform); - return true; - } - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - rclcpp::get_logger("tf_help"), - "Exception in transformPose: %s", - ex.what() - ); - return false; - } - return false; -} - -} // namespace nav_2d_utils diff --git a/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp b/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp index c7f5c502f76..fd7d209d3d7 100644 --- a/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp +++ b/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp @@ -48,59 +48,55 @@ using nav_2d_utils::posesToPath; TEST(nav_2d_utils, PosesToPathEmpty) { - std::vector poses; - nav_msgs::msg::Path path = posesToPath(poses); + std::vector poses; + auto node = std::make_shared("twod_utils_test_node"); + rclcpp::Time time = node->now(); + std::string frame = "test_frame"; + + nav_msgs::msg::Path path = posesToPath(poses, frame, time); EXPECT_EQ(path.poses.size(), 0ul); + EXPECT_EQ(path.header.frame_id, frame); + EXPECT_EQ(path.header.stamp, time); } TEST(nav_2d_utils, PosesToPathNonEmpty) { - std::vector poses; - geometry_msgs::msg::PoseStamped pose1; - rclcpp::Time time1, time2; auto node = std::make_shared("twod_utils_test_node"); - time1 = node->now(); + rclcpp::Time time = node->now(); + std::string frame = "map"; tf2::Quaternion quat1, quat2; quat1.setRPY(0, 0, 0.123); - pose1.pose.position.x = 1.0; - pose1.pose.position.y = 2.0; - pose1.pose.orientation.w = quat1.w(); - pose1.pose.orientation.x = quat1.x(); - pose1.pose.orientation.y = quat1.y(); - pose1.pose.orientation.z = quat1.z(); - pose1.header.stamp = time1; - pose1.header.frame_id = "frame1_id"; - - geometry_msgs::msg::PoseStamped pose2; - pose2.pose.position.x = 4.0; - pose2.pose.position.y = 5.0; quat2.setRPY(0, 0, 0.987); - pose2.pose.orientation.w = quat2.w(); - pose2.pose.orientation.x = quat2.x(); - pose2.pose.orientation.y = quat2.y(); - pose2.pose.orientation.z = quat2.z(); - time2 = node->now(); - pose2.header.stamp = time2; - pose2.header.frame_id = "frame2_id"; + geometry_msgs::msg::Pose pose1; + pose1.position.x = 1.0; + pose1.position.y = 2.0; + pose1.orientation = tf2::toMsg(quat1); - poses.push_back(pose1); - poses.push_back(pose2); + geometry_msgs::msg::Pose pose2; + pose2.position.x = 4.0; + pose2.position.y = 5.0; + pose2.orientation = tf2::toMsg(quat2); - nav_msgs::msg::Path path = posesToPath(poses); + std::vector poses = {pose1, pose2}; + + nav_msgs::msg::Path path = posesToPath(poses, frame, time); EXPECT_EQ(path.poses.size(), 2ul); EXPECT_EQ(path.poses[0].pose.position.x, 1.0); EXPECT_EQ(path.poses[0].pose.position.y, 2.0); - EXPECT_EQ(path.poses[0].header.stamp, time1); - EXPECT_EQ(path.poses[0].header.frame_id, "frame1_id"); EXPECT_EQ(path.poses[1].pose.position.x, 4.0); EXPECT_EQ(path.poses[1].pose.position.y, 5.0); - EXPECT_EQ(path.poses[1].header.frame_id, "frame2_id"); - EXPECT_EQ(path.header.stamp, time1); + for (const auto & stamped_pose : path.poses) { + EXPECT_EQ(stamped_pose.header.frame_id, frame); + EXPECT_EQ(stamped_pose.header.stamp, time); + } + + EXPECT_EQ(path.header.frame_id, frame); + EXPECT_EQ(path.header.stamp, time); } diff --git a/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt index f0cf339a322..fd861291fca 100644 --- a/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt +++ b/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt @@ -11,6 +11,3 @@ target_include_directories(2d_utils_tests ament_add_gtest(path_ops_tests path_ops_test.cpp) target_link_libraries(path_ops_tests path_ops) - -ament_add_gtest(tf_help_tests tf_help_test.cpp) -target_link_libraries(tf_help_tests tf_help conversions) diff --git a/nav2_dwb_controller/nav_2d_utils/test/tf_help_test.cpp b/nav2_dwb_controller/nav_2d_utils/test/tf_help_test.cpp deleted file mode 100644 index 4095567473a..00000000000 --- a/nav2_dwb_controller/nav_2d_utils/test/tf_help_test.cpp +++ /dev/null @@ -1,98 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, Wilco Bonestroo - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include - -#include "gtest/gtest.h" -#include "nav_2d_utils/tf_help.hpp" - -TEST(TF_Help, TransformToSelf) { - bool result; - - std::shared_ptr tf; - std::string frame = "frame_id"; - geometry_msgs::msg::PoseStamped in_pose; - in_pose.header.frame_id = "frame_id"; - in_pose.pose.position.x = 1.0; - in_pose.pose.position.y = 2.0; - in_pose.pose.position.z = 3.0; - tf2::Quaternion qt; - qt.setRPY(0.5, 1.0, 1.5); - in_pose.pose.orientation.w = qt.w(); - in_pose.pose.orientation.x = qt.x(); - in_pose.pose.orientation.y = qt.y(); - in_pose.pose.orientation.z = qt.z(); - - geometry_msgs::msg::PoseStamped out_pose; - rclcpp::Duration transform_tolerance(0, 500); - - result = nav_2d_utils::transformPose(tf, frame, in_pose, out_pose, transform_tolerance); - - EXPECT_TRUE(result); - EXPECT_EQ(out_pose.header.frame_id, "frame_id"); - EXPECT_EQ(out_pose.pose.position.x, 1.0); - EXPECT_EQ(out_pose.pose.position.y, 2.0); - EXPECT_EQ(out_pose.pose.position.z, 3.0); - EXPECT_EQ(out_pose.pose.orientation.w, qt.w()); - EXPECT_EQ(out_pose.pose.orientation.x, qt.x()); - EXPECT_EQ(out_pose.pose.orientation.y, qt.y()); - EXPECT_EQ(out_pose.pose.orientation.z, qt.z()); -} - -TEST(TF_Help, EmptyBuffer) { - auto clock = std::make_shared(RCL_ROS_TIME); - auto buffer = std::make_shared(clock); - - std::string frame = "frame_id"; - geometry_msgs::msg::PoseStamped in_pose; - in_pose.header.frame_id = "other_frame_id"; - in_pose.pose.position.x = 1.0; - in_pose.pose.position.y = 2.0; - in_pose.pose.position.z = 3.0; - tf2::Quaternion qt; - qt.setRPY(0.5, 1.0, 1.5); - in_pose.pose.orientation.w = qt.w(); - in_pose.pose.orientation.x = qt.x(); - in_pose.pose.orientation.y = qt.y(); - in_pose.pose.orientation.z = qt.z(); - - geometry_msgs::msg::PoseStamped out_pose; - rclcpp::Duration transform_tolerance(0, 500); - - bool result; - result = nav_2d_utils::transformPose(buffer, frame, in_pose, out_pose, transform_tolerance); - - EXPECT_FALSE(result); -} diff --git a/nav2_graceful_controller/CMakeLists.txt b/nav2_graceful_controller/CMakeLists.txt index 1a72a698085..d9720ca6401 100644 --- a/nav2_graceful_controller/CMakeLists.txt +++ b/nav2_graceful_controller/CMakeLists.txt @@ -3,12 +3,12 @@ project(nav2_graceful_controller) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(nav2_util REQUIRED) -find_package(nav_2d_utils REQUIRED) find_package(nav_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) @@ -57,7 +57,6 @@ target_link_libraries(${library_name} PUBLIC nav2_ros_common::nav2_ros_common nav2_util::nav2_util_core nav2_costmap_2d::nav2_costmap_2d_core - nav_2d_utils::tf_help ${nav_msgs_TARGETS} pluginlib::pluginlib rclcpp::rclcpp @@ -103,7 +102,6 @@ ament_export_dependencies( nav2_core nav2_util nav2_costmap_2d - nav_2d_utils nav_msgs pluginlib rclcpp diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp index aaa64f1bb62..a24a84c2228 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp @@ -35,7 +35,7 @@ class PathHandler * @brief Constructor for nav2_graceful_controller::PathHandler */ PathHandler( - tf2::Duration transform_tolerance, + double transform_tolerance, std::shared_ptr tf, std::shared_ptr costmap_ros); @@ -71,7 +71,7 @@ class PathHandler nav_msgs::msg::Path getPlan() {return global_plan_;} protected: - rclcpp::Duration transform_tolerance_{0, 0}; + double transform_tolerance_{0}; std::shared_ptr tf_buffer_; std::shared_ptr costmap_ros_; nav_msgs::msg::Path global_plan_; diff --git a/nav2_graceful_controller/package.xml b/nav2_graceful_controller/package.xml index a195e466bec..98b6e1e257a 100644 --- a/nav2_graceful_controller/package.xml +++ b/nav2_graceful_controller/package.xml @@ -11,11 +11,11 @@ nav2_common angles + backward_ros geometry_msgs nav2_core nav2_costmap_2d nav2_util - nav_2d_utils nav_msgs pluginlib rclcpp diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index fb0010698f4..0706c86378d 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -48,7 +48,7 @@ void GracefulController::configure( // Handles global path transformations path_handler_ = std::make_unique( - tf2::durationFromSec(params_->transform_tolerance), tf_buffer_, costmap_ros_); + params_->transform_tolerance, tf_buffer_, costmap_ros_); // Handles the control law to generate the velocity commands control_law_ = std::make_unique( diff --git a/nav2_graceful_controller/src/path_handler.cpp b/nav2_graceful_controller/src/path_handler.cpp index 9dbfc291f8d..b60b4a874f0 100644 --- a/nav2_graceful_controller/src/path_handler.cpp +++ b/nav2_graceful_controller/src/path_handler.cpp @@ -22,7 +22,7 @@ #include "nav2_core/controller_exceptions.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" -#include "nav_2d_utils/tf_help.hpp" +#include "nav2_util/robot_utils.hpp" #include "nav2_graceful_controller/path_handler.hpp" namespace nav2_graceful_controller @@ -31,7 +31,7 @@ namespace nav2_graceful_controller using nav2_util::geometry_utils::euclidean_distance; PathHandler::PathHandler( - tf2::Duration transform_tolerance, + double transform_tolerance, std::shared_ptr tf, std::shared_ptr costmap_ros) : transform_tolerance_(transform_tolerance), tf_buffer_(tf), costmap_ros_(costmap_ros) @@ -49,8 +49,8 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( // Let's get the pose of the robot in the frame of the plan geometry_msgs::msg::PoseStamped robot_pose; - if (!nav_2d_utils::transformPose( - tf_buffer_, global_plan_.header.frame_id, pose, robot_pose, + if (!nav2_util::transformPoseInTargetFrame( + pose, robot_pose, *tf_buffer_, global_plan_.header.frame_id, transform_tolerance_)) { throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); @@ -88,9 +88,9 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( stamped_pose.header.frame_id = global_plan_.header.frame_id; stamped_pose.header.stamp = robot_pose.header.stamp; stamped_pose.pose = global_plan_pose.pose; - if (!nav_2d_utils::transformPose( - tf_buffer_, costmap_ros_->getBaseFrameID(), stamped_pose, - transformed_pose, transform_tolerance_)) + if (!nav2_util::transformPoseInTargetFrame( + stamped_pose, transformed_pose, *tf_buffer_, + costmap_ros_->getBaseFrameID(), transform_tolerance_)) { throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); } diff --git a/nav2_lifecycle_manager/CMakeLists.txt b/nav2_lifecycle_manager/CMakeLists.txt index 654b7153d06..64b1f4ad88d 100644 --- a/nav2_lifecycle_manager/CMakeLists.txt +++ b/nav2_lifecycle_manager/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_lifecycle_manager) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(bondcpp REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(geometry_msgs REQUIRED) diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 99b0b2f9250..0b6790935fc 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -10,6 +10,7 @@ ament_cmake nav2_common + backward_ros bondcpp diagnostic_updater geometry_msgs diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index d5948da2079..ea901a20290 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -314,12 +314,12 @@ def getMap(self) -> None: self.map = result.map self.get_logger().info('Laser scan will be populated using map data') else: - self.get_logger().warn( + self.get_logger().warning( 'Failed to get map, ' 'Laser scan will be populated using max range' ) else: - self.get_logger().warn( + self.get_logger().warning( 'Failed to get map, ' 'Laser scan will be populated using max range' ) diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index 438e1aa12fb..55160b2f7de 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -4,6 +4,7 @@ project(nav2_map_server) list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(GRAPHICSMAGICKCPP REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(nav_msgs REQUIRED) 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 f6d5f75a278..ffb3d3d1fce 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -14,6 +14,7 @@ nav2_common + backward_ros rclcpp_lifecycle nav_msgs std_msgs 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/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 5197a8859de..9dabc27507f 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -3,6 +3,7 @@ project(nav2_mppi_controller) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp index c59f28e2fad..afe8e83eea9 100644 --- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -28,7 +28,7 @@ #include #include #include -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_mppi_controller/optimizer.hpp" #include "nav2_mppi_controller/motion_models.hpp" diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp index b5d88008ab1..b3f7b03faab 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp @@ -21,7 +21,7 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/path.hpp" diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 5eba18b9d5d..e70cfcab786 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -26,7 +26,7 @@ #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_core/goal_checker.hpp" #include "nav2_core/controller_exceptions.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "pluginlib/class_loader.hpp" #include "geometry_msgs/msg/twist.hpp" diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp index cfa87f5ace4..4233c91ef28 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp @@ -23,7 +23,7 @@ #include #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/path.hpp" #include "builtin_interfaces/msg/time.hpp" @@ -98,17 +98,6 @@ class PathHandler geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp); protected: - /** - * @brief Transform a pose to another frame - * @param frame Frame to transform to - * @param in_pose Input pose - * @param out_pose Output pose - * @return Bool if successful - */ - bool transformPose( - const std::string & frame, const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose) const; - /** * @brief Get largest dimension of costmap (radially) * @return Max distance from center of costmap to edge diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index adde8cf93dc..a27716ab997 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -12,6 +12,7 @@ ament_cmake_ros angles + backward_ros geometry_msgs libomp-dev nav2_common diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 93593982c5c..55931f6bc62 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -17,6 +17,7 @@ #include "nav2_mppi_controller/tools/path_handler.hpp" #include "nav2_mppi_controller/tools/utils.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/robot_utils.hpp" namespace mppi { @@ -85,7 +86,8 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( geometry_msgs::msg::PoseStamped costmap_plan_pose; global_plan_pose->header.stamp = global_pose.header.stamp; global_plan_pose->header.frame_id = global_plan_.header.frame_id; - transformPose(costmap_->getGlobalFrameID(), *global_plan_pose, costmap_plan_pose); + nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_buffer_, + costmap_->getGlobalFrameID(), transform_tolerance_); // Check if pose is inside the costmap if (!costmap_->getCostmap()->worldToMap( @@ -109,7 +111,9 @@ geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame( } geometry_msgs::msg::PoseStamped robot_pose; - if (!transformPose(global_plan_up_to_inversion_.header.frame_id, pose, robot_pose)) { + if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_buffer_, + global_plan_up_to_inversion_.header.frame_id, transform_tolerance_)) + { throw nav2_core::ControllerTFError( "Unable to transform robot pose into global plan's frame"); } @@ -142,27 +146,6 @@ nav_msgs::msg::Path PathHandler::transformPath( return transformed_plan; } -bool PathHandler::transformPose( - const std::string & frame, const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose) const -{ - if (in_pose.header.frame_id == frame) { - out_pose = in_pose; - return true; - } - - try { - tf_buffer_->transform( - in_pose, out_pose, frame, - tf2::durationFromSec(transform_tolerance_)); - out_pose.header.frame_id = frame; - return true; - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what()); - } - return false; -} - double PathHandler::getMaxCostmapDist() { const auto & costmap = costmap_->getCostmap(); @@ -196,7 +179,9 @@ geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal( throw nav2_core::ControllerTFError("Goal pose has an empty frame_id"); } geometry_msgs::msg::PoseStamped transformed_goal; - if (!transformPose(costmap_->getGlobalFrameID(), goal, transformed_goal)) { + if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *tf_buffer_, + costmap_->getGlobalFrameID(), transform_tolerance_)) + { throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame"); } return transformed_goal; diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index d201e20eeb8..733229b77df 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -21,7 +21,7 @@ #include #include #include -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_mppi_controller/optimizer.hpp" #include "nav2_mppi_controller/tools/parameters_handler.hpp" diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index 510d9ef300b..eec9ea0ec57 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -18,7 +18,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "nav2_mppi_controller/optimizer.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" // Tests main optimizer functions diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp index 375978e98da..e0f17e48411 100644 --- a/nav2_mppi_controller/test/path_handler_test.cpp +++ b/nav2_mppi_controller/test/path_handler_test.cpp @@ -18,7 +18,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "nav2_mppi_controller/tools/path_handler.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" // Tests path handling @@ -46,13 +46,6 @@ class PathHandlerWrapper : public PathHandler return getGlobalPlanConsideringBoundsInCostmapFrame(pose); } - bool transformPoseWrapper( - const std::string & frame, const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose) const - { - return transformPose(frame, in_pose, out_pose); - } - geometry_msgs::msg::PoseStamped transformToGlobalPlanFrameWrapper( const geometry_msgs::msg::PoseStamped & pose) { @@ -180,12 +173,10 @@ TEST(PathHandlerTests, TestTransforms) path.poses[i].header.frame_id = "map"; } - geometry_msgs::msg::PoseStamped robot_pose, output_pose; + geometry_msgs::msg::PoseStamped robot_pose; robot_pose.header.frame_id = "odom"; robot_pose.pose.position.x = 2.5; - EXPECT_TRUE(handler.transformPoseWrapper("map", robot_pose, output_pose)); - EXPECT_EQ(output_pose.pose.position.x, 2.5); EXPECT_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose), std::runtime_error); handler.setPath(path); diff --git a/nav2_mppi_controller/test/utils/utils.hpp b/nav2_mppi_controller/test/utils/utils.hpp index b0185ce7cfe..2772ea9a858 100644 --- a/nav2_mppi_controller/test/utils/utils.hpp +++ b/nav2_mppi_controller/test/utils/utils.hpp @@ -22,7 +22,7 @@ #include -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 6252ab32e4e..f01fbdba6a5 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_msgs) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(nav2_common REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(nav_msgs REQUIRED) diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 12a085fbf88..61e84addc37 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -12,6 +12,7 @@ ament_cmake nav2_common + backward_ros rclcpp std_msgs builtin_interfaces diff --git a/nav2_navfn_planner/CMakeLists.txt b/nav2_navfn_planner/CMakeLists.txt index df0a924a40a..0da443e5e35 100644 --- a/nav2_navfn_planner/CMakeLists.txt +++ b/nav2_navfn_planner/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_navfn_planner) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index e00bcefaf29..fe4c11b373a 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -12,6 +12,7 @@ ament_cmake nav2_common + backward_ros builtin_interfaces geometry_msgs nav2_costmap_2d diff --git a/nav2_planner/CMakeLists.txt b/nav2_planner/CMakeLists.txt index 9c2e44e69b2..d3037ddf839 100644 --- a/nav2_planner/CMakeLists.txt +++ b/nav2_planner/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_planner) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(nav2_common REQUIRED) diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index c9fc2a46dd6..372f4c7c2f9 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -32,8 +32,8 @@ #include "nav2_util/robot_utils.hpp" #include "nav2_ros_common/simple_action_server.hpp" #include "nav2_ros_common/service_server.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index b0cdf1c8846..50ad1add559 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -10,6 +10,7 @@ ament_cmake nav2_common + backward_ros geometry_msgs lifecycle_msgs nav2_core diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt index 2cd43c87154..493418fda67 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -3,6 +3,7 @@ project(nav2_regulated_pure_pursuit_controller) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp index cc8b7915330..135a304dc26 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp @@ -43,7 +43,7 @@ class PathHandler * @brief Constructor for nav2_regulated_pure_pursuit_controller::PathHandler */ PathHandler( - tf2::Duration transform_tolerance, + double transform_tolerance, std::shared_ptr tf, std::shared_ptr costmap_ros); @@ -65,18 +65,6 @@ class PathHandler const geometry_msgs::msg::PoseStamped & pose, double max_robot_pose_search_dist, bool reject_unit_path = false); - /** - * @brief Transform a pose to another frame. - * @param frame Frame ID to transform to - * @param in_pose Pose input to transform - * @param out_pose transformed output - * @return bool if successful - */ - bool transformPose( - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose) const; - void setPlan(const nav_msgs::msg::Path & path) {global_plan_ = path;} nav_msgs::msg::Path getPlan() {return global_plan_;} @@ -89,7 +77,7 @@ class PathHandler double getCostmapMaxExtent() const; rclcpp::Logger logger_ {rclcpp::get_logger("RPPPathHandler")}; - tf2::Duration transform_tolerance_; + double transform_tolerance_; std::shared_ptr tf_; std::shared_ptr costmap_ros_; nav_msgs::msg::Path global_plan_; diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 454b9542c7d..70df60839d7 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -12,6 +12,7 @@ nav2_common angles + backward_ros geometry_msgs nav2_core nav2_costmap_2d diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp index 1fcc39b7e37..c91bb8dc8f7 100644 --- a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp @@ -23,6 +23,7 @@ #include "nav2_core/controller_exceptions.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" namespace nav2_regulated_pure_pursuit_controller { @@ -30,7 +31,7 @@ namespace nav2_regulated_pure_pursuit_controller using nav2_util::geometry_utils::euclidean_distance; PathHandler::PathHandler( - tf2::Duration transform_tolerance, + double transform_tolerance, std::shared_ptr tf, std::shared_ptr costmap_ros) : transform_tolerance_(transform_tolerance), tf_(tf), costmap_ros_(costmap_ros) @@ -60,7 +61,9 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( // let's get the pose of the robot in the frame of the plan geometry_msgs::msg::PoseStamped robot_pose; - if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { + if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_, global_plan_.header.frame_id, + transform_tolerance_)) + { throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); } @@ -101,7 +104,9 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( stamped_pose.header.frame_id = global_plan_.header.frame_id; stamped_pose.header.stamp = robot_pose.header.stamp; stamped_pose.pose = global_plan_pose.pose; - if (!transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose)) { + if (!nav2_util::transformPoseInTargetFrame(stamped_pose, transformed_pose, *tf_, + costmap_ros_->getBaseFrameID(), transform_tolerance_)) + { throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); } transformed_pose.pose.position.z = 0.0; @@ -128,24 +133,4 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( return transformed_plan; } -bool PathHandler::transformPose( - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose) const -{ - if (in_pose.header.frame_id == frame) { - out_pose = in_pose; - return true; - } - - try { - tf_->transform(in_pose, out_pose, frame, transform_tolerance_); - out_pose.header.frame_id = frame; - return true; - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what()); - } - return false; -} - } // namespace nav2_regulated_pure_pursuit_controller 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 d8153ea16bd..1bd12ae3a06 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 @@ -61,7 +61,7 @@ void RegulatedPurePursuitController::configure( // Handles global path transformations path_handler_ = std::make_unique( - tf2::durationFromSec(params_->transform_tolerance), tf_, costmap_ros_); + params_->transform_tolerance, tf_, costmap_ros_); // Checks for imminent collisions collision_checker_ = std::make_unique(node, costmap_ros_, params_); diff --git a/nav2_ros_common/CMakeLists.txt b/nav2_ros_common/CMakeLists.txt index fc529730ea8..13dfd418833 100644 --- a/nav2_ros_common/CMakeLists.txt +++ b/nav2_ros_common/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_ros_common) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(bond REQUIRED) find_package(bondcpp REQUIRED) find_package(geometry_msgs REQUIRED) diff --git a/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp b/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp index 929cb709d44..f978c2432f9 100644 --- a/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp +++ b/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp @@ -67,14 +67,8 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode rclcpp::Parameter( bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true)); - nav2::declare_parameter_if_not_declared( - this, "bond_heartbeat_period", rclcpp::ParameterValue(0.1)); - this->get_parameter("bond_heartbeat_period", bond_heartbeat_period); - - bool autostart_node = false; - nav2::declare_parameter_if_not_declared( - this, "autostart_node", rclcpp::ParameterValue(false)); - this->get_parameter("autostart_node", autostart_node); + bond_heartbeat_period = this->declare_or_get_parameter("bond_heartbeat_period", 0.1); + bool autostart_node = this->declare_or_get_parameter("autostart_node", false); if (autostart_node) { autostart(); } @@ -108,6 +102,26 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode } } + /** + * @brief Declares or gets a parameter. + * If the parameter is already declared, returns its value; + * otherwise declares it and returns the default value. + * @param parameter_name Name of the parameter + * @param default_value Default value of the parameter + * @param parameter_descriptor Optional parameter descriptor + * @return The value of the param from the override if existent, otherwise the default value. + */ + template + inline ParamType declare_or_get_parameter( + const std::string & parameter_name, + const ParamType & default_value, + const ParameterDescriptor & parameter_descriptor = ParameterDescriptor()) + { + return nav2::declare_or_get_parameter( + this, parameter_name, + default_value, parameter_descriptor); + } + /** * @brief Create a subscription to a topic using Nav2 QoS profiles and SubscriptionOptions * @param topic_name Name of topic diff --git a/nav2_ros_common/package.xml b/nav2_ros_common/package.xml index 33731f20e15..e2e2eb2695d 100644 --- a/nav2_ros_common/package.xml +++ b/nav2_ros_common/package.xml @@ -11,6 +11,7 @@ nav2_common + backward_ros bond bondcpp builtin_interfaces diff --git a/nav2_rotation_shim_controller/CMakeLists.txt b/nav2_rotation_shim_controller/CMakeLists.txt index c8140e661a6..0adcf17d8f1 100644 --- a/nav2_rotation_shim_controller/CMakeLists.txt +++ b/nav2_rotation_shim_controller/CMakeLists.txt @@ -3,6 +3,7 @@ project(nav2_rotation_shim_controller) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_controller REQUIRED) diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 2505599a071..02fbffd55f0 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -11,6 +11,7 @@ nav2_common angles + backward_ros geometry_msgs nav2_core nav2_controller diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 0b97876182f..ee87908d827 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -24,7 +24,7 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_controller/plugins/simple_goal_checker.hpp" #include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" class RotationShimShim : public nav2_rotation_shim_controller::RotationShimController { diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt index 3ceb96f6753..fcc53119651 100644 --- a/nav2_route/CMakeLists.txt +++ b/nav2_route/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_route CXX) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) diff --git a/nav2_route/README.md b/nav2_route/README.md index c0eb510b1db..b1c878637bd 100644 --- a/nav2_route/README.md +++ b/nav2_route/README.md @@ -5,7 +5,7 @@ The Route Server is a Nav2 Task server to compliment the Planner Server's free-s * Augment the global planner with long-distance routing to a goal and using free-space feasible planning in a more localized fashion for the immediate 10m, 100m, etc future. This graph has very few rules associated with it and may be generated manually or automatically via AI, geometric, or probabilistic techniques. -docs.nav2.org includes tutorials for how to generate such a graph by annotations on a grid map created via SLAM, but can also be procedurally generated. +docs.nav2.org includes tutorials for how to generate such a graph by annotations on a grid map created via SLAM, but can also be procedurally generated using a tool like [NVIDIA's SWAGGER](https://github.com/nvidia-isaac/SWAGGER). This package then takes a planning request and uses this graph to find a valid route through the environment via an optimal search-based algorithm. It uses plugin-based scoring functions applied each edge based on arbitrary user-defined semantic information and the chosen optimization criteria(s). The Nav2 Route Server may also live monitor and analyze the route's process to execute custom behaviors on entering or leaving edges or achieving particular graph nodes. These behaviors are defined as another type of plugin and can leverage the graph's edges' and nodes' arbitrary semantic data. @@ -427,3 +427,7 @@ A special condition of this when a routing or rerouting request is made up of on This is an application problem which can be addressed above but may have other creative solutions for your application. It is on you as an application developer to determine if this is a problem for you and what the most appropriate solution is, since different applications will have different levels of flexibility of deviating from the route or extremely strict route interpretations. Note that there are parameters like `prune_route`, `min_prune_distance_from_start` and `min_prune_distance_from_goal` which impact the pruning behavior while tracking a route using poses. There is also the option to request routes using NodeIDs instead of poses, which obviously would never have this issue since they are definitionally on the route graph at all times. That bypasses the entire issue if the goal(s) are always known to be on the graph. + +### Graph Generation + +There are tutorials on docs.nav2.org regarding how to generate a file for your application, please review those for more information on how to generate a graph file for your application. diff --git a/nav2_route/include/nav2_route/edge_scorer.hpp b/nav2_route/include/nav2_route/edge_scorer.hpp index c294ac47e43..7b6534b50c4 100644 --- a/nav2_route/include/nav2_route/edge_scorer.hpp +++ b/nav2_route/include/nav2_route/edge_scorer.hpp @@ -19,7 +19,7 @@ #include #include -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" #include "nav2_ros_common/node_utils.hpp" diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp index db26121fcef..8e407d66c91 100644 --- a/nav2_route/include/nav2_route/goal_intent_extractor.hpp +++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp @@ -19,7 +19,7 @@ #include #include -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" #include "nav2_core/route_exceptions.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_ros_common/node_utils.hpp" diff --git a/nav2_route/include/nav2_route/graph_loader.hpp b/nav2_route/include/nav2_route/graph_loader.hpp index 2a39da1bee1..c2235e5e914 100644 --- a/nav2_route/include/nav2_route/graph_loader.hpp +++ b/nav2_route/include/nav2_route/graph_loader.hpp @@ -23,8 +23,8 @@ #include #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_route/types.hpp" diff --git a/nav2_route/include/nav2_route/graph_saver.hpp b/nav2_route/include/nav2_route/graph_saver.hpp index 7cc25d83c04..8155689df55 100644 --- a/nav2_route/include/nav2_route/graph_saver.hpp +++ b/nav2_route/include/nav2_route/graph_saver.hpp @@ -23,8 +23,8 @@ #include #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_route/types.hpp" diff --git a/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp b/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp index e20bade1070..e45980f3a45 100644 --- a/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp +++ b/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp @@ -18,7 +18,7 @@ #include #include -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "pluginlib/class_loader.hpp" #include "nav2_route/types.hpp" diff --git a/nav2_route/include/nav2_route/node_spatial_tree.hpp b/nav2_route/include/nav2_route/node_spatial_tree.hpp index 15d9e2a5528..744d30791c7 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/include/nav2_route/route_planner.hpp b/nav2_route/include/nav2_route/route_planner.hpp index bdadc6c9dd0..cd0c978bc90 100644 --- a/nav2_route/include/nav2_route/route_planner.hpp +++ b/nav2_route/include/nav2_route/route_planner.hpp @@ -22,8 +22,8 @@ #include #include -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_route/types.hpp" #include "nav2_route/utils.hpp" #include "nav2_route/edge_scorer.hpp" diff --git a/nav2_route/include/nav2_route/route_server.hpp b/nav2_route/include/nav2_route/route_server.hpp index 3d384e93c0b..ce52c07e750 100644 --- a/nav2_route/include/nav2_route/route_server.hpp +++ b/nav2_route/include/nav2_route/route_server.hpp @@ -23,8 +23,8 @@ #include #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_ros_common/simple_action_server.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/robot_utils.hpp" diff --git a/nav2_route/include/nav2_route/route_tracker.hpp b/nav2_route/include/nav2_route/route_tracker.hpp index 9c2e7058aec..a2a3d4e897c 100644 --- a/nav2_route/include/nav2_route/route_tracker.hpp +++ b/nav2_route/include/nav2_route/route_tracker.hpp @@ -16,7 +16,7 @@ #include #include -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_ros_common/simple_action_server.hpp" #include "nav2_msgs/action/compute_and_track_route.hpp" diff --git a/nav2_route/package.xml b/nav2_route/package.xml index 855356eefc2..93fffbf8e1a 100644 --- a/nav2_route/package.xml +++ b/nav2_route/package.xml @@ -10,6 +10,7 @@ ament_cmake nav2_common + backward_ros rclcpp rclcpp_lifecycle std_msgs diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp index 2a44005111b..fee6bf7b671 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -35,7 +35,7 @@ RouteServer::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_base_interface(), get_node_timers_interface()); tf_->setCreateTimerInterface(timer_interface); - transform_listener_ = std::make_shared(*tf_); + transform_listener_ = std::make_shared(*tf_, this, true); auto node = shared_from_this(); graph_vis_publisher_ = diff --git a/nav2_route/test/test_edge_scorers.cpp b/nav2_route/test/test_edge_scorers.cpp index 07af4293d0b..8c16ced8306 100644 --- a/nav2_route/test/test_edge_scorers.cpp +++ b/nav2_route/test/test_edge_scorers.cpp @@ -27,8 +27,8 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_2d_publisher.hpp" #include "nav2_core/route_exceptions.hpp" -#include "tf2_ros/static_transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/static_transform_broadcaster.hpp" +#include "tf2_ros/transform_listener.hpp" class RclCppFixture { diff --git a/nav2_route/test/test_goal_intent_extractor.cpp b/nav2_route/test/test_goal_intent_extractor.cpp index c3ac8d176cc..21c49708193 100644 --- a/nav2_route/test/test_goal_intent_extractor.cpp +++ b/nav2_route/test/test_goal_intent_extractor.cpp @@ -19,9 +19,9 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_ros_common/service_client.hpp" #include "nav2_ros_common/node_thread.hpp" diff --git a/nav2_route/test/test_goal_intent_search.cpp b/nav2_route/test/test_goal_intent_search.cpp index a829af0370f..1f3e8fd4b1e 100644 --- a/nav2_route/test/test_goal_intent_search.cpp +++ b/nav2_route/test/test_goal_intent_search.cpp @@ -19,9 +19,9 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_ros_common/service_client.hpp" #include "nav2_ros_common/node_thread.hpp" diff --git a/nav2_route/test/test_graph_loader.cpp b/nav2_route/test/test_graph_loader.cpp index 99341852f78..e76fe863314 100644 --- a/nav2_route/test/test_graph_loader.cpp +++ b/nav2_route/test/test_graph_loader.cpp @@ -20,7 +20,7 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_route/graph_loader.hpp" -#include "tf2_ros/static_transform_broadcaster.h" +#include "tf2_ros/static_transform_broadcaster.hpp" class RclCppFixture { diff --git a/nav2_route/test/test_graph_saver.cpp b/nav2_route/test/test_graph_saver.cpp index a6897f11602..4abdc0b74d3 100644 --- a/nav2_route/test/test_graph_saver.cpp +++ b/nav2_route/test/test_graph_saver.cpp @@ -22,7 +22,7 @@ #include "nav2_ros_common/node_utils.hpp" #include "nav2_route/graph_loader.hpp" #include "nav2_route/graph_saver.hpp" -#include "tf2_ros/static_transform_broadcaster.h" +#include "tf2_ros/static_transform_broadcaster.hpp" class RclCppFixture { diff --git a/nav2_route/test/test_route_server.cpp b/nav2_route/test/test_route_server.cpp index f8906529d31..5fb73b4ce0f 100644 --- a/nav2_route/test/test_route_server.cpp +++ b/nav2_route/test/test_route_server.cpp @@ -19,9 +19,9 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "std_srvs/srv/trigger.hpp" #include "nav2_ros_common/service_client.hpp" diff --git a/nav2_route/test/test_route_tracker.cpp b/nav2_route/test/test_route_tracker.cpp index 9d874a1dc5a..07ca24a4e91 100644 --- a/nav2_route/test/test_route_tracker.cpp +++ b/nav2_route/test/test_route_tracker.cpp @@ -19,9 +19,9 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_core/route_exceptions.hpp" #include "nav2_route/route_tracker.hpp" diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index 958047a22c2..62e19b2f7a2 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -3,6 +3,7 @@ project(nav2_rviz_plugins) find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_lifecycle_manager REQUIRED) diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index 3a4cdd9e50f..44ad28d279e 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -35,9 +35,9 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include "nav2_util/geometry_utils.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/buffer.hpp" class QPushButton; diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 91de46fa84c..3a25157d1ce 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -12,6 +12,7 @@ qtbase5-dev ament_index_cpp + backward_ros geometry_msgs nav2_lifecycle_manager nav2_msgs diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 32ce21abc0a..ea93fc61711 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -34,7 +34,7 @@ ManageLifecycleNodes) from nav_msgs.msg import Goals, OccupancyGrid, Path import rclpy -from rclpy.action import ActionClient # type: ignore[attr-defined] +from rclpy.action import ActionClient from rclpy.action.client import ClientGoalHandle from rclpy.client import Client from rclpy.duration import Duration as rclpyDuration @@ -1242,7 +1242,7 @@ def info(self, msg: str) -> None: return def warn(self, msg: str) -> None: - self.get_logger().warn(msg) + self.get_logger().warning(msg) return def error(self, msg: str) -> None: diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index a344a41aa0f..051818f7c9a 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -4,6 +4,7 @@ project(nav2_smac_planner) find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index f8acf8bd6a6..9400ea3b37b 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -12,6 +12,7 @@ ament_index_cpp angles + backward_ros eigen geometry_msgs nav2_core diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index ad1ae0647c9..1f374af35d0 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -414,15 +414,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( @@ -435,15 +437,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 @@ -460,7 +464,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 d689507e8f4..fa89e90636c 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/CMakeLists.txt b/nav2_smoother/CMakeLists.txt index b2e3555ab2a..5831d10189c 100644 --- a/nav2_smoother/CMakeLists.txt +++ b/nav2_smoother/CMakeLists.txt @@ -3,12 +3,12 @@ project(nav2_smoother) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) -find_package(nav_2d_utils REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) @@ -44,8 +44,6 @@ target_link_libraries(${library_name} PUBLIC nav2_ros_common::nav2_ros_common ) target_link_libraries(${library_name} PRIVATE - nav_2d_utils::conversions - nav_2d_utils::tf_help rclcpp_components::component tf2::tf2 ) diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index e7d4708d790..339bdeb5586 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -12,11 +12,11 @@ nav2_common angles + backward_ros nav2_core nav2_costmap_2d nav2_msgs nav2_util - nav_2d_utils pluginlib rclcpp rclcpp_components diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 4408b5a43e0..74714bafb81 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -23,9 +23,7 @@ #include "nav2_core/smoother_exceptions.hpp" #include "nav2_smoother/nav2_smoother.hpp" #include "nav2_ros_common/node_utils.hpp" -#include "nav_2d_utils/conversions.hpp" -#include "nav_2d_utils/tf_help.hpp" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/create_timer_ros.hpp" using namespace std::chrono_literals; @@ -80,7 +78,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State & state) auto timer_interface = std::make_shared( get_node_base_interface(), get_node_timers_interface()); tf_->setCreateTimerInterface(timer_interface); - transform_listener_ = std::make_shared(*tf_); + transform_listener_ = std::make_shared(*tf_, this, true); std::string costmap_topic, footprint_topic, robot_base_frame; double transform_tolerance = 0.1; diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index db9f323e8a1..5179784f83a 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/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 2f62894abb7..d36535924e0 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 @@ -26,9 +26,9 @@ #include "behaviortree_cpp/bt_factory.h" #include "behaviortree_cpp/utils/shared_library.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_util/odometry_utils.hpp" #include "nav2_util/string_utils.hpp" diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp index bb3f3b18159..40e2aa9a754 100644 --- a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp +++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp @@ -35,8 +35,8 @@ #include "std_msgs/msg/empty.hpp" #include "tf2/utils.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" namespace nav2_system_tests { diff --git a/nav2_system_tests/src/behaviors/backup/backup_tester.py b/nav2_system_tests/src/behaviors/backup/backup_tester.py index 65d5e494d15..1d51f3d492a 100755 --- a/nav2_system_tests/src/behaviors/backup/backup_tester.py +++ b/nav2_system_tests/src/behaviors/backup/backup_tester.py @@ -23,7 +23,7 @@ from nav2_msgs.msg import Costmap from nav2_msgs.srv import ManageLifecycleNodes import rclpy -from rclpy.action import ActionClient # type: ignore[attr-defined] +from rclpy.action import ActionClient from rclpy.action.client import ClientGoalHandle from rclpy.client import Client from rclpy.duration import Duration @@ -313,7 +313,7 @@ def info_msg(self, msg: str) -> None: self.get_logger().info(msg) def warn_msg(self, msg: str) -> None: - self.get_logger().warn(msg) + self.get_logger().warning(msg) def error_msg(self, msg: str) -> None: self.get_logger().error(msg) diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py b/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py index 61b6a0db13f..9f65ecb5897 100755 --- a/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py +++ b/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py @@ -23,7 +23,7 @@ from nav2_msgs.msg import Costmap from nav2_msgs.srv import ManageLifecycleNodes import rclpy -from rclpy.action import ActionClient # type: ignore[attr-defined] +from rclpy.action import ActionClient from rclpy.action.client import ClientGoalHandle from rclpy.client import Client from rclpy.duration import Duration @@ -322,7 +322,7 @@ def info_msg(self, msg: str) -> None: self.get_logger().info(msg) def warn_msg(self, msg: str) -> None: - self.get_logger().warn(msg) + self.get_logger().warning(msg) def error_msg(self, msg: str) -> None: self.get_logger().error(msg) diff --git a/nav2_system_tests/src/behaviors/spin/spin_tester.py b/nav2_system_tests/src/behaviors/spin/spin_tester.py index cb8a9a9ab65..a5892f4fc80 100755 --- a/nav2_system_tests/src/behaviors/spin/spin_tester.py +++ b/nav2_system_tests/src/behaviors/spin/spin_tester.py @@ -23,7 +23,7 @@ from nav2_msgs.msg import Costmap from nav2_msgs.srv import ManageLifecycleNodes import rclpy -from rclpy.action import ActionClient # type: ignore[attr-defined] +from rclpy.action import ActionClient from rclpy.action.client import ClientGoalHandle from rclpy.client import Client from rclpy.duration import Duration @@ -303,7 +303,7 @@ def info_msg(self, msg: str) -> None: self.get_logger().info(msg) def warn_msg(self, msg: str) -> None: - self.get_logger().warn(msg) + self.get_logger().warning(msg) def error_msg(self, msg: str) -> None: self.get_logger().error(msg) diff --git a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp index 5a7313c0cd5..bea4506e68e 100644 --- a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp +++ b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp @@ -33,8 +33,8 @@ #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "tf2/utils.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" namespace nav2_system_tests { diff --git a/nav2_system_tests/src/costmap_filters/tester_node.py b/nav2_system_tests/src/costmap_filters/tester_node.py index 47b4b2352ac..6f523fa4324 100755 --- a/nav2_system_tests/src/costmap_filters/tester_node.py +++ b/nav2_system_tests/src/costmap_filters/tester_node.py @@ -30,7 +30,7 @@ from nav2_msgs.srv import ManageLifecycleNodes from nav_msgs.msg import OccupancyGrid, Path import rclpy -from rclpy.action import ActionClient # type: ignore[attr-defined] +from rclpy.action import ActionClient from rclpy.client import Client from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy @@ -160,7 +160,7 @@ def info_msg(self, msg: str) -> None: self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') def warn_msg(self, msg: str) -> None: - self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m') + self.get_logger().warning('\033[1;37;43m' + msg + '\033[0m') def error_msg(self, msg: str) -> None: self.get_logger().error('\033[1;37;41m' + msg + '\033[0m') diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py index 7e26d36c570..b522f8e69a2 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -23,7 +23,7 @@ from nav2_msgs.srv import ManageLifecycleNodes from rcl_interfaces.srv import SetParameters import rclpy -from rclpy.action import ActionClient # type: ignore[attr-defined] +from rclpy.action import ActionClient from rclpy.action.client import ClientGoalHandle from rclpy.client import Client from rclpy.node import Node @@ -155,7 +155,7 @@ def info_msg(self, msg: str) -> None: self.get_logger().info(msg) def warn_msg(self, msg: str) -> None: - self.get_logger().warn(msg) + self.get_logger().warning(msg) def error_msg(self, msg: str) -> None: self.get_logger().error(msg) diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index d878bc49f22..3c14f795d4f 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -32,7 +32,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "nav2_planner/planner_server.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" namespace nav2_system_tests { diff --git a/nav2_system_tests/src/route/tester_node.py b/nav2_system_tests/src/route/tester_node.py index 18de9f7f8f5..018b8f2dd33 100755 --- a/nav2_system_tests/src/route/tester_node.py +++ b/nav2_system_tests/src/route/tester_node.py @@ -25,7 +25,7 @@ from nav2_msgs.srv import ManageLifecycleNodes from nav2_simple_commander.robot_navigator import BasicNavigator import rclpy -from rclpy.action import ActionClient # type: ignore[attr-defined] +from rclpy.action import ActionClient from rclpy.client import Client from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy diff --git a/nav2_system_tests/src/system/nav_through_poses_tester_node.py b/nav2_system_tests/src/system/nav_through_poses_tester_node.py index f941b3c41da..e4bb015cafb 100755 --- a/nav2_system_tests/src/system/nav_through_poses_tester_node.py +++ b/nav2_system_tests/src/system/nav_through_poses_tester_node.py @@ -25,7 +25,7 @@ from nav2_msgs.action import NavigateThroughPoses from nav2_msgs.srv import ManageLifecycleNodes import rclpy -from rclpy.action import ActionClient # type: ignore[attr-defined] +from rclpy.action import ActionClient from rclpy.client import Client from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy @@ -62,7 +62,7 @@ def info_msg(self, msg: str) -> None: self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') def warn_msg(self, msg: str) -> None: - self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m') + self.get_logger().warning('\033[1;37;43m' + msg + '\033[0m') def error_msg(self, msg: str) -> None: self.get_logger().error('\033[1;37;41m' + msg + '\033[0m') diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index 49b29aa3c0f..cbd4897016f 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -29,7 +29,7 @@ from nav2_msgs.action import NavigateToPose from nav2_msgs.srv import ManageLifecycleNodes import rclpy -from rclpy.action import ActionClient # type: ignore[attr-defined] +from rclpy.action import ActionClient from rclpy.client import Client from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy @@ -69,7 +69,7 @@ def info_msg(self, msg: str) -> None: self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') def warn_msg(self, msg: str) -> None: - self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m') + self.get_logger().warning('\033[1;37;43m' + msg + '\033[0m') def error_msg(self, msg: str) -> None: self.get_logger().error('\033[1;37;41m' + msg + '\033[0m') diff --git a/nav2_system_tests/src/system_failure/tester_node.py b/nav2_system_tests/src/system_failure/tester_node.py index 784fbf967a5..8bbe13aa960 100755 --- a/nav2_system_tests/src/system_failure/tester_node.py +++ b/nav2_system_tests/src/system_failure/tester_node.py @@ -26,7 +26,7 @@ from nav2_msgs.action import NavigateToPose from nav2_msgs.srv import ManageLifecycleNodes import rclpy -from rclpy.action import ActionClient # type: ignore[attr-defined] +from rclpy.action import ActionClient from rclpy.client import Client from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy @@ -64,7 +64,7 @@ def info_msg(self, msg: str) -> None: self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') def warn_msg(self, msg: str) -> None: - self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m') + self.get_logger().warning('\033[1;37;43m' + msg + '\033[0m') def error_msg(self, msg: str) -> None: self.get_logger().error('\033[1;37;41m' + msg + '\033[0m') diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 12a38d8b424..4ea17924f81 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -23,7 +23,7 @@ from nav2_msgs.srv import ManageLifecycleNodes from rcl_interfaces.srv import SetParameters import rclpy -from rclpy.action import ActionClient # type: ignore[attr-defined] +from rclpy.action import ActionClient from rclpy.action.client import ClientGoalHandle from rclpy.client import Client from rclpy.node import Node @@ -199,7 +199,7 @@ def info_msg(self, msg: str) -> None: self.get_logger().info(msg) def warn_msg(self, msg: str) -> None: - self.get_logger().warn(msg) + self.get_logger().warning(msg) def error_msg(self, msg: str) -> None: self.get_logger().error(msg) diff --git a/nav2_theta_star_planner/CMakeLists.txt b/nav2_theta_star_planner/CMakeLists.txt index 87da1fa9cbc..0a478f2fb9b 100644 --- a/nav2_theta_star_planner/CMakeLists.txt +++ b/nav2_theta_star_planner/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_theta_star_planner) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 0279b52fe7e..414de9f3066 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -11,6 +11,7 @@ ament_cmake nav2_common + backward_ros geometry_msgs nav2_core nav2_costmap_2d diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index 08a7908f3b3..2a2f4061818 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_util) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(bond REQUIRED) find_package(bondcpp REQUIRED) find_package(geometry_msgs REQUIRED) diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index cffbe80a26b..faca6d301dc 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -26,7 +26,7 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2/time.hpp" #include "tf2/transform_datatypes.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 0acd0f1a37b..334879c3981 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -13,6 +13,7 @@ nav2_common + backward_ros bond bondcpp builtin_interfaces diff --git a/nav2_util/src/base_footprint_publisher.hpp b/nav2_util/src/base_footprint_publisher.hpp index 67f50625101..15b16037bed 100644 --- a/nav2_util/src/base_footprint_publisher.hpp +++ b/nav2_util/src/base_footprint_publisher.hpp @@ -21,10 +21,10 @@ #include "rclcpp/rclcpp.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/buffer.h" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/buffer.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/utils.hpp" diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 912d73d0859..d333df9cf14 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -50,6 +50,11 @@ bool transformPoseInTargetFrame( { static rclcpp::Logger logger = rclcpp::get_logger("transformPoseInTargetFrame"); + if(input_pose.header.frame_id == target_frame) { + transformed_pose = input_pose; + return true; + } + try { transformed_pose = tf_buffer.transform( input_pose, target_frame, diff --git a/nav2_util/test/test_robot_utils.cpp b/nav2_util/test/test_robot_utils.cpp index 119addc9355..33c55aa2a3b 100644 --- a/nav2_util/test/test_robot_utils.cpp +++ b/nav2_util/test/test_robot_utils.cpp @@ -16,11 +16,11 @@ #include #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_util/robot_utils.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "gtest/gtest.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/create_timer_ros.hpp" TEST(RobotUtils, LookupExceptionError) { diff --git a/nav2_velocity_smoother/CMakeLists.txt b/nav2_velocity_smoother/CMakeLists.txt index 75c59b8a89c..916ee93ed2c 100644 --- a/nav2_velocity_smoother/CMakeLists.txt +++ b/nav2_velocity_smoother/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_velocity_smoother) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_util REQUIRED) diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index b0ed66dd3e6..52cfb61d9d7 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -10,6 +10,7 @@ ament_cmake nav2_common + backward_ros geometry_msgs nav2_util rclcpp diff --git a/nav2_voxel_grid/CMakeLists.txt b/nav2_voxel_grid/CMakeLists.txt index e204367ada8..093fee48a84 100644 --- a/nav2_voxel_grid/CMakeLists.txt +++ b/nav2_voxel_grid/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_voxel_grid) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index ef9eedd9b42..36d5b14ba6b 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -12,6 +12,7 @@ ament_cmake nav2_common + backward_ros rclcpp ament_lint_common diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index f41c6e5f730..17d406adef9 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_waypoint_follower) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(cv_bridge REQUIRED) find_package(geographic_msgs REQUIRED) find_package(geometry_msgs REQUIRED) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 83edf012880..816feb9eca3 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -37,9 +37,9 @@ #include "nav2_core/waypoint_task_executor.hpp" #include "robot_localization/srv/from_ll.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" namespace nav2_waypoint_follower { diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 4aa90590587..980881aa7a9 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -10,6 +10,7 @@ ament_cmake nav2_common + backward_ros cv_bridge geographic_msgs geometry_msgs