From 4e5d2dfc66cb75eb390d614d13e5b64efbf30284 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Sat, 4 Jan 2025 15:40:29 -0800 Subject: [PATCH] deprecating C API for TF2 and fixing test failures (#4823) * deprecating C API for TF2 and fixing test failures Signed-off-by: Steve Macenski * Update costmap_2d_ros.cpp Signed-off-by: Steve Macenski * Update 2d_utils_test.cpp Signed-off-by: Steve Macenski * fix remaining issue Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- nav2_amcl/src/amcl_node.cpp | 6 ++--- .../nav2_behavior_tree/ros_topic_logger.hpp | 2 +- .../remove_in_collision_goals_action.cpp | 2 +- .../action/truncate_path_local_action.cpp | 2 +- .../transform_available_condition.cpp | 2 +- .../action/test_truncate_path_action.cpp | 4 +-- .../include/nav2_behaviors/timed_behavior.hpp | 2 +- nav2_behaviors/plugins/spin.cpp | 2 +- .../collision_detector_node.hpp | 2 +- .../collision_monitor_node.hpp | 2 +- .../nav2_collision_monitor/polygon.hpp | 2 +- .../include/nav2_collision_monitor/source.hpp | 2 +- nav2_collision_monitor/src/pointcloud.cpp | 2 +- nav2_collision_monitor/src/polygon.cpp | 2 +- nav2_collision_monitor/src/polygon_source.cpp | 2 +- nav2_collision_monitor/src/range.cpp | 2 +- nav2_collision_monitor/src/scan.cpp | 2 +- .../src/constrained_smoother.cpp | 2 +- .../plugins/simple_goal_checker.cpp | 2 +- .../nav2_costmap_2d/costmap_2d_publisher.hpp | 4 +-- .../nav2_costmap_2d/costmap_2d_ros.hpp | 10 ++++---- .../costmap_filters/keepout_filter.cpp | 2 +- nav2_costmap_2d/plugins/static_layer.cpp | 2 +- nav2_costmap_2d/src/footprint_subscriber.cpp | 2 +- nav2_costmap_2d/src/observation_buffer.cpp | 2 +- .../test_costmap_topic_collision_checker.cpp | 2 +- .../opennav_docking/simple_charging_dock.hpp | 2 +- .../simple_non_charging_dock.hpp | 2 +- .../include/opennav_docking/utils.hpp | 2 +- .../opennav_docking/src/controller.cpp | 2 +- .../opennav_docking/src/docking_server.cpp | 2 +- .../opennav_docking/test/test_pose_filter.cpp | 2 +- .../test/test_simple_charging_dock.cpp | 2 +- .../test/test_simple_non_charging_dock.cpp | 2 +- .../include/nav_2d_utils/conversions.hpp | 2 +- .../nav_2d_utils/src/conversions.cpp | 2 +- .../nav_2d_utils/test/2d_utils_test.cpp | 2 +- .../ego_polar_coords.hpp | 2 +- nav2_map_server/src/map_io.cpp | 4 +-- .../nav2_mppi_controller/tools/utils.hpp | 2 +- .../test/test_shim_controller.cpp | 25 +++++++++++-------- .../nav2_smac_planner/smac_planner_2d.hpp | 2 +- .../nav2_smac_planner/smac_planner_hybrid.hpp | 2 +- .../smac_planner_lattice.hpp | 2 +- .../include/nav2_smac_planner/utils.hpp | 2 +- nav2_smac_planner/src/smoother.cpp | 2 +- .../nav2_smoother/savitzky_golay_smoother.hpp | 2 +- .../include/nav2_smoother/simple_smoother.hpp | 2 +- .../include/nav2_smoother/smoother_utils.hpp | 2 +- .../assisted_teleop_behavior_tester.hpp | 2 +- .../behaviors/wait/wait_behavior_tester.hpp | 2 +- nav2_util/include/nav2_util/robot_utils.hpp | 4 +-- nav2_util/src/base_footprint_publisher.hpp | 2 +- nav2_util/src/costmap.cpp | 2 +- nav2_util/src/robot_utils.cpp | 2 +- .../test/test_base_footprint_publisher.cpp | 2 +- 56 files changed, 79 insertions(+), 76 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index a590eee1f11..c04d8c5f578 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -35,10 +35,10 @@ #include "nav2_util/string_utils.hpp" #include "nav2_amcl/sensors/laser/laser.hpp" #include "rclcpp/node_options.hpp" -#include "tf2/convert.h" -#include "tf2/utils.h" +#include "tf2/convert.hpp" +#include "tf2/utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/LinearMath/Transform.h" +#include "tf2/LinearMath/Transform.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/message_filter.h" #include "tf2_ros/transform_broadcaster.h" 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 8a75712b56b..a65334e5b61 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 @@ -23,7 +23,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_msgs/msg/behavior_tree_log.hpp" #include "nav2_msgs/msg/behavior_tree_status_change.h" -#include "tf2/time.h" +#include "tf2/time.hpp" #include "tf2_ros/buffer_interface.h" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp index 78e6c2e35df..8728cd1cd86 100644 --- a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp @@ -18,7 +18,7 @@ #include "nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp" #include "nav2_behavior_tree/bt_utils.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace nav2_behavior_tree 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 f002710be6b..271e64added 100644 --- a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp +++ b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp @@ -24,7 +24,7 @@ #include "nav2_util/robot_utils.hpp" #include "nav_msgs/msg/path.hpp" #include "rclcpp/rclcpp.hpp" -#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Quaternion.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/create_timer_ros.h" diff --git a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp index ab589048c82..4f84e3e9fc8 100644 --- a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp @@ -17,7 +17,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "tf2/time.h" +#include "tf2/time.hpp" #include "tf2_ros/buffer.h" #include "nav2_behavior_tree/plugins/condition/transform_available_condition.hpp" diff --git a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp index de0ae1282d4..56b1b4b0567 100644 --- a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp @@ -23,8 +23,8 @@ #include "nav_msgs/msg/path.hpp" #include "nav2_util/geometry_utils.hpp" #include "rclcpp/rclcpp.hpp" -#include "tf2/LinearMath/Matrix3x3.h" -#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Matrix3x3.hpp" +#include "tf2/LinearMath/Quaternion.hpp" #include "behaviortree_cpp/bt_factory.h" diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 23a3640bcd5..a8979910e61 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -35,7 +35,7 @@ #include "nav2_core/behavior.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #pragma GCC diagnostic pop diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index ce89eb34f23..bd0ff1bf4d1 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -19,7 +19,7 @@ #include #include "nav2_behaviors/plugins/spin.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_util/node_utils.hpp" 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 7c9785588e2..a3a429b2daa 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 @@ -22,7 +22,7 @@ #include "rclcpp/rclcpp.hpp" -#include "tf2/time.h" +#include "tf2/time.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index c6caeeb5b2d..69274b06fdb 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 @@ -25,7 +25,7 @@ #include "visualization_msgs/msg/marker_array.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "tf2/time.h" +#include "tf2/time.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index b81571765b9..1cbd01ba163 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -23,7 +23,7 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" -#include "tf2/time.h" +#include "tf2/time.hpp" #include "tf2_ros/buffer.h" #include "nav2_util/lifecycle_node.hpp" diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 4432ec243b1..3384bf8d333 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" -#include "tf2/time.h" +#include "tf2/time.hpp" #include "tf2_ros/buffer.h" #include "nav2_collision_monitor/types.hpp" diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 77daab68055..c54c0524ea0 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -17,7 +17,7 @@ #include #include "sensor_msgs/point_cloud2_iterator.hpp" -#include "tf2/transform_datatypes.h" +#include "tf2/transform_datatypes.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 19ac5053de8..a528c74d640 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -19,7 +19,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/point32.hpp" -#include "tf2/transform_datatypes.h" +#include "tf2/transform_datatypes.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" diff --git a/nav2_collision_monitor/src/polygon_source.cpp b/nav2_collision_monitor/src/polygon_source.cpp index 149ac43153f..a147529670f 100644 --- a/nav2_collision_monitor/src/polygon_source.cpp +++ b/nav2_collision_monitor/src/polygon_source.cpp @@ -18,7 +18,7 @@ #include #include "geometry_msgs/msg/polygon_stamped.hpp" -#include "tf2/transform_datatypes.h" +#include "tf2/transform_datatypes.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index fe555b171f3..d871c795082 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -18,7 +18,7 @@ #include #include -#include "tf2/transform_datatypes.h" +#include "tf2/transform_datatypes.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index f891df69a0a..0bdcefcd66b 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -17,7 +17,7 @@ #include #include -#include "tf2/transform_datatypes.h" +#include "tf2/transform_datatypes.hpp" #include "nav2_util/robot_utils.hpp" diff --git a/nav2_constrained_smoother/src/constrained_smoother.cpp b/nav2_constrained_smoother/src/constrained_smoother.cpp index 6ff2f504a25..6b5bef4a303 100644 --- a/nav2_constrained_smoother/src/constrained_smoother.cpp +++ b/nav2_constrained_smoother/src/constrained_smoother.cpp @@ -30,7 +30,7 @@ #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" using nav2_util::declare_parameter_if_not_declared; using nav2_util::geometry_utils::euclidean_distance; diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index 597f843eaac..30b90f4f4ab 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -43,7 +43,7 @@ #include "nav2_util/geometry_utils.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #pragma GCC diagnostic pop using rcl_interfaces::msg::ParameterType; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp index 97a9c0a4ff8..41041c44b6e 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp @@ -50,9 +50,9 @@ #include "nav2_msgs/msg/costmap.hpp" #include "nav2_msgs/msg/costmap_update.hpp" #include "nav2_msgs/srv/get_costmap.hpp" -#include "tf2/transform_datatypes.h" +#include "tf2/transform_datatypes.hpp" #include "nav2_util/lifecycle_node.hpp" -#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Quaternion.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace nav2_costmap_2d 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 3f514e9ad26..797b211666f 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 @@ -54,16 +54,16 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/srv/get_costs.hpp" #include "pluginlib/class_loader.hpp" -#include "tf2/convert.h" -#include "tf2/LinearMath/Transform.h" +#include "tf2/convert.hpp" +#include "tf2/LinearMath/Transform.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "tf2/time.h" -#include "tf2/transform_datatypes.h" +#include "tf2/time.hpp" +#include "tf2/transform_datatypes.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #pragma GCC diagnostic pop namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index 3ac785296a2..74277fb58d3 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -38,7 +38,7 @@ #include #include #include -#include "tf2/convert.h" +#include "tf2/convert.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp" diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 896dfc77a5f..c067e1a0071 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -43,7 +43,7 @@ #include #include "pluginlib/class_list_macros.hpp" -#include "tf2/convert.h" +#include "tf2/convert.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_util/validate_messages.hpp" diff --git a/nav2_costmap_2d/src/footprint_subscriber.cpp b/nav2_costmap_2d/src/footprint_subscriber.cpp index 2c269fa3b06..7cd65781e27 100644 --- a/nav2_costmap_2d/src/footprint_subscriber.cpp +++ b/nav2_costmap_2d/src/footprint_subscriber.cpp @@ -19,7 +19,7 @@ #include "nav2_costmap_2d/footprint_subscriber.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #pragma GCC diagnostic pop namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/observation_buffer.cpp b/nav2_costmap_2d/src/observation_buffer.cpp index 8a06823df38..a3df54e080d 100644 --- a/nav2_costmap_2d/src/observation_buffer.cpp +++ b/nav2_costmap_2d/src/observation_buffer.cpp @@ -42,7 +42,7 @@ #include #include -#include "tf2/convert.h" +#include "tf2/convert.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" using namespace std::chrono_literals; 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 ec6748f980e..a069bf1f1b8 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 @@ -35,7 +35,7 @@ #include "tf2_ros/transform_broadcaster.h" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #pragma GCC diagnostic pop #include "nav2_util/geometry_utils.hpp" diff --git a/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp b/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp index c9b46e4a400..65a5596c18c 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp @@ -23,7 +23,7 @@ #include "sensor_msgs/msg/battery_state.hpp" #include "sensor_msgs/msg/joint_state.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "opennav_docking_core/charging_dock.hpp" #include "opennav_docking/pose_filter.hpp" diff --git a/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp b/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp index cecb5299150..d037e1aa79c 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp @@ -23,7 +23,7 @@ #include "sensor_msgs/msg/battery_state.hpp" #include "sensor_msgs/msg/joint_state.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "opennav_docking_core/non_charging_dock.hpp" #include "opennav_docking/pose_filter.hpp" diff --git a/nav2_docking/opennav_docking/include/opennav_docking/utils.hpp b/nav2_docking/opennav_docking/include/opennav_docking/utils.hpp index ce0b5660036..1acd623f3ee 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/utils.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/utils.hpp @@ -24,7 +24,7 @@ #include "nav2_util/geometry_utils.hpp" #include "angles/angles.h" #include "opennav_docking/types.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" namespace utils { diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index ec2a1de9ab9..8128ba82253 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/nav2_docking/opennav_docking/src/controller.cpp @@ -20,7 +20,7 @@ #include "nav2_util/geometry_utils.hpp" #include "nav2_util/node_utils.hpp" #include "nav_2d_utils/conversions.hpp" -#include "tf2/utils.h" +#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 d7af36c9ebc..1497a2355f5 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -15,7 +15,7 @@ #include "angles/angles.h" #include "opennav_docking/docking_server.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" using namespace std::chrono_literals; using rcl_interfaces::msg::ParameterType; diff --git a/nav2_docking/opennav_docking/test/test_pose_filter.cpp b/nav2_docking/opennav_docking/test/test_pose_filter.cpp index f010c9c0108..207fff3f8c9 100644 --- a/nav2_docking/opennav_docking/test/test_pose_filter.cpp +++ b/nav2_docking/opennav_docking/test/test_pose_filter.cpp @@ -18,7 +18,7 @@ #include "rclcpp/rclcpp.hpp" #include "opennav_docking/pose_filter.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" // Testing the pose filter diff --git a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp index 7193bb188b0..44ac7442cf7 100644 --- a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp @@ -20,7 +20,7 @@ #include "opennav_docking/simple_charging_dock.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" // Testing the simple charging dock plugin diff --git a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp index 89c423f5ff3..9705082af1d 100644 --- a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp @@ -20,7 +20,7 @@ #include "opennav_docking/simple_non_charging_dock.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" // Testing the simple non-charging dock plugin 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 910361615ef..901e9f0a1e5 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 @@ -44,7 +44,7 @@ #include "nav_2d_msgs/msg/pose2_d_stamped.hpp" #include "nav_msgs/msg/path.hpp" #include "rclcpp/rclcpp.hpp" -#include "tf2/convert.h" +#include "tf2/convert.hpp" namespace nav_2d_utils { diff --git a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp index 42fdff6bb3f..5abed18bdcb 100644 --- a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp +++ b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp @@ -48,7 +48,7 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #pragma GCC diagnostic pop #include "nav2_util/geometry_utils.hpp" 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 8f2bd171b2d..c8424038a16 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 @@ -33,8 +33,8 @@ */ #include -#include #include +#include #include "gtest/gtest.h" #include "nav_2d_utils/conversions.hpp" diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp index f0f1229a103..00b44d01617 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp @@ -19,7 +19,7 @@ #include "angles/angles.h" #include "geometry_msgs/msg/pose.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" namespace nav2_graceful_controller { diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index b97342cf820..11a9b9b895e 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -46,8 +46,8 @@ #include "yaml-cpp/yaml.h" -#include "tf2/LinearMath/Matrix3x3.h" -#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Matrix3x3.hpp" +#include "tf2/LinearMath/Quaternion.hpp" #include "nav2_util/occ_grid_values.hpp" #ifdef _WIN32 diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 72fbaeb6011..a7ca647aa88 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -36,7 +36,7 @@ #include "angles/angles.h" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 1d63a77b47e..537f344d746 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -176,15 +176,16 @@ TEST(RotationShimControllerTest, rotationAndTransformTests) auto node = std::make_shared("ShimControllerTest"); std::string name = "PathFollower"; auto tf = std::make_shared(node->get_clock()); - auto costmap = std::make_shared("fake_costmap"); - rclcpp_lifecycle::State state; - costmap->on_configure(state); + auto costmap = std::make_shared("fake_costmap", "/", false); + costmap->configure(); // set a valid primary controller so we can do lifecycle node->declare_parameter( "PathFollower.primary_controller", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); + node->declare_parameter("controller_frequency", 1.0); + auto controller = std::make_shared(); controller->configure(node, name, tf, costmap); controller->activate(); @@ -193,8 +194,8 @@ TEST(RotationShimControllerTest, rotationAndTransformTests) nav_msgs::msg::Path path; path.header.frame_id = "fake_frame"; path.poses.resize(10); - path.poses[1].pose.position.x = 0.1; - path.poses[1].pose.position.y = 0.1; + path.poses[1].pose.position.x = 0.15; + path.poses[1].pose.position.y = 0.15; path.poses[2].pose.position.x = 1.0; path.poses[2].pose.position.y = 1.0; path.poses[3].pose.position.x = 10.0; @@ -204,14 +205,14 @@ TEST(RotationShimControllerTest, rotationAndTransformTests) const geometry_msgs::msg::Twist velocity; EXPECT_EQ( controller->computeRotateToHeadingCommandWrapper( - 0.7, path.poses[0], velocity).twist.angular.z, 1.8); + 0.7, path.poses[1], velocity).twist.angular.z, 1.8); EXPECT_EQ( controller->computeRotateToHeadingCommandWrapper( - -0.7, path.poses[0], velocity).twist.angular.z, -1.8); + -0.7, path.poses[1], velocity).twist.angular.z, -1.8); EXPECT_EQ( controller->computeRotateToHeadingCommandWrapper( - 0.87, path.poses[0], velocity).twist.angular.z, 1.8); + 0.87, path.poses[1], velocity).twist.angular.z, 1.8); // in base_link, so should pass through values without issue geometry_msgs::msg::PoseStamped pt; @@ -237,8 +238,9 @@ TEST(RotationShimControllerTest, computeVelocityTests) auto tf = std::make_shared(node->get_clock()); auto listener = std::make_shared(*tf, node, true); auto costmap = std::make_shared("fake_costmap"); - rclcpp_lifecycle::State state; - costmap->on_configure(state); + costmap->set_parameter(rclcpp::Parameter("origin_x", -25.0)); + costmap->set_parameter(rclcpp::Parameter("origin_y", -25.0)); + costmap->configure(); auto tf_broadcaster = std::make_shared(node); geometry_msgs::msg::TransformStamped transform; @@ -254,6 +256,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) node->declare_parameter( "PathFollower.primary_controller", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); + node->declare_parameter("controller_frequency", 1.0); auto controller = std::make_shared(); controller->configure(node, name, tf, costmap); @@ -308,7 +311,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // this should allow it to find the sampled point, then transform to base_link // validly because we setup the TF for it. The 1.0 should be selected since default min // is 0.5 and that should cause a pass off to the RPP controller which will throw - // and exception because the costmap is bogus + // and exception because it is off of the costmap controller->setPlan(path); tf_broadcaster->sendTransform(transform); EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error); diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp index 059871d8677..b386f8d6edb 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp @@ -32,7 +32,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "rcl_interfaces/msg/set_parameters_result.hpp" namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index a4b7ed180bb..4469579f178 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -32,7 +32,7 @@ #include "geometry_msgs/msg/pose_array.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" namespace nav2_smac_planner { diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index 74be801ad2b..e204c8e01fb 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -31,7 +31,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" namespace nav2_smac_planner { diff --git a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp index 89cb2f8dc77..c2edd13b6e2 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp @@ -23,7 +23,7 @@ #include "nlohmann/json.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/pose.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" #include "visualization_msgs/msg/marker_array.hpp" diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp index 298efa51e2f..4372c10127d 100644 --- a/nav2_smac_planner/src/smoother.cpp +++ b/nav2_smac_planner/src/smoother.cpp @@ -21,7 +21,7 @@ #include "angles/angles.h" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "nav2_smac_planner/smoother.hpp" diff --git a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp index 090aa07af85..170d31d7cb7 100644 --- a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp @@ -31,7 +31,7 @@ #include "nav2_util/node_utils.hpp" #include "nav_msgs/msg/path.hpp" #include "angles/angles.h" -#include "tf2/utils.h" +#include "tf2/utils.hpp" namespace nav2_smoother { diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index da0f98bf965..3686f433376 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp @@ -31,7 +31,7 @@ #include "nav2_util/node_utils.hpp" #include "nav_msgs/msg/path.hpp" #include "angles/angles.h" -#include "tf2/utils.h" +#include "tf2/utils.hpp" namespace nav2_smoother { diff --git a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp index c7a45f383c0..c9e326c7f36 100644 --- a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp +++ b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp @@ -30,7 +30,7 @@ #include "nav2_util/node_utils.hpp" #include "nav_msgs/msg/path.hpp" #include "angles/angles.h" -#include "tf2/utils.h" +#include "tf2/utils.hpp" namespace smoother_utils { 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 626588bbd09..ebfb1f53a64 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 @@ -34,7 +34,7 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/empty.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" 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 18e131de88b..cef62bdaf56 100644 --- a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp +++ b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp @@ -31,7 +31,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index cb7d2b9fdfb..e76a2cce708 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -24,8 +24,8 @@ #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2/time.h" -#include "tf2/transform_datatypes.h" +#include "tf2/time.hpp" +#include "tf2/transform_datatypes.hpp" #include "tf2_ros/buffer.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_util/src/base_footprint_publisher.hpp b/nav2_util/src/base_footprint_publisher.hpp index 25d58d504bf..67f50625101 100644 --- a/nav2_util/src/base_footprint_publisher.hpp +++ b/nav2_util/src/base_footprint_publisher.hpp @@ -26,7 +26,7 @@ #include "tf2_ros/transform_broadcaster.h" #include "tf2_ros/buffer.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" namespace nav2_util { diff --git a/nav2_util/src/costmap.cpp b/nav2_util/src/costmap.cpp index 91f3b2fbc82..5ab609634b8 100644 --- a/nav2_util/src/costmap.cpp +++ b/nav2_util/src/costmap.cpp @@ -15,7 +15,7 @@ #include #include #include "nav2_util/costmap.hpp" -#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Quaternion.hpp" #include "nav2_util/geometry_utils.hpp" using std::vector; diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index a9f4d137fa2..736709ce641 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -18,7 +18,7 @@ #include #include -#include "tf2/convert.h" +#include "tf2/convert.hpp" #include "nav2_util/robot_utils.hpp" #include "rclcpp/logger.hpp" diff --git a/nav2_util/test/test_base_footprint_publisher.cpp b/nav2_util/test/test_base_footprint_publisher.cpp index cd788831325..c85a4c931b7 100644 --- a/nav2_util/test/test_base_footprint_publisher.cpp +++ b/nav2_util/test/test_base_footprint_publisher.cpp @@ -17,7 +17,7 @@ #include "base_footprint_publisher.hpp" #include "gtest/gtest.h" -#include "tf2/exceptions.h" +#include "tf2/exceptions.hpp" class RclCppFixture {