From 3a4219dde7b5425912d83a8ff429878465a2522b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 8 Aug 2024 10:29:19 -0400 Subject: [PATCH 01/21] Switch nav2_velocity_smoother to modern CMake idioms. (#4606) Signed-off-by: Chris Lalancette --- nav2_velocity_smoother/CMakeLists.txt | 66 ++++++++++------------ nav2_velocity_smoother/package.xml | 9 ++- nav2_velocity_smoother/test/CMakeLists.txt | 8 ++- 3 files changed, 42 insertions(+), 41 deletions(-) diff --git a/nav2_velocity_smoother/CMakeLists.txt b/nav2_velocity_smoother/CMakeLists.txt index ae92e41d90..7e1e3cb6b9 100644 --- a/nav2_velocity_smoother/CMakeLists.txt +++ b/nav2_velocity_smoother/CMakeLists.txt @@ -2,57 +2,42 @@ cmake_minimum_required(VERSION 3.5) project(nav2_velocity_smoother) find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) +find_package(nav2_util REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav2_util REQUIRED) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() +find_package(rclcpp_lifecycle REQUIRED) nav2_package() -include_directories( - include -) - set(executable_name velocity_smoother) set(library_name ${executable_name}_core) -set(dependencies - rclcpp - rclcpp_components - geometry_msgs - nav2_util -) - # Main library add_library(${library_name} SHARED src/velocity_smoother.cpp ) -ament_target_dependencies(${library_name} - ${dependencies} +target_include_directories(${library_name} + PUBLIC + "$" + "$" +) +target_link_libraries(${library_name} PUBLIC + ${geometry_msgs_TARGETS} + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) +target_link_libraries(${library_name} PRIVATE + rclcpp_components::component ) # Main executable add_executable(${executable_name} src/main.cpp ) -ament_target_dependencies(${executable_name} - ${dependencies} -) -target_link_libraries(${executable_name} ${library_name}) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - set(ament_cmake_copyright_FOUND TRUE) - ament_lint_auto_find_test_dependencies() - - find_package(ament_cmake_gtest REQUIRED) -endif() +target_link_libraries(${executable_name} PRIVATE ${library_name} rclcpp::rclcpp) rclcpp_components_register_nodes(${library_name} "nav2_velocity_smoother::VelocitySmoother") @@ -68,17 +53,28 @@ install(TARGETS ${executable_name} ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + find_package(nav_msgs REQUIRED) + + ament_find_gtest() add_subdirectory(test) endif() -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(${library_name}) -ament_export_dependencies(${dependencies}) +ament_export_dependencies( + geometry_msgs + nav2_util + rclcpp + rclcpp_lifecycle +) ament_package() diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 65cf0ae986..5163de4f84 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -9,14 +9,17 @@ ament_cmake nav2_common - rclcpp - rclcpp_components + geometry_msgs nav2_util + rclcpp + rclcpp_components + rclcpp_lifecycle + ament_cmake_gtest ament_lint_common ament_lint_auto - ament_cmake_gtest + nav_msgs ament_cmake diff --git a/nav2_velocity_smoother/test/CMakeLists.txt b/nav2_velocity_smoother/test/CMakeLists.txt index 781049578f..fedd9de499 100644 --- a/nav2_velocity_smoother/test/CMakeLists.txt +++ b/nav2_velocity_smoother/test/CMakeLists.txt @@ -2,9 +2,11 @@ ament_add_gtest(velocity_smoother_tests test_velocity_smoother.cpp ) -ament_target_dependencies(velocity_smoother_tests - ${dependencies} -) target_link_libraries(velocity_smoother_tests ${library_name} + ${geometry_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) From 746bb880a775201fbdc4cca38a873cc3d0d9ab40 Mon Sep 17 00:00:00 2001 From: Vladyslav Hrynchak Date: Thu, 8 Aug 2024 18:12:31 +0300 Subject: [PATCH 02/21] Fixed timed_behavior.hpp (#4602) Signed-off-by: Vladyslav Hrynchak --- .../include/nav2_behaviors/timed_behavior.hpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index eab6330d50..2e560fb721 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -237,15 +237,6 @@ class TimedBehavior : public nav2_core::Behavior while (rclcpp::ok()) { elasped_time_ = clock_->now() - start_time; - if (action_server_->is_cancel_requested()) { - RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str()); - stopRobot(); - result->total_elapsed_time = elasped_time_; - onActionCompletion(result); - action_server_->terminate_all(result); - return; - } - // TODO(orduno) #868 Enable preempting a Behavior on-the-fly without stopping if (action_server_->is_preempt_requested()) { RCLCPP_ERROR( @@ -259,6 +250,15 @@ class TimedBehavior : public nav2_core::Behavior return; } + if (action_server_->is_cancel_requested()) { + RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str()); + stopRobot(); + result->total_elapsed_time = elasped_time_; + onActionCompletion(result); + action_server_->terminate_all(result); + return; + } + ResultStatus on_cycle_update_result = onCycleUpdate(); switch (on_cycle_update_result.status) { case Status::SUCCEEDED: From dc16f4ae51ab76655f898f35a79addb9a08cc269 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 8 Aug 2024 13:06:44 -0400 Subject: [PATCH 03/21] Switch nav2_graceful_controller to modern CMake idioms. (#4608) Signed-off-by: Chris Lalancette --- nav2_graceful_controller/CMakeLists.txt | 95 +++++++++++++------ .../ego_polar_coords.hpp | 4 +- nav2_graceful_controller/package.xml | 19 ++-- .../src/graceful_controller.cpp | 3 + nav2_graceful_controller/test/CMakeLists.txt | 20 ++-- 5 files changed, 94 insertions(+), 47 deletions(-) diff --git a/nav2_graceful_controller/CMakeLists.txt b/nav2_graceful_controller/CMakeLists.txt index 9cec5b9204..e79ea8d01f 100644 --- a/nav2_graceful_controller/CMakeLists.txt +++ b/nav2_graceful_controller/CMakeLists.txt @@ -2,44 +2,38 @@ cmake_minimum_required(VERSION 3.5) project(nav2_graceful_controller) find_package(ament_cmake REQUIRED) +find_package(angles 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(rclcpp REQUIRED) -find_package(geometry_msgs REQUIRED) +find_package(nav_2d_utils REQUIRED) find_package(nav_msgs REQUIRED) find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rcl_interfaces REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -find_package(nav_2d_utils REQUIRED) -find_package(angles REQUIRED) +find_package(tf2_ros REQUIRED) find_package(visualization_msgs REQUIRED) nav2_package() -include_directories( - include -) - -set(dependencies - rclcpp - geometry_msgs - nav2_costmap_2d - pluginlib - nav_msgs - nav2_util - nav2_core - tf2 - tf2_geometry_msgs - nav_2d_utils - angles - visualization_msgs -) - # Add Smooth Control Law as library add_library(smooth_control_law SHARED src/smooth_control_law.cpp) -ament_target_dependencies(smooth_control_law ${dependencies}) +target_include_directories(smooth_control_law + PUBLIC + "$" + "$" +) +target_link_libraries(smooth_control_law PUBLIC + angles::angles + ${geometry_msgs_TARGETS} + nav2_util::nav2_util_core + tf2::tf2 +) # Add Graceful Controller set(library_name nav2_graceful_controller) @@ -50,18 +44,40 @@ add_library(${library_name} SHARED src/path_handler.cpp src/utils.cpp ) - -target_link_libraries(${library_name} smooth_control_law) -ament_target_dependencies(${library_name} ${dependencies}) +target_include_directories(${library_name} + PUBLIC + "$" + "$" +) +target_link_libraries(${library_name} PUBLIC + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_util::nav2_util_core + nav2_costmap_2d::nav2_costmap_2d_core + nav_2d_utils::tf_help + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + smooth_control_law + tf2::tf2 + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} +) +target_link_libraries(${library_name} PRIVATE + tf2_geometry_msgs::tf2_geometry_msgs +) install(TARGETS smooth_control_law ${library_name} + EXPORT nav2_graceful_controller ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) @@ -70,12 +86,31 @@ if(BUILD_TESTING) # the following line skips the linter which checks for copyrights set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + ament_find_gtest() + add_subdirectory(test) endif() -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(smooth_control_law ${library_name}) -ament_export_dependencies(${dependencies}) +ament_export_dependencies( + geometry_msgs + nav2_core + nav2_util + nav2_costmap_2d + nav_2d_utils + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + rcl_interfaces + tf2 + tf2_ros + visualization_msgs +) +ament_export_targets(nav2_graceful_controller) pluginlib_export_plugin_description_file(nav2_core graceful_controller_plugin.xml) 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 eadf608844..f0f1229a10 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 @@ -15,13 +15,11 @@ #ifndef NAV2_GRACEFUL_CONTROLLER__EGO_POLAR_COORDS_HPP_ #define NAV2_GRACEFUL_CONTROLLER__EGO_POLAR_COORDS_HPP_ -#include +#include #include "angles/angles.h" #include "geometry_msgs/msg/pose.hpp" #include "tf2/utils.h" -#include "tf2/transform_datatypes.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace nav2_graceful_controller { diff --git a/nav2_graceful_controller/package.xml b/nav2_graceful_controller/package.xml index b6deb17b72..b45196dcf5 100644 --- a/nav2_graceful_controller/package.xml +++ b/nav2_graceful_controller/package.xml @@ -8,24 +8,27 @@ Apache-2.0 ament_cmake + nav2_common - nav2_common + angles + geometry_msgs nav2_core - nav2_util nav2_costmap_2d - rclcpp - geometry_msgs - nav2_msgs + nav2_util + nav_2d_utils + nav_msgs pluginlib + rclcpp + rclcpp_lifecycle + rcl_interfaces tf2 tf2_geometry_msgs - nav_2d_utils - angles + tf2_ros visualization_msgs + ament_cmake_gtest ament_lint_auto ament_lint_common - ament_cmake_gtest nav2_controller diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index f8610a55c9..b442ede1fe 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include + #include "nav2_core/controller_exceptions.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_graceful_controller/graceful_controller.hpp" diff --git a/nav2_graceful_controller/test/CMakeLists.txt b/nav2_graceful_controller/test/CMakeLists.txt index 1becbba0b1..d78b32e594 100644 --- a/nav2_graceful_controller/test/CMakeLists.txt +++ b/nav2_graceful_controller/test/CMakeLists.txt @@ -4,18 +4,26 @@ find_package(nav2_controller REQUIRED) ament_add_gtest(test_graceful_controller test_graceful_controller.cpp ) -ament_target_dependencies(test_graceful_controller - ${dependencies} - nav2_controller -) target_link_libraries(test_graceful_controller ${library_name} + ${geometry_msgs_TARGETS} + nav2_controller::simple_goal_checker + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + rclcpp::rclcpp + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} ) # Egopolar test ament_add_gtest(test_egopolar test_egopolar.cpp ) -ament_target_dependencies(test_egopolar - geometry_msgs tf2_geometry_msgs angles +target_link_libraries(test_egopolar + ${library_name} + ${geometry_msgs_TARGETS} + rclcpp::rclcpp + tf2_geometry_msgs::tf2_geometry_msgs ) From 47a1235ceee5f95ce738af01c84ce9e468463ded Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 8 Aug 2024 19:09:31 +0200 Subject: [PATCH 04/21] Remove in collision goals (#4595) * Remove goals in collision Signed-off-by: Tony Najjar * fix for main Signed-off-by: Tony Najjar * Fix linting Signed-off-by: Tony Najjar * linting fixes Signed-off-by: Tony Najjar * PR fixes Signed-off-by: Tony Najjar * remove second costmap Signed-off-by: Tony Najjar * Minor fixes Signed-off-by: Tony Najjar * getcosts instead of cost Signed-off-by: Tony Najjar * get future once Signed-off-by: Tony Najjar * replace with BTServiceNode Signed-off-by: Tony Najjar * Add test Signed-off-by: Tony Najjar * fix costmap tool Signed-off-by: Tony Najjar * fix ament_uncrustify Signed-off-by: Tony Najjar * remove duplicate Signed-off-by: Tony Najjar * add return Signed-off-by: Tony Najjar * fix typo Signed-off-by: Tony Najjar * reset request before sending Signed-off-by: Tony Najjar * pr comments fixes Signed-off-by: Tony Najjar * fix ament cmake Signed-off-by: Tony Najjar * fix test Signed-off-by: Tony Najjar * fix Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar --- nav2_behavior_tree/CMakeLists.txt | 3 + .../remove_in_collision_goals_action.hpp | 72 +++++++ nav2_behavior_tree/nav2_tree_nodes.xml | 8 + .../remove_in_collision_goals_action.cpp | 77 ++++++++ .../action/remove_passed_goals_action.cpp | 1 + .../test/plugins/action/CMakeLists.txt | 4 + .../test_remove_in_collision_goals_action.cpp | 180 ++++++++++++++++++ .../nav2_costmap_2d/costmap_2d_ros.hpp | 10 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 48 +++-- .../test/unit/costmap_cost_service_test.cpp | 32 ++-- nav2_msgs/CMakeLists.txt | 2 +- nav2_msgs/srv/GetCost.srv | 8 - nav2_msgs/srv/GetCosts.srv | 6 + .../nav2_rviz_plugins/costmap_cost_tool.hpp | 10 +- nav2_rviz_plugins/src/costmap_cost_tool.cpp | 25 +-- 15 files changed, 421 insertions(+), 65 deletions(-) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp create mode 100644 nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp delete mode 100644 nav2_msgs/srv/GetCost.srv create mode 100644 nav2_msgs/srv/GetCosts.srv diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index b33948bf90..8f45cd318f 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -176,6 +176,9 @@ list(APPEND plugin_libs nav2_navigate_through_poses_action_bt_node) add_library(nav2_remove_passed_goals_action_bt_node SHARED plugins/action/remove_passed_goals_action.cpp) list(APPEND plugin_libs nav2_remove_passed_goals_action_bt_node) +add_library(nav2_remove_in_collision_goals_action_bt_node SHARED plugins/action/remove_in_collision_goals_action.cpp) +list(APPEND plugin_libs nav2_remove_in_collision_goals_action_bt_node) + add_library(nav2_get_pose_from_path_action_bt_node SHARED plugins/action/get_pose_from_path_action.cpp) list(APPEND plugin_libs nav2_get_pose_from_path_action_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp new file mode 100644 index 0000000000..73aab1bd8c --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp @@ -0,0 +1,72 @@ +// Copyright (c) 2024 Angsa 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__ACTION__REMOVE_IN_COLLISION_GOALS_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_IN_COLLISION_GOALS_ACTION_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_behavior_tree/bt_service_node.hpp" +#include "nav2_msgs/srv/get_costs.hpp" + +namespace nav2_behavior_tree +{ + +class RemoveInCollisionGoals : public BtServiceNode +{ +public: + typedef std::vector Goals; + + /** + * @brief A constructor for nav2_behavior_tree::RemoveInCollisionGoals + * @param service_node_name Service name this node creates a client for + * @param conf BT node configuration + */ + RemoveInCollisionGoals( + const std::string & service_node_name, + const BT::NodeConfiguration & conf); + + /** + * @brief The main override required by a BT service + * @return BT::NodeStatus Status of tick execution + */ + void on_tick() override; + + BT::NodeStatus on_completion(std::shared_ptr response) + override; + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("input_goals", "Original goals to remove from"), + BT::InputPort("cost_threshold", 254.0, + "Cost threshold for considering a goal in collision"), + BT::InputPort("use_footprint", true, "Whether to use footprint cost"), + BT::OutputPort("output_goals", "Goals with in-collision goals removed"), + }); + } + +private: + bool use_footprint_; + double cost_threshold_; + Goals input_goals_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_IN_COLLISION_GOALS_ACTION_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 925e9c3579..71c20ee4ec 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -100,6 +100,14 @@ Set of goals after removing any passed + + Costmap service name responsible for getting the cost + A vector of goals to check if in collision + Whether to use the footprint cost or the point cost. + The cost threshold above which a waypoint is considered in collision and should be removed. + A vector of goals containing only those that are not in collision. + + Path to be smoothed 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 new file mode 100644 index 0000000000..2fe8395ad4 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp @@ -0,0 +1,77 @@ +// Copyright (c) 2024 Angsa 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 + +#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_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace nav2_behavior_tree +{ + +RemoveInCollisionGoals::RemoveInCollisionGoals( + const std::string & service_node_name, + const BT::NodeConfiguration & conf) +: BtServiceNode(service_node_name, conf, + "/global_costmap/get_cost_global_costmap") +{} + + +void RemoveInCollisionGoals::on_tick() +{ + getInput("use_footprint", use_footprint_); + getInput("cost_threshold", cost_threshold_); + getInput("input_goals", input_goals_); + + if (input_goals_.empty()) { + setOutput("output_goals", input_goals_); + should_send_request_ = false; + return; + } + request_ = std::make_shared(); + request_->use_footprint = use_footprint_; + + for (const auto & goal : input_goals_) { + geometry_msgs::msg::Pose2D pose; + pose.x = goal.pose.position.x; + pose.y = goal.pose.position.y; + pose.theta = tf2::getYaw(goal.pose.orientation); + request_->poses.push_back(pose); + } +} + +BT::NodeStatus RemoveInCollisionGoals::on_completion( + std::shared_ptr response) +{ + Goals valid_goal_poses; + for (size_t i = 0; i < response->costs.size(); ++i) { + if (response->costs[i] < cost_threshold_) { + valid_goal_poses.push_back(input_goals_[i]); + } + } + setOutput("output_goals", valid_goal_poses); + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("RemoveInCollisionGoals"); +} diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index 86f5fffd6b..1b0e449431 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -43,6 +43,7 @@ void RemovePassedGoals::initialize() robot_base_frame_ = BT::deconflictPortAndParamFrame( node, "robot_base_frame", this); + initialized_ = true; } inline BT::NodeStatus RemovePassedGoals::tick() diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 1be5a262ba..e538e63cd0 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -51,6 +51,10 @@ plugin_add_test(test_truncate_path_local_action test_truncate_path_local_action. plugin_add_test(test_remove_passed_goals_action test_remove_passed_goals_action.cpp nav2_remove_passed_goals_action_bt_node) +plugin_add_test(test_remove_in_collision_goals_action + test_remove_in_collision_goals_action.cpp + nav2_remove_in_collision_goals_action_bt_node) + plugin_add_test(test_get_pose_from_path_action test_get_pose_from_path_action.cpp nav2_get_pose_from_path_action_bt_node) plugin_add_test(test_planner_selector_node test_planner_selector_node.cpp nav2_planner_selector_bt_node) diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp new file mode 100644 index 0000000000..dbb4889298 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp @@ -0,0 +1,180 @@ +// Copyright (c) 2024 Angsa 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 +#include +#include + +#include "behaviortree_cpp/bt_factory.h" + +#include "utils/test_service.hpp" +#include "nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp" +#include "utils/test_behavior_tree_fixture.hpp" + + +class RemoveInCollisionGoalsService : public TestService +{ +public: + RemoveInCollisionGoalsService() + : TestService("/global_costmap/get_cost_global_costmap") + {} + + virtual void handle_service( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + (void)request_header; + (void)request; + response->costs = {100, 50, 5, 254}; + } +}; + + +class RemoveInCollisionGoalsTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("in_collision_goals_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "RemoveInCollisionGoals", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + server_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + static std::shared_ptr server_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr RemoveInCollisionGoalsTestFixture::node_ = nullptr; + +BT::NodeConfiguration * RemoveInCollisionGoalsTestFixture::config_ = nullptr; +std::shared_ptr +RemoveInCollisionGoalsTestFixture::server_ = nullptr; +std::shared_ptr RemoveInCollisionGoalsTestFixture::factory_ = nullptr; +std::shared_ptr RemoveInCollisionGoalsTestFixture::tree_ = nullptr; + +TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new goal and set it on blackboard + std::vector poses; + poses.resize(4); + poses[0].pose.position.x = 0.0; + poses[0].pose.position.y = 0.0; + + poses[1].pose.position.x = 0.5; + poses[1].pose.position.y = 0.0; + + poses[2].pose.position.x = 1.0; + poses[2].pose.position.y = 0.0; + + poses[3].pose.position.x = 2.0; + poses[3].pose.position.y = 0.0; + + config_->blackboard->set("goals", poses); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check that it removed the point in range + std::vector output_poses; + EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); + + EXPECT_EQ(output_poses.size(), 3u); + EXPECT_EQ(output_poses[0], poses[0]); + EXPECT_EQ(output_poses[1], poses[1]); + EXPECT_EQ(output_poses[2], poses[2]); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize service and spin on new thread + RemoveInCollisionGoalsTestFixture::server_ = + std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(RemoveInCollisionGoalsTestFixture::server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} 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 c95a19902d..33feaa700d 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 @@ -52,7 +52,7 @@ #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav2_costmap_2d/layer.hpp" #include "nav2_util/lifecycle_node.hpp" -#include "nav2_msgs/srv/get_cost.hpp" +#include "nav2_msgs/srv/get_costs.hpp" #include "pluginlib/class_loader.hpp" #include "tf2/convert.h" #include "tf2/LinearMath/Transform.h" @@ -345,10 +345,10 @@ class Costmap2DROS : public nav2_util::LifecycleNode * @param request x and y coordinates in map * @param response cost of the point */ - void getCostCallback( + void getCostsCallback( const std::shared_ptr, - const std::shared_ptr request, - const std::shared_ptr response); + const std::shared_ptr request, + const std::shared_ptr response); protected: // Publishers and subscribers @@ -425,7 +425,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::vector padded_footprint_; // Services - rclcpp::Service::SharedPtr get_cost_service_; + rclcpp::Service::SharedPtr get_cost_service_; std::unique_ptr clear_costmap_service_; // Dynamic parameters handler diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 0fdfb5d2b8..dcf6d50b7a 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -252,9 +252,9 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) } // Service to get the cost at a point - get_cost_service_ = create_service( + get_cost_service_ = create_service( "get_cost_" + getName(), - std::bind(&Costmap2DROS::getCostCallback, this, std::placeholders::_1, std::placeholders::_2, + std::bind(&Costmap2DROS::getCostsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); // Add cleaning service @@ -825,34 +825,40 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter return result; } -void Costmap2DROS::getCostCallback( +void Costmap2DROS::getCostsCallback( const std::shared_ptr, - const std::shared_ptr request, - const std::shared_ptr response) + const std::shared_ptr request, + const std::shared_ptr response) { unsigned int mx, my; Costmap2D * costmap = layered_costmap_->getCostmap(); - if (request->use_footprint) { - Footprint footprint = layered_costmap_->getFootprint(); - FootprintCollisionChecker collision_checker(costmap); + for (const auto & pose : request->poses) { + bool in_bounds = costmap->worldToMap(pose.x, pose.y, mx, my); - RCLCPP_INFO( - get_logger(), "Received request to get cost at footprint pose (%.2f, %.2f, %.2f)", - request->x, request->y, request->theta); + if (!in_bounds) { + response->costs.push_back(-1.0); + continue; + } - response->cost = collision_checker.footprintCostAtPose( - request->x, request->y, request->theta, footprint); - } else if (costmap->worldToMap(request->x, request->y, mx, my)) { - RCLCPP_INFO( - get_logger(), "Received request to get cost at point (%f, %f)", request->x, request->y); + if (request->use_footprint) { + Footprint footprint = layered_costmap_->getFootprint(); + FootprintCollisionChecker collision_checker(costmap); - // Get the cost at the map coordinates - response->cost = static_cast(costmap->getCost(mx, my)); - } else { - RCLCPP_WARN(get_logger(), "Point (%f, %f) is out of bounds", request->x, request->y); - response->cost = -1.0; + RCLCPP_DEBUG( + get_logger(), "Received request to get cost at footprint pose (%.2f, %.2f, %.2f)", + pose.x, pose.y, pose.theta); + + response->costs.push_back( + collision_checker.footprintCostAtPose(pose.x, pose.y, pose.theta, footprint)); + } else { + RCLCPP_DEBUG( + get_logger(), "Received request to get cost at point (%f, %f)", pose.x, pose.y); + + // Get the cost at the map coordinates + response->costs.push_back(static_cast(costmap->getCost(mx, my))); + } } } diff --git a/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp index 8563d6dd16..803e2a1f8e 100644 --- a/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp +++ b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp @@ -18,7 +18,7 @@ #include #include -#include "nav2_msgs/srv/get_cost.hpp" +#include "nav2_msgs/srv/get_costs.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" class RclCppFixture @@ -37,21 +37,23 @@ class GetCostServiceTest : public ::testing::Test void SetUp() override { costmap_ = std::make_shared("costmap"); - client_ = costmap_->create_client( + client_ = costmap_->create_client( "/costmap/get_cost_costmap"); costmap_->on_configure(rclcpp_lifecycle::State()); ASSERT_TRUE(client_->wait_for_service(10s)); } std::shared_ptr costmap_; - rclcpp::Client::SharedPtr client_; + rclcpp::Client::SharedPtr client_; }; TEST_F(GetCostServiceTest, TestWithoutFootprint) { - auto request = std::make_shared(); - request->x = 0.5; - request->y = 1.0; + auto request = std::make_shared(); + geometry_msgs::msg::Pose2D pose; + pose.x = 0.5; + pose.y = 1.0; + request->poses.push_back(pose); request->use_footprint = false; auto result_future = client_->async_send_request(request); @@ -60,8 +62,8 @@ TEST_F(GetCostServiceTest, TestWithoutFootprint) result_future) == rclcpp::FutureReturnCode::SUCCESS) { auto response = result_future.get(); - EXPECT_GE(response->cost, 0.0) << "Cost is less than 0"; - EXPECT_LE(response->cost, 255.0) << "Cost is greater than 255"; + EXPECT_GE(response->costs[0], 0.0) << "Cost is less than 0"; + EXPECT_LE(response->costs[0], 255.0) << "Cost is greater than 255"; } else { FAIL() << "Failed to call service"; } @@ -69,10 +71,12 @@ TEST_F(GetCostServiceTest, TestWithoutFootprint) TEST_F(GetCostServiceTest, TestWithFootprint) { - auto request = std::make_shared(); - request->x = 1.0; - request->y = 1.0; - request->theta = 0.5; + auto request = std::make_shared(); + geometry_msgs::msg::Pose2D pose; + pose.x = 0.5; + pose.y = 1.0; + pose.theta = 0.5; + request->poses.push_back(pose); request->use_footprint = true; auto result_future = client_->async_send_request(request); @@ -81,8 +85,8 @@ TEST_F(GetCostServiceTest, TestWithFootprint) result_future) == rclcpp::FutureReturnCode::SUCCESS) { auto response = result_future.get(); - EXPECT_GE(response->cost, 0.0) << "Cost is less than 0"; - EXPECT_LE(response->cost, 255.0) << "Cost is greater than 255"; + EXPECT_GE(response->costs[0], 0.0) << "Cost is less than 0"; + EXPECT_LE(response->costs[0], 255.0) << "Cost is greater than 255"; } else { FAIL() << "Failed to call service"; } diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 8cee7cdb7f..a9f93b5ecb 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -30,7 +30,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Particle.msg" "msg/ParticleCloud.msg" "msg/MissedWaypoint.msg" - "srv/GetCost.srv" + "srv/GetCosts.srv" "srv/GetCostmap.srv" "srv/IsPathValid.srv" "srv/ClearCostmapExceptRegion.srv" diff --git a/nav2_msgs/srv/GetCost.srv b/nav2_msgs/srv/GetCost.srv deleted file mode 100644 index 577654f55c..0000000000 --- a/nav2_msgs/srv/GetCost.srv +++ /dev/null @@ -1,8 +0,0 @@ -# Get costmap cost at given point - -bool use_footprint -float32 x -float32 y -float32 theta ---- -float32 cost \ No newline at end of file diff --git a/nav2_msgs/srv/GetCosts.srv b/nav2_msgs/srv/GetCosts.srv new file mode 100644 index 0000000000..0328d47639 --- /dev/null +++ b/nav2_msgs/srv/GetCosts.srv @@ -0,0 +1,6 @@ +# Get costmap costs at given poses + +bool use_footprint +geometry_msgs/Pose2D[] poses +--- +float32[] costs \ No newline at end of file diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp index b70529f11a..a14a9f98b5 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include #include @@ -41,15 +41,15 @@ class CostmapCostTool : public rviz_common::Tool void callCostService(float x, float y); - void handleLocalCostResponse(rclcpp::Client::SharedFuture); - void handleGlobalCostResponse(rclcpp::Client::SharedFuture); + void handleLocalCostResponse(rclcpp::Client::SharedFuture); + void handleGlobalCostResponse(rclcpp::Client::SharedFuture); private Q_SLOTS: void updateAutoDeactivate(); private: - rclcpp::Client::SharedPtr local_cost_client_; - rclcpp::Client::SharedPtr global_cost_client_; + rclcpp::Client::SharedPtr local_cost_client_; + rclcpp::Client::SharedPtr global_cost_client_; // The Node pointer that we need to keep alive for the duration of this plugin. std::shared_ptr node_ptr_; diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 5873b54f45..d5ef565874 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -54,9 +54,9 @@ void CostmapCostTool::onInitialize() rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); local_cost_client_ = - node->create_client("/local_costmap/get_cost_local_costmap"); + node->create_client("/local_costmap/get_cost_local_costmap"); global_cost_client_ = - node->create_client("/global_costmap/get_cost_global_costmap"); + node->create_client("/global_costmap/get_cost_global_costmap"); } void CostmapCostTool::activate() {} @@ -95,9 +95,12 @@ int CostmapCostTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) void CostmapCostTool::callCostService(float x, float y) { // Create request for local costmap - auto request = std::make_shared(); - request->x = x; - request->y = y; + auto request = std::make_shared(); + geometry_msgs::msg::Pose2D pose; + pose.x = x; + pose.y = y; + request->poses.push_back(pose); + request->use_footprint = false; // Call local costmap service if (local_cost_client_->wait_for_service(std::chrono::seconds(1))) { @@ -113,24 +116,24 @@ void CostmapCostTool::callCostService(float x, float y) } void CostmapCostTool::handleLocalCostResponse( - rclcpp::Client::SharedFuture future) + rclcpp::Client::SharedFuture future) { rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); auto response = future.get(); - if (response->cost != -1) { - RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", response->cost); + if (response->costs[0] != -1) { + RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", response->costs[0]); } else { RCLCPP_ERROR(node->get_logger(), "Failed to get local costmap cost"); } } void CostmapCostTool::handleGlobalCostResponse( - rclcpp::Client::SharedFuture future) + rclcpp::Client::SharedFuture future) { rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); auto response = future.get(); - if (response->cost != -1) { - RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", response->cost); + if (response->costs[0] != -1) { + RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", response->costs[0]); } else { RCLCPP_ERROR(node->get_logger(), "Failed to get global costmap cost"); } From 45dd27af3d6558a1680f591693c19274c306d96d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 8 Aug 2024 14:28:34 -0400 Subject: [PATCH 05/21] Switch costmap_queue to use modern CMake idioms. (#4609) Signed-off-by: Chris Lalancette --- .../costmap_queue/CMakeLists.txt | 46 +++++++++++-------- nav2_dwb_controller/costmap_queue/package.xml | 4 +- 2 files changed, 28 insertions(+), 22 deletions(-) diff --git a/nav2_dwb_controller/costmap_queue/CMakeLists.txt b/nav2_dwb_controller/costmap_queue/CMakeLists.txt index 3cb0d967f8..28d47ed3dd 100644 --- a/nav2_dwb_controller/costmap_queue/CMakeLists.txt +++ b/nav2_dwb_controller/costmap_queue/CMakeLists.txt @@ -4,26 +4,20 @@ project(costmap_queue) find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_costmap_2d REQUIRED) -find_package(rclcpp REQUIRED) nav2_package() -include_directories( - include -) - add_library(${PROJECT_NAME} SHARED src/costmap_queue.cpp src/limited_costmap_queue.cpp ) - -set(dependencies - rclcpp - nav2_costmap_2d +target_include_directories(${PROJECT_NAME} + PUBLIC + "$" + "$" ) - -ament_target_dependencies(${PROJECT_NAME} - ${dependencies} +target_link_libraries(${PROJECT_NAME} PUBLIC + nav2_costmap_2d::nav2_costmap_2d_core ) if(BUILD_TESTING) @@ -34,24 +28,36 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + find_package(rclcpp REQUIRED) + + ament_find_gtest() + ament_add_gtest(mbq_test test/mbq_test.cpp) - ament_target_dependencies(mbq_test ${dependencies}) + target_link_libraries(mbq_test + ${PROJECT_NAME} + ) ament_add_gtest(utest test/utest.cpp) - ament_target_dependencies(utest ${dependencies}) - target_link_libraries(utest ${PROJECT_NAME}) + target_link_libraries(utest + ${PROJECT_NAME} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + ) endif() install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(nav2_costmap_2d) +ament_export_targets(${PROJECT_NAME}) ament_package() diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 4ea5fa964d..3a9a7bf9cc 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -10,11 +10,11 @@ nav2_common nav2_costmap_2d - rclcpp + ament_cmake_gtest ament_lint_common ament_lint_auto - ament_cmake_gtest + rclcpp ament_cmake From 4d2c2c6a4137aa52cf6ebd5ada07122ce7616c64 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 8 Aug 2024 17:28:06 -0400 Subject: [PATCH 06/21] Switch dwb_core to modern CMake idioms. (#4610) Signed-off-by: Chris Lalancette --- nav2_dwb_controller/dwb_core/CMakeLists.txt | 93 +++++++++++-------- nav2_dwb_controller/dwb_core/package.xml | 45 ++++----- .../dwb_core/test/CMakeLists.txt | 5 +- 3 files changed, 76 insertions(+), 67 deletions(-) diff --git a/nav2_dwb_controller/dwb_core/CMakeLists.txt b/nav2_dwb_controller/dwb_core/CMakeLists.txt index 58c6323b65..e689b31805 100644 --- a/nav2_dwb_controller/dwb_core/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_core/CMakeLists.txt @@ -2,64 +2,64 @@ cmake_minimum_required(VERSION 3.5) project(dwb_core) find_package(ament_cmake REQUIRED) -find_package(nav2_common REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_2d_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) find_package(dwb_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) -find_package(pluginlib REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(visualization_msgs REQUIRED) +find_package(nav2_util REQUIRED) +find_package(nav_2d_msgs REQUIRED) find_package(nav_2d_utils REQUIRED) find_package(nav_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) find_package(tf2_ros REQUIRED) -find_package(nav2_util REQUIRED) -find_package(nav2_core REQUIRED) +find_package(visualization_msgs REQUIRED) nav2_package() -include_directories( - include -) - -set(dependencies - rclcpp - std_msgs - geometry_msgs - nav_2d_msgs - dwb_msgs - nav2_costmap_2d - pluginlib - sensor_msgs - visualization_msgs - nav_2d_utils - nav_msgs - tf2_ros - nav2_util - nav2_core -) - add_library(dwb_core SHARED src/dwb_local_planner.cpp src/publisher.cpp src/illegal_trajectory_tracker.cpp src/trajectory_utils.cpp ) - -ament_target_dependencies(dwb_core - ${dependencies} +target_include_directories(dwb_core PUBLIC + "$" + "$" +) +target_link_libraries(dwb_core PUBLIC + ${builtin_interfaces_TARGETS} + ${dwb_msgs_TARGETS} + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + 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 + rclcpp_lifecycle::rclcpp_lifecycle + ${sensor_msgs_TARGETS} + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} ) install(TARGETS dwb_core + EXPORT dwb_core ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) @@ -69,12 +69,31 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) + ament_find_gtest() + add_subdirectory(test) endif() -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(dwb_core) -ament_export_dependencies(${dependencies}) +ament_export_dependencies( + builtin_interfaces + dwb_msgs + geometry_msgs + nav2_core + nav2_costmap_2d + nav2_util + nav_2d_msgs + nav_2d_utils + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + sensor_msgs + tf2_ros + visualization_msgs +) +ament_export_targets(dwb_core) pluginlib_export_plugin_description_file(nav2_core local_planner_plugin.xml) diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 01f8e64b1c..ea3e167a2b 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -10,38 +10,25 @@ ament_cmake nav2_common - rclcpp - std_msgs - geometry_msgs - nav_2d_msgs - dwb_msgs - nav2_costmap_2d - pluginlib - sensor_msgs - visualization_msgs - nav_2d_utils - nav_msgs - tf2_ros - nav2_util - nav2_core - - rclcpp - std_msgs - rclcpp - std_msgs - geometry_msgs - dwb_msgs - nav2_costmap_2d - nav_2d_utils - pluginlib - nav_msgs - tf2_ros - nav2_util - nav2_core + builtin_interfaces + dwb_msgs + geometry_msgs + nav2_core + nav2_costmap_2d + nav2_util + nav_2d_msgs + nav_2d_utils + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + sensor_msgs + tf2_ros + visualization_msgs + ament_cmake_gtest ament_lint_common ament_lint_auto - ament_cmake_gtest ament_cmake diff --git a/nav2_dwb_controller/dwb_core/test/CMakeLists.txt b/nav2_dwb_controller/dwb_core/test/CMakeLists.txt index 4c009a9416..ad76143f03 100644 --- a/nav2_dwb_controller/dwb_core/test/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_core/test/CMakeLists.txt @@ -1,2 +1,5 @@ ament_add_gtest(utils_test utils_test.cpp) -target_link_libraries(utils_test dwb_core) +target_link_libraries(utils_test + dwb_core + ${dwb_msgs_TARGETS} +) From 5321e00972740fb03b6c512c128ecf19470e340d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 9 Aug 2024 11:17:31 -0400 Subject: [PATCH 07/21] Switch dwb_critics to modern CMake idioms. (#4611) Signed-off-by: Chris Lalancette --- .../dwb_critics/CMakeLists.txt | 92 ++++++++++--------- nav2_dwb_controller/dwb_critics/package.xml | 8 +- .../dwb_critics/test/CMakeLists.txt | 31 ++++++- 3 files changed, 79 insertions(+), 52 deletions(-) diff --git a/nav2_dwb_controller/dwb_critics/CMakeLists.txt b/nav2_dwb_controller/dwb_critics/CMakeLists.txt index 32b95518e2..17833b7569 100644 --- a/nav2_dwb_controller/dwb_critics/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_critics/CMakeLists.txt @@ -2,68 +2,66 @@ cmake_minimum_required(VERSION 3.5) project(dwb_critics) find_package(ament_cmake REQUIRED) -find_package(nav2_common REQUIRED) find_package(angles REQUIRED) -find_package(nav2_costmap_2d REQUIRED) find_package(costmap_queue REQUIRED) find_package(dwb_core REQUIRED) +find_package(dwb_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_util REQUIRED) find_package(nav_2d_msgs REQUIRED) find_package(nav_2d_utils REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(nav2_util REQUIRED) nav2_package() -include_directories( - include -) - add_library(${PROJECT_NAME} SHARED - src/alignment_util.cpp - src/map_grid.cpp - src/goal_dist.cpp - src/path_dist.cpp - src/goal_align.cpp - src/path_align.cpp - src/base_obstacle.cpp - src/obstacle_footprint.cpp - src/oscillation.cpp - src/prefer_forward.cpp - src/rotate_to_goal.cpp - src/twirling.cpp + src/alignment_util.cpp + src/map_grid.cpp + src/goal_dist.cpp + src/path_dist.cpp + src/goal_align.cpp + src/path_align.cpp + src/base_obstacle.cpp + src/obstacle_footprint.cpp + src/oscillation.cpp + src/prefer_forward.cpp + src/rotate_to_goal.cpp + src/twirling.cpp ) - -set(dependencies - angles - nav2_costmap_2d - costmap_queue - dwb_core - geometry_msgs - nav_2d_msgs - nav_2d_utils - pluginlib - rclcpp - sensor_msgs - nav2_util +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$" ) - -ament_target_dependencies(${PROJECT_NAME} - ${dependencies} +target_link_libraries(${PROJECT_NAME} PUBLIC + costmap_queue::costmap_queue + dwb_core::dwb_core + ${dwb_msgs_TARGETS} + ${geometry_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_2d_msgs_TARGETS} + rclcpp::rclcpp +) +target_link_libraries(${PROJECT_NAME} PRIVATE + angles::angles + nav2_util::nav2_util_core + nav_2d_utils::path_ops + pluginlib::pluginlib ) install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) install(FILES default_critics.xml - DESTINATION share/${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME} ) if(BUILD_TESTING) @@ -72,15 +70,23 @@ if(BUILD_TESTING) set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) + ament_find_gtest() add_subdirectory(test) endif() -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(${PROJECT_NAME}) ament_export_dependencies( - ${dependencies} + costmap_queue + dwb_core + dwb_msgs + geometry_msgs + nav2_costmap_2d + nav_2d_msgs + rclcpp ) +ament_export_targets(${PROJECT_NAME}) pluginlib_export_plugin_description_file(dwb_core default_critics.xml) diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 43e104b547..92d1f2df16 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -10,20 +10,20 @@ nav2_common angles - nav2_costmap_2d - nav2_util costmap_queue dwb_core + dwb_msgs geometry_msgs + nav2_costmap_2d + nav2_util nav_2d_msgs nav_2d_utils pluginlib rclcpp - sensor_msgs + ament_cmake_gtest ament_lint_common ament_lint_auto - ament_cmake_gtest ament_cmake diff --git a/nav2_dwb_controller/dwb_critics/test/CMakeLists.txt b/nav2_dwb_controller/dwb_critics/test/CMakeLists.txt index 07672ca698..67fc8cb6a3 100644 --- a/nav2_dwb_controller/dwb_critics/test/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_critics/test/CMakeLists.txt @@ -1,14 +1,35 @@ ament_add_gtest(prefer_forward_tests prefer_forward_test.cpp) -target_link_libraries(prefer_forward_tests dwb_critics) +target_link_libraries(prefer_forward_tests + dwb_critics + nav2_costmap_2d::nav2_costmap_2d_core + nav2_util::nav2_util_core + rclcpp::rclcpp +) ament_add_gtest(base_obstacle_tests base_obstacle_test.cpp) -target_link_libraries(base_obstacle_tests dwb_critics) +target_link_libraries(base_obstacle_tests + dwb_critics + dwb_core::dwb_core + rclcpp::rclcpp +) ament_add_gtest(obstacle_footprint_tests obstacle_footprint_test.cpp) -target_link_libraries(obstacle_footprint_tests dwb_critics) +target_link_libraries(obstacle_footprint_tests + dwb_critics + dwb_core::dwb_core + rclcpp::rclcpp +) ament_add_gtest(alignment_util_tests alignment_util_test.cpp) -target_link_libraries(alignment_util_tests dwb_critics) +target_link_libraries(alignment_util_tests + dwb_critics + dwb_core::dwb_core + rclcpp::rclcpp +) ament_add_gtest(twirling_tests twirling_test.cpp) -target_link_libraries(twirling_tests dwb_critics) +target_link_libraries(twirling_tests + dwb_critics + dwb_core::dwb_core + rclcpp::rclcpp +) From 3f311124e356dbfa33f35e23caa5474a57f99ab7 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 12 Aug 2024 12:33:33 -0700 Subject: [PATCH 08/21] Adding new Nav2 loopback simulator (#4614) * adding Nav2 loopback sim Signed-off-by: Steve Macenski * drop performance by half * lintin * Add multirobot usecase comment * fixing copy paste error * fixing review comments --------- Signed-off-by: Steve Macenski --- nav2_bringup/launch/bringup_launch.py | 11 +- .../launch/tb3_loopback_simulation.launch.py | 224 +++++++++++++++++ .../launch/tb4_loopback_simulation.launch.py | 232 ++++++++++++++++++ nav2_bringup/params/nav2_params.yaml | 8 + nav2_loopback_sim/README.md | 51 ++++ .../launch/loopback_simulation.launch.py | 52 ++++ .../nav2_loopback_sim/loopback_simulator.py | 223 +++++++++++++++++ nav2_loopback_sim/nav2_loopback_sim/utils.py | 65 +++++ nav2_loopback_sim/package.xml | 25 ++ nav2_loopback_sim/pytest.ini | 2 + nav2_loopback_sim/resource/nav2_loopback_sim | 0 nav2_loopback_sim/setup.cfg | 4 + nav2_loopback_sim/setup.py | 30 +++ nav2_loopback_sim/test/test_copyright.py | 23 ++ nav2_loopback_sim/test/test_flake8.py | 25 ++ nav2_loopback_sim/test/test_pep257.py | 23 ++ 16 files changed, 996 insertions(+), 2 deletions(-) create mode 100644 nav2_bringup/launch/tb3_loopback_simulation.launch.py create mode 100644 nav2_bringup/launch/tb4_loopback_simulation.launch.py create mode 100644 nav2_loopback_sim/README.md create mode 100644 nav2_loopback_sim/launch/loopback_simulation.launch.py create mode 100644 nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py create mode 100644 nav2_loopback_sim/nav2_loopback_sim/utils.py create mode 100644 nav2_loopback_sim/package.xml create mode 100644 nav2_loopback_sim/pytest.ini create mode 100644 nav2_loopback_sim/resource/nav2_loopback_sim create mode 100644 nav2_loopback_sim/setup.cfg create mode 100644 nav2_loopback_sim/setup.py create mode 100644 nav2_loopback_sim/test/test_copyright.py create mode 100644 nav2_loopback_sim/test/test_flake8.py create mode 100644 nav2_loopback_sim/test/test_pep257.py diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index f39a0b8e88..eee19f6b36 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -48,6 +48,7 @@ def generate_launch_description(): use_composition = LaunchConfiguration('use_composition') use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') + use_localization = LaunchConfiguration('use_localization') # Map fully qualified names to relative ones so the node's namespace can be prepended. # In case of the transforms (tf), currently, there doesn't seem to be a better alternative @@ -99,6 +100,11 @@ def generate_launch_description(): 'map', default_value='', description='Full path to map yaml file to load' ) + declare_use_localization_cmd = DeclareLaunchArgument( + 'use_localization', default_value='True', + description='Whether to enable localization or not' + ) + declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', @@ -151,7 +157,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(launch_dir, 'slam_launch.py') ), - condition=IfCondition(slam), + condition=IfCondition(PythonExpression([slam, ' and ', use_localization])), launch_arguments={ 'namespace': namespace, 'use_sim_time': use_sim_time, @@ -164,7 +170,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(launch_dir, 'localization_launch.py') ), - condition=IfCondition(PythonExpression(['not ', slam])), + condition=IfCondition(PythonExpression(['not ', slam, ' and ', use_localization])), launch_arguments={ 'namespace': namespace, 'map': map_yaml_file, @@ -210,6 +216,7 @@ def generate_launch_description(): ld.add_action(declare_use_composition_cmd) ld.add_action(declare_use_respawn_cmd) ld.add_action(declare_log_level_cmd) + ld.add_action(declare_use_localization_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) diff --git a/nav2_bringup/launch/tb3_loopback_simulation.launch.py b/nav2_bringup/launch/tb3_loopback_simulation.launch.py new file mode 100644 index 0000000000..9e598e854c --- /dev/null +++ b/nav2_bringup/launch/tb3_loopback_simulation.launch.py @@ -0,0 +1,224 @@ +# Copyright (C) 2024 Open Navigation LLC +# +# 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. + +"""This is all-in-one launch script intended for use by nav2 developers.""" + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetParameter +from launch_ros.descriptions import ParameterFile + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + loopback_sim_dir = get_package_share_directory('nav2_loopback_sim') + launch_dir = os.path.join(bringup_dir, 'launch') + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + + # Create the launch configuration variables + namespace = LaunchConfiguration('namespace') + use_namespace = LaunchConfiguration('use_namespace') + map_yaml_file = LaunchConfiguration('map') + params_file = LaunchConfiguration('params_file') + autostart = LaunchConfiguration('autostart') + use_composition = LaunchConfiguration('use_composition') + use_respawn = LaunchConfiguration('use_respawn') + + # Launch configuration variables specific to simulation + rviz_config_file = LaunchConfiguration('rviz_config_file') + use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') + use_rviz = LaunchConfiguration('use_rviz') + + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', default_value='', description='Top-level namespace' + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + 'use_namespace', + default_value='false', + description='Whether to apply a namespace to the navigation stack', + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'), + ) + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', + default_value='True', + description='Whether to use composed bringup', + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config_file', + default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), + description='Full path to the RVIZ config file to use', + ) + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + 'use_robot_state_pub', + default_value='True', + description='Whether to start the robot state publisher', + ) + + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) + + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + + start_robot_state_publisher_cmd = Node( + condition=IfCondition(use_robot_state_pub), + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + namespace=namespace, + output='screen', + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], + remappings=remappings, + ) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={ + 'namespace': namespace, + 'use_namespace': use_namespace, + 'use_sim_time': 'True', + 'rviz_config': rviz_config_file, + }.items(), + ) + + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), + launch_arguments={ + 'namespace': namespace, + 'use_namespace': use_namespace, + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'autostart': autostart, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'use_localization': 'False', # Don't use SLAM, AMCL + }.items(), + ) + + loopback_sim_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(loopback_sim_dir, 'loopback_simulation.launch.py')), + launch_arguments={ + 'params_file': params_file, + }.items(), + ) + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + convert_types=True, + ), + allow_substs=True, + ) + + start_map_server = GroupAction( + actions=[ + SetParameter('use_sim_time', True), + Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params, {'yaml_filename': map_yaml_file}], + remappings=remappings, + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_map_server', + output='screen', + parameters=[ + configured_params, + {'autostart': autostart}, {'node_names': ['map_server']}], + ), + ] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_use_respawn_cmd) + + # Add the actions to launch all of the navigation nodes + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(start_map_server) + ld.add_action(loopback_sim_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + + return ld diff --git a/nav2_bringup/launch/tb4_loopback_simulation.launch.py b/nav2_bringup/launch/tb4_loopback_simulation.launch.py new file mode 100644 index 0000000000..0ad911ddc7 --- /dev/null +++ b/nav2_bringup/launch/tb4_loopback_simulation.launch.py @@ -0,0 +1,232 @@ +# Copyright (C) 2024 Open Navigation LLC +# +# 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. + +"""This is all-in-one launch script intended for use by nav2 developers.""" + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, LaunchConfiguration +from launch_ros.actions import Node, SetParameter +from launch_ros.descriptions import ParameterFile + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + loopback_sim_dir = get_package_share_directory('nav2_loopback_sim') + launch_dir = os.path.join(bringup_dir, 'launch') + desc_dir = get_package_share_directory('nav2_minimal_tb4_description') + + # Create the launch configuration variables + namespace = LaunchConfiguration('namespace') + use_namespace = LaunchConfiguration('use_namespace') + map_yaml_file = LaunchConfiguration('map') + params_file = LaunchConfiguration('params_file') + autostart = LaunchConfiguration('autostart') + use_composition = LaunchConfiguration('use_composition') + use_respawn = LaunchConfiguration('use_respawn') + + # Launch configuration variables specific to simulation + rviz_config_file = LaunchConfiguration('rviz_config_file') + use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') + use_rviz = LaunchConfiguration('use_rviz') + + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', default_value='', description='Top-level namespace' + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + 'use_namespace', + default_value='false', + description='Whether to apply a namespace to the navigation stack', + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + default_value=os.path.join(bringup_dir, 'maps', 'depot.yaml'), # Try warehouse.yaml! + description='Full path to map file to load', + ) + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', + default_value='True', + description='Whether to use composed bringup', + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config_file', + default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), + description='Full path to the RVIZ config file to use', + ) + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + 'use_robot_state_pub', + default_value='True', + description='Whether to start the robot state publisher', + ) + + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) + + sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro') + start_robot_state_publisher_cmd = Node( + condition=IfCondition(use_robot_state_pub), + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + namespace=namespace, + output='screen', + parameters=[ + {'use_sim_time': True, 'robot_description': Command(['xacro', ' ', sdf])} + ], + remappings=remappings, + ) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={ + 'namespace': namespace, + 'use_namespace': use_namespace, + 'use_sim_time': 'True', + 'rviz_config': rviz_config_file, + }.items(), + ) + + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), + launch_arguments={ + 'namespace': namespace, + 'use_namespace': use_namespace, + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'autostart': autostart, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'use_localization': 'False', # Don't use SLAM, AMCL + }.items(), + ) + + loopback_sim_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(loopback_sim_dir, 'loopback_simulation.launch.py')), + launch_arguments={ + 'params_file': params_file, + 'scan_frame_id': 'rplidar_link', + }.items(), + ) + + static_publisher_cmd = Node( + package='tf2_ros', + executable='static_transform_publisher', + arguments=[ + '0.0', '0.0', '0.0', '0', '0', '0', + 'base_footprint', 'base_link'] + ) + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + convert_types=True, + ), + allow_substs=True, + ) + + start_map_server = GroupAction( + actions=[ + SetParameter('use_sim_time', True), + Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params, {'yaml_filename': map_yaml_file}], + remappings=remappings, + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_map_server', + output='screen', + parameters=[ + configured_params, + {'autostart': autostart}, {'node_names': ['map_server']}], + ), + ] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_use_respawn_cmd) + + # Add the actions to launch all of the navigation nodes + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(static_publisher_cmd) + ld.add_action(start_map_server) + ld.add_action(loopback_sim_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + + return ld diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 307eb2f37a..2581635e7f 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -419,3 +419,11 @@ docking_server: k_delta: 2.0 v_linear_min: 0.15 v_linear_max: 0.15 + +loopback_simulator: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + map_frame_id: "map" + scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' + update_duration: 0.02 diff --git a/nav2_loopback_sim/README.md b/nav2_loopback_sim/README.md new file mode 100644 index 0000000000..1e1aec6569 --- /dev/null +++ b/nav2_loopback_sim/README.md @@ -0,0 +1,51 @@ +# Nav2 Loopback Simulation + +The Nav2 loopback simulator is a stand-alone simulator to create a "loopback" for non-physical simulation to replace robot hardware, physics simulators (Gazebo, Bullet, Isaac Sim, etc). It computes the robot's odometry based on the command velocity's output request to create a perfect 'frictionless plane'-style simulation for unit testing, system testing, R&D on higher level systems, and testing behaviors without concerning yourself with localization accuracy or system dynamics. + +This was created by Steve Macenski of [Open Navigation LLC](https://opennav.org) and donated to Nav2 by the support of our project sponsors. If you rely on Nav2, please consider supporting the project! + +**⚠️ If you need professional services related to Nav2, please contact [Open Navigation](https://www.opennav.org/) at info@opennav.org.** + +It is drop-in replacable with AMR simulators and global localization by providing: +- Map -> Odom transform +- Odom -> Base Link transform, `nav_msgs/Odometry` odometry +- Accepts the standard `/initialpose` topic for transporting the robot to another location + +Note: This does not provide sensor data, so it is required that the global (and probably local) costmap contain the `StaticLayer` to avoid obstacles. + +It is convenient to be able to test systems by being able to: +- Arbitrarily transport the robot to any location and accurately navigate without waiting for a particle filter to converge for testing behaviors and reproducing higher-level issues +- Write unit or system tests on areas that are not dependent on low-level controller or localization performance without needing to spin up a compute-heavy process like Gazebo or Isaac Sim to provide odometry and sensor data, such as global planning, autonomy behavior trees, etc +- Perform R&D on various sensitive systems easily without concerning yourself with the errors accumulated with localization performance or imperfect dynamic models to get a proof of concept started +- Simulate N robots simultaneously with a lower compute footprint +- When otherwise highly compute constrained and need to simulate a robotic system + +## How to Use + +``` +ros2 run nav2_loopback_sim loopback_simulator # As a node, if in simulation +ros2 launch nav2_loopback_sim loopback_simulation.launch.py # As a launch file +ros2 launch nav2_bringup tb3_loopback_simulation.launch.py # Nav2 integrated navigation demo using it +ros2 launch nav2_bringup tb4_loopback_simulation.launch.py # Nav2 integrated navigation demo using it +``` + +## API + +### Parameters + +- `update_duration`: the duration between updates (default 0.01 -- 100hz) +- `base_frame_id`: The base frame to use (default `base_link`) +- `odom_frame_id`: The odom frame to use (default `odom`) +- `map_frame_id`: The map frame to use (default `map`) +- `scan_frame_id`: The can frame to use to publish a scan to keep the collision monitor fed and happy (default `base_scan` for TB3, `rplidar_link` for TB4) + +### Topics + +This node subscribes to: +- `initialpose`: To set the initial robot pose or relocalization request analog to other localization systems +- `cmd_vel`: Nav2's output twist to get the commanded velocity + +This node publishes: +- `odom`: To publish odometry from twist +- `tf`: To publish map->odom and odom->base_link transforms +- `scan`: To publish a bogus max range laser scan sensor to make the collision monitor happy diff --git a/nav2_loopback_sim/launch/loopback_simulation.launch.py b/nav2_loopback_sim/launch/loopback_simulation.launch.py new file mode 100644 index 0000000000..0124cb848f --- /dev/null +++ b/nav2_loopback_sim/launch/loopback_simulation.launch.py @@ -0,0 +1,52 @@ +# Copyright (c) 2024 Open Navigation LLC +# +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + bringup_dir = get_package_share_directory('nav2_bringup') + params_file = LaunchConfiguration('params_file') + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) + + scan_frame_id = LaunchConfiguration('scan_frame_id') + declare_scan_frame_id_cmd = DeclareLaunchArgument( + 'scan_frame_id', + default_value='base_scan', + ) + + loopback_sim_cmd = Node( + package='nav2_loopback_sim', + executable='loopback_simulator', + name='loopback_simulator', + output='screen', + parameters=[params_file, {'scan_frame_id': scan_frame_id}], + ) + + ld = LaunchDescription() + ld.add_action(declare_scan_frame_id_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(loopback_sim_cmd) + return ld diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py new file mode 100644 index 0000000000..dd476aa986 --- /dev/null +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -0,0 +1,223 @@ +# Copyright (c) 2024 Open Navigation LLC +# +# 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. + + +import math + +from geometry_msgs.msg import PoseWithCovarianceStamped, Twist +from geometry_msgs.msg import Quaternion, TransformStamped, Vector3 +from nav_msgs.msg import Odometry +import rclpy +from rclpy.duration import Duration +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from sensor_msgs.msg import LaserScan +from tf2_ros import TransformBroadcaster +import tf_transformations + +from .utils import addYawToQuat, matrixToTransform, transformStampedToMatrix + + +""" +This is a loopback simulator that replaces a physics simulator to create a +frictionless, inertialess, and collisionless simulation environment. It +accepts cmd_vel messages and publishes odometry & TF messages based on the +cumulative velocities received to mimick global localization and simulation. +It also accepts initialpose messages to set the initial pose of the robot +to place anywhere. +""" + + +class LoopbackSimulator(Node): + + def __init__(self): + super().__init__(node_name='loopback_simulator') + self.curr_cmd_vel = None + self.curr_cmd_vel_time = self.get_clock().now() + self.initial_pose = None + self.timer = None + self.setupTimer = None + + self.declare_parameter('update_duration', 0.01) + self.update_dur = self.get_parameter('update_duration').get_parameter_value().double_value + + self.declare_parameter('base_frame_id', 'base_footprint') + self.base_frame_id = self.get_parameter('base_frame_id').get_parameter_value().string_value + + self.declare_parameter('map_frame_id', 'map') + self.map_frame_id = self.get_parameter('map_frame_id').get_parameter_value().string_value + + self.declare_parameter('odom_frame_id', 'odom') + self.odom_frame_id = self.get_parameter('odom_frame_id').get_parameter_value().string_value + + self.declare_parameter('scan_frame_id', 'base_scan') + self.scan_frame_id = self.get_parameter('scan_frame_id').get_parameter_value().string_value + + self.t_map_to_odom = TransformStamped() + self.t_map_to_odom.header.frame_id = self.map_frame_id + self.t_map_to_odom.child_frame_id = self.odom_frame_id + self.t_odom_to_base_link = TransformStamped() + self.t_odom_to_base_link.header.frame_id = self.odom_frame_id + self.t_odom_to_base_link.child_frame_id = self.base_frame_id + + self.tf_broadcaster = TransformBroadcaster(self) + + self.initial_pose_sub = self.create_subscription( + PoseWithCovarianceStamped, + 'initialpose', self.initialPoseCallback, 10) + self.cmd_vel_sub = self.create_subscription( + Twist, + 'cmd_vel', self.cmdVelCallback, 10) + self.odom_pub = self.create_publisher(Odometry, 'odom', 10) + + sensor_qos = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, + depth=10) + self.scan_pub = self.create_publisher(LaserScan, 'scan', sensor_qos) + + self.setupTimer = self.create_timer(0.1, self.setupTimerCallback) + self.info('Loopback simulator initialized') + + def setupTimerCallback(self): + # Publish initial identity odom transform & laser scan to warm up system + self.tf_broadcaster.sendTransform(self.t_odom_to_base_link) + self.publishLaserScan() + + def cmdVelCallback(self, msg): + self.debug('Received cmd_vel') + if self.initial_pose is None: + # Don't consider velocities before the initial pose is set + return + self.curr_cmd_vel = msg + self.curr_cmd_vel_time = self.get_clock().now() + + def initialPoseCallback(self, msg): + self.info('Received initial pose!') + if self.initial_pose is None: + # Initialize transforms (map->odom as input pose, odom->base_link start from identity) + self.initial_pose = msg.pose.pose + self.t_map_to_odom.transform.translation.x = self.initial_pose.position.x + self.t_map_to_odom.transform.translation.y = self.initial_pose.position.y + self.t_map_to_odom.transform.translation.z = 0.0 + self.t_map_to_odom.transform.rotation = self.initial_pose.orientation + self.t_odom_to_base_link.transform.translation = Vector3() + self.t_odom_to_base_link.transform.rotation = Quaternion() + self.publishTransforms(self.t_map_to_odom, self.t_odom_to_base_link) + + # Start republication timer and velocity processing + if self.setupTimer is not None: + self.setupTimer.cancel() + self.setupTimer.destroy() + self.setupTimer = None + self.timer = self.create_timer(self.update_dur, self.timerCallback) + return + + self.initial_pose = msg.pose.pose + + # Adjust map->odom transform based on new initial pose, keeping odom->base_link the same + t_map_to_base_link = TransformStamped() + t_map_to_base_link.header = msg.header + t_map_to_base_link.child_frame_id = 'base_link' + t_map_to_base_link.transform.translation.x = self.initial_pose.position.x + t_map_to_base_link.transform.translation.y = self.initial_pose.position.y + t_map_to_base_link.transform.translation.z = 0.0 + t_map_to_base_link.transform.rotation = self.initial_pose.orientation + mat_map_to_base_link = transformStampedToMatrix(t_map_to_base_link) + mat_odom_to_base_link = transformStampedToMatrix(self.t_odom_to_base_link) + mat_base_link_to_odom = tf_transformations.inverse_matrix(mat_odom_to_base_link) + mat_map_to_odom = \ + tf_transformations.concatenate_matrices(mat_map_to_base_link, mat_base_link_to_odom) + self.t_map_to_odom.transform = matrixToTransform(mat_map_to_odom) + + def timerCallback(self): + # If no data, just republish existing transforms without change + one_sec = Duration(seconds=1) + if self.curr_cmd_vel is None or self.get_clock().now() - self.curr_cmd_vel_time > one_sec: + self.publishTransforms(self.t_map_to_odom, self.t_odom_to_base_link) + self.curr_cmd_vel = None + return + + # Update odom->base_link from cmd_vel + dx = self.curr_cmd_vel.linear.x * self.update_dur + dy = self.curr_cmd_vel.linear.y * self.update_dur + dth = self.curr_cmd_vel.angular.z * self.update_dur + q = [self.t_odom_to_base_link.transform.rotation.x, + self.t_odom_to_base_link.transform.rotation.y, + self.t_odom_to_base_link.transform.rotation.z, + self.t_odom_to_base_link.transform.rotation.w] + _, _, yaw = tf_transformations.euler_from_quaternion(q) + self.t_odom_to_base_link.transform.translation.x += dx * math.cos(yaw) - dy * math.sin(yaw) + self.t_odom_to_base_link.transform.translation.y += dx * math.sin(yaw) + dy * math.cos(yaw) + self.t_odom_to_base_link.transform.rotation = \ + addYawToQuat(self.t_odom_to_base_link.transform.rotation, dth) + + self.publishTransforms(self.t_map_to_odom, self.t_odom_to_base_link) + self.publishOdometry(self.t_odom_to_base_link) + self.publishLaserScan() + + def publishLaserScan(self): + # Publish a bogus laser scan for collision monitor + scan = LaserScan() + # scan.header.stamp = (self.get_clock().now()).to_msg() + scan.header.frame_id = self.scan_frame_id + scan.angle_min = -math.pi + scan.angle_max = math.pi + scan.angle_increment = 0.005817705996 # 0.333 degrees + scan.time_increment = 0.0 + scan.scan_time = 0.1 + scan.range_min = 0.1 + scan.range_max = 100.0 + num_samples = int((scan.angle_max - scan.angle_min) / scan.angle_increment) + scan.ranges = [scan.range_max - 0.01] * num_samples + self.scan_pub.publish(scan) + + def publishTransforms(self, map_to_odom, odom_to_base_link): + map_to_odom.header.stamp = \ + (self.get_clock().now() + Duration(seconds=self.update_dur)).to_msg() + odom_to_base_link.header.stamp = self.get_clock().now().to_msg() + self.tf_broadcaster.sendTransform(map_to_odom) + self.tf_broadcaster.sendTransform(odom_to_base_link) + + def publishOdometry(self, odom_to_base_link): + odom = Odometry() + odom.header.stamp = self.get_clock().now().to_msg() + odom.header.frame_id = 'odom' + odom.child_frame_id = 'base_link' + odom.pose.pose.position.x = odom_to_base_link.transform.translation.x + odom.pose.pose.position.y = odom_to_base_link.transform.translation.y + odom.pose.pose.orientation = odom_to_base_link.transform.rotation + odom.twist.twist = self.curr_cmd_vel + self.odom_pub.publish(odom) + + def info(self, msg): + self.get_logger().info(msg) + return + + def debug(self, msg): + self.get_logger().debug(msg) + return + + +def main(): + rclpy.init() + loopback_simulator = LoopbackSimulator() + rclpy.spin(loopback_simulator) + loopback_simulator.destroy_node() + rclpy.shutdown() + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py new file mode 100644 index 0000000000..0ed85689dd --- /dev/null +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -0,0 +1,65 @@ +# Copyright (c) 2024 Open Navigation LLC +# +# 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. + + +from geometry_msgs.msg import Quaternion, Transform +import numpy as np +import tf_transformations + + +""" +Transformation utilities for the loopback simulator +""" + + +def addYawToQuat(quaternion, yaw_to_add): + q = [quaternion.x, quaternion.y, quaternion.z, quaternion.w] + q2 = tf_transformations.quaternion_from_euler(0.0, 0.0, yaw_to_add) + q_new = tf_transformations.quaternion_multiply(q, q2) + new_quaternion = Quaternion() + new_quaternion.x = q_new[0] + new_quaternion.y = q_new[1] + new_quaternion.z = q_new[2] + new_quaternion.w = q_new[3] + return new_quaternion + + +def transformStampedToMatrix(transform): + translation = transform.transform.translation + rotation = transform.transform.rotation + matrix = np.eye(4) + matrix[0, 3] = translation.x + matrix[1, 3] = translation.y + matrix[2, 3] = translation.z + rotation_matrix = tf_transformations.quaternion_matrix([ + rotation.x, + rotation.y, + rotation.z, + rotation.w + ]) + matrix[:3, :3] = rotation_matrix[:3, :3] + return matrix + + +def matrixToTransform(matrix): + transform = Transform() + transform.translation.x = matrix[0, 3] + transform.translation.y = matrix[1, 3] + transform.translation.z = matrix[2, 3] + quaternion = tf_transformations.quaternion_from_matrix(matrix) + transform.rotation.x = quaternion[0] + transform.rotation.y = quaternion[1] + transform.rotation.z = quaternion[2] + transform.rotation.w = quaternion[3] + return transform diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml new file mode 100644 index 0000000000..a2a253def9 --- /dev/null +++ b/nav2_loopback_sim/package.xml @@ -0,0 +1,25 @@ + + + + nav2_loopback_sim + 1.3.1 + A loopback simulator to replace physics simulation + steve macenski + Apache-2.0 + + rclpy + geometry_msgs + nav_msgs + tf_transformations + tf2_ros + python3-transforms3d + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/nav2_loopback_sim/pytest.ini b/nav2_loopback_sim/pytest.ini new file mode 100644 index 0000000000..fe55d2ed64 --- /dev/null +++ b/nav2_loopback_sim/pytest.ini @@ -0,0 +1,2 @@ +[pytest] +junit_family=xunit2 diff --git a/nav2_loopback_sim/resource/nav2_loopback_sim b/nav2_loopback_sim/resource/nav2_loopback_sim new file mode 100644 index 0000000000..e69de29bb2 diff --git a/nav2_loopback_sim/setup.cfg b/nav2_loopback_sim/setup.cfg new file mode 100644 index 0000000000..20a46b06d6 --- /dev/null +++ b/nav2_loopback_sim/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/nav2_loopback_sim +[install] +install_scripts=$base/lib/nav2_loopback_sim diff --git a/nav2_loopback_sim/setup.py b/nav2_loopback_sim/setup.py new file mode 100644 index 0000000000..0d56733614 --- /dev/null +++ b/nav2_loopback_sim/setup.py @@ -0,0 +1,30 @@ +from glob import glob +import os + +from setuptools import setup + + +package_name = 'nav2_loopback_sim' + +setup( + name=package_name, + version='1.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name), glob('launch/*')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='stevemacenski', + maintainer_email='steve@opennav.org', + description='A loopback simulator to replace physics simulation', + license='Apache-2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'loopback_simulator = nav2_loopback_sim.loopback_simulator:main', + ], + }, +) diff --git a/nav2_loopback_sim/test/test_copyright.py b/nav2_loopback_sim/test/test_copyright.py new file mode 100644 index 0000000000..cc8ff03f79 --- /dev/null +++ b/nav2_loopback_sim/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/nav2_loopback_sim/test/test_flake8.py b/nav2_loopback_sim/test/test_flake8.py new file mode 100644 index 0000000000..26030113cc --- /dev/null +++ b/nav2_loopback_sim/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, 'Found %d code style errors / warnings:\n' % len( + errors + ) + '\n'.join(errors) diff --git a/nav2_loopback_sim/test/test_pep257.py b/nav2_loopback_sim/test/test_pep257.py new file mode 100644 index 0000000000..b234a3840f --- /dev/null +++ b/nav2_loopback_sim/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From 22d24021e21d4ade7c19fd65de006d2cbae22e3f Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 19 Aug 2024 10:33:51 -0700 Subject: [PATCH 09/21] readding namespaced rviz config (#4625) Signed-off-by: Steve Macenski --- nav2_bringup/rviz/nav2_namespaced_view.rviz | 379 ++++++++++++++++++++ 1 file changed, 379 insertions(+) create mode 100644 nav2_bringup/rviz/nav2_namespaced_view.rviz diff --git a/nav2_bringup/rviz/nav2_namespaced_view.rviz b/nav2_bringup/rviz/nav2_namespaced_view.rviz new file mode 100644 index 0000000000..4dcd23e24b --- /dev/null +++ b/nav2_bringup/rviz/nav2_namespaced_view.rviz @@ -0,0 +1,379 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 195 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /RobotModel1/Status1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Global Planner1/Global Costmap1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 464 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Use Timestamp: false + Value: true + - Alpha: 1 + Class: nav2_rviz_plugins/ParticleCloud + Color: 0; 180; 0 + Enabled: true + Max Arrow Length: 0.3 + Min Arrow Length: 0.02 + Name: Amcl Particle Swarm + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particle_cloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + Enabled: true + Name: Controller + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waypoints + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -1.5708 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 160 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 914 + Hide Left Dock: false + Hide Right Dock: true + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000338fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000003070000006e0000006200ffffff000000010000010f00000338fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000338000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000033800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1545 + X: 186 + Y: 117 + From fe639ed961c534de4e1d2040d1512adfb24da5fe Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 20 Aug 2024 11:26:17 -0400 Subject: [PATCH 10/21] Switch dwb_plugins to modern CMake idioms. (#4629) Signed-off-by: Chris Lalancette --- .../dwb_plugins/CMakeLists.txt | 75 ++++++++++++------- .../include/dwb_plugins/velocity_iterator.hpp | 1 - nav2_dwb_controller/dwb_plugins/package.xml | 10 ++- .../dwb_plugins/test/CMakeLists.txt | 25 ++++++- 4 files changed, 76 insertions(+), 35 deletions(-) diff --git a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt index a1806c524a..75a00b9b7f 100644 --- a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt @@ -2,38 +2,41 @@ cmake_minimum_required(VERSION 3.5) project(dwb_plugins) find_package(ament_cmake REQUIRED) -find_package(nav2_common REQUIRED) -find_package(angles REQUIRED) find_package(dwb_core REQUIRED) +find_package(dwb_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_util REQUIRED) find_package(nav_2d_msgs REQUIRED) find_package(nav_2d_utils REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) -find_package(nav2_util REQUIRED) +find_package(rcl_interfaces REQUIRED) nav2_package() -set(dependencies - angles - dwb_core - nav_2d_msgs - nav_2d_utils - pluginlib - rclcpp - nav2_util +add_library(standard_traj_generator SHARED + src/standard_traj_generator.cpp + src/limited_accel_generator.cpp + src/kinematic_parameters.cpp + src/xy_theta_iterator.cpp) +target_include_directories(standard_traj_generator PUBLIC + "$" + "$" ) - -include_directories( - include +target_link_libraries(standard_traj_generator PUBLIC + dwb_core::dwb_core + ${dwb_msgs_TARGETS} + ${geometry_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_2d_msgs_TARGETS} + rclcpp::rclcpp + ${rcl_interfaces_TARGETS} +) +target_link_libraries(standard_traj_generator PRIVATE + nav_2d_utils::conversions + pluginlib::pluginlib ) - -add_library(standard_traj_generator SHARED - src/standard_traj_generator.cpp - src/limited_accel_generator.cpp - src/kinematic_parameters.cpp - src/xy_theta_iterator.cpp) -ament_target_dependencies(standard_traj_generator ${dependencies}) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -42,23 +45,39 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) + ament_find_gtest() + + find_package(nav2_costmap_2d REQUIRED) + find_package(rclcpp_lifecycle REQUIRED) add_subdirectory(test) endif() install(TARGETS standard_traj_generator - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) install(FILES plugins.xml - DESTINATION share/${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME} ) -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(standard_traj_generator) +ament_export_dependencies( + dwb_core + dwb_msgs + geometry_msgs + nav2_util + nav_2d_msgs + rclcpp + rcl_interfaces +) +ament_export_targets(${PROJECT_NAME}) + pluginlib_export_plugin_description_file(dwb_core plugins.xml) ament_package() diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp index 55a663fddb..e5feefc9fa 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp @@ -38,7 +38,6 @@ #include #include -#include "rclcpp/rclcpp.hpp" #include "nav_2d_msgs/msg/twist2_d.hpp" #include "dwb_plugins/kinematic_parameters.hpp" #include "nav2_util/lifecycle_node.hpp" diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 86311cfb56..3e0b0c941e 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -12,17 +12,21 @@ ament_cmake nav2_common - angles dwb_core + dwb_msgs + geometry_msgs + nav2_util nav_2d_msgs nav_2d_utils pluginlib rclcpp - nav2_util + rcl_interfaces + ament_cmake_gtest ament_lint_common ament_lint_auto - ament_cmake_gtest + nav2_costmap_2d + rclcpp_lifecycle ament_cmake diff --git a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt index c5b9308e8d..a785393843 100644 --- a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt @@ -1,10 +1,29 @@ ament_add_gtest(vtest velocity_iterator_test.cpp) +target_link_libraries(vtest standard_traj_generator) ament_add_gtest(twist_gen_test twist_gen.cpp) -target_link_libraries(twist_gen_test standard_traj_generator) +target_link_libraries(twist_gen_test + standard_traj_generator + dwb_core::dwb_core + ${dwb_msgs_TARGETS} + ${geometry_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_2d_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) ament_add_gtest(kinematic_parameters_test kinematic_parameters_test.cpp) -target_link_libraries(kinematic_parameters_test standard_traj_generator) +target_link_libraries(kinematic_parameters_test + standard_traj_generator + rclcpp::rclcpp + ${rcl_interfaces_TARGETS} +) ament_add_gtest(speed_limit_test speed_limit_test.cpp) -target_link_libraries(speed_limit_test standard_traj_generator) +target_link_libraries(speed_limit_test + standard_traj_generator + nav2_costmap_2d::nav2_costmap_2d_core + nav2_util::nav2_util_core + rclcpp::rclcpp +) From 4f3dc6812d69d4a348bf002ae94a72e84e5d0c40 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 20 Aug 2024 13:32:14 -0400 Subject: [PATCH 11/21] Switch opennav_docking_core to modern CMake idioms. (#4630) Signed-off-by: Chris Lalancette --- .../opennav_docking_core/CMakeLists.txt | 38 +++++++++++++------ .../opennav_docking_core/charging_dock.hpp | 3 +- nav2_docking/opennav_docking_core/package.xml | 8 ++-- 3 files changed, 31 insertions(+), 18 deletions(-) diff --git a/nav2_docking/opennav_docking_core/CMakeLists.txt b/nav2_docking/opennav_docking_core/CMakeLists.txt index 639319bc5e..3d62476eff 100644 --- a/nav2_docking/opennav_docking_core/CMakeLists.txt +++ b/nav2_docking/opennav_docking_core/CMakeLists.txt @@ -2,23 +2,34 @@ cmake_minimum_required(VERSION 3.5) project(opennav_docking_core) find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) -find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) -find_package(nav2_util REQUIRED) -find_package(nav2_msgs REQUIRED) +find_package(tf2_ros REQUIRED) nav2_package() -set(dependencies - rclcpp - rclcpp_lifecycle - nav2_msgs - nav2_util +add_library(opennav_docking_core INTERFACE) +target_include_directories(opennav_docking_core + INTERFACE + "$" + "$" +) +target_link_libraries(opennav_docking_core INTERFACE + ${geometry_msgs_TARGETS} + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros +) + +install(TARGETS opennav_docking_core + EXPORT opennav_docking_core + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) @@ -28,7 +39,12 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_export_include_directories(include) -ament_export_dependencies(${dependencies}) +ament_export_include_directories(include/${PROJECT_NAME}) +ament_export_dependencies( + geometry_msgs + rclcpp_lifecycle + tf2_ros +) +ament_export_targets(opennav_docking_core) ament_package() 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 b58f9408fb..d4566e5836 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 @@ -18,9 +18,8 @@ #include #include -#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav2_util/lifecycle_node.hpp" #include "tf2_ros/buffer.h" diff --git a/nav2_docking/opennav_docking_core/package.xml b/nav2_docking/opennav_docking_core/package.xml index bcd03d786b..2df639d516 100644 --- a/nav2_docking/opennav_docking_core/package.xml +++ b/nav2_docking/opennav_docking_core/package.xml @@ -8,16 +8,14 @@ Apache-2.0 ament_cmake + nav2_common - rclcpp + geometry_msgs rclcpp_lifecycle - nav2_util - nav2_msgs + tf2_ros ament_lint_common ament_lint_auto - ament_cmake_gtest - ament_cmake_pytest ament_cmake From ebd9ee176cfa2e368ea7494ac4df825816399e1d Mon Sep 17 00:00:00 2001 From: Jatin Patil <89979346+JatinPatil2003@users.noreply.github.com> Date: Wed, 21 Aug 2024 00:17:51 +0530 Subject: [PATCH 12/21] Added laser data from map in nav2_loopback_sim (#4617) * Added laser data from map Signed-off-by: Jatin Patil * Fixed Linting Signed-off-by: Jatin Patil * Fixed Linting Signed-off-by: Jatin Patil * Fixed few requested changes Signed-off-by: Jatin Patil * Fixed linting Signed-off-by: Jatin Patil * Requested changes Signed-off-by: Jatin Patil * Linting Signed-off-by: Jatin Patil * Added parameters and fixed requested changes Signed-off-by: Jatin Patil * linting Signed-off-by: Jatin Patil * Added scan using LineIterator Signed-off-by: Jatin Patil * LineIterator max_distance or range_max Signed-off-by: Jatin Patil * min of max_distance or range_max Signed-off-by: Jatin Patil * final updates working correctly Signed-off-by: Jatin Patil * Fix lint Signed-off-by: Jatin Patil * requested changes Signed-off-by: Jatin Patil * Update nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py Signed-off-by: Steve Macenski * Update nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py Signed-off-by: Steve Macenski --------- Signed-off-by: Jatin Patil Signed-off-by: Steve Macenski Co-authored-by: Steve Macenski --- .../nav2_loopback_sim/loopback_simulator.py | 162 ++++++++++++++++-- nav2_loopback_sim/nav2_loopback_sim/utils.py | 12 ++ 2 files changed, 155 insertions(+), 19 deletions(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index dd476aa986..62e27e9743 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -17,17 +17,24 @@ from geometry_msgs.msg import PoseWithCovarianceStamped, Twist from geometry_msgs.msg import Quaternion, TransformStamped, Vector3 +from nav2_simple_commander.line_iterator import LineIterator from nav_msgs.msg import Odometry +from nav_msgs.srv import GetMap import rclpy from rclpy.duration import Duration from rclpy.node import Node from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy from sensor_msgs.msg import LaserScan -from tf2_ros import TransformBroadcaster +from tf2_ros import Buffer, TransformBroadcaster, TransformListener import tf_transformations -from .utils import addYawToQuat, matrixToTransform, transformStampedToMatrix - +from .utils import ( + addYawToQuat, + getMapOccupancy, + matrixToTransform, + transformStampedToMatrix, + worldToMap, +) """ This is a loopback simulator that replaces a physics simulator to create a @@ -48,6 +55,8 @@ def __init__(self): self.initial_pose = None self.timer = None self.setupTimer = None + self.map = None + self.mat_base_to_laser = None self.declare_parameter('update_duration', 0.01) self.update_dur = self.get_parameter('update_duration').get_parameter_value().double_value @@ -64,6 +73,14 @@ def __init__(self): self.declare_parameter('scan_frame_id', 'base_scan') self.scan_frame_id = self.get_parameter('scan_frame_id').get_parameter_value().string_value + self.declare_parameter('scan_publish_dur', 0.1) + self.scan_publish_dur = self.get_parameter( + 'scan_publish_dur').get_parameter_value().double_value + + self.declare_parameter('publish_map_odom_tf', True) + self.publish_map_odom_tf = self.get_parameter( + 'publish_map_odom_tf').get_parameter_value().bool_value + self.t_map_to_odom = TransformStamped() self.t_map_to_odom.header.frame_id = self.map_frame_id self.t_map_to_odom.child_frame_id = self.odom_frame_id @@ -88,12 +105,31 @@ def __init__(self): self.scan_pub = self.create_publisher(LaserScan, 'scan', sensor_qos) self.setupTimer = self.create_timer(0.1, self.setupTimerCallback) + + self.map_client = self.create_client(GetMap, '/map_server/map') + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + self.getMap() + self.info('Loopback simulator initialized') + def getBaseToLaserTf(self): + try: + # Wait for transform and lookup + transform = self.tf_buffer.lookup_transform( + self.base_frame_id, self.scan_frame_id, rclpy.time.Time()) + self.mat_base_to_laser = transformStampedToMatrix(transform) + + except Exception as ex: + self.get_logger().error('Transform lookup failed: %s' % str(ex)) + def setupTimerCallback(self): # Publish initial identity odom transform & laser scan to warm up system self.tf_broadcaster.sendTransform(self.t_odom_to_base_link) - self.publishLaserScan() + if self.mat_base_to_laser is None: + self.getBaseToLaserTf() def cmdVelCallback(self, msg): self.debug('Received cmd_vel') @@ -122,6 +158,7 @@ def initialPoseCallback(self, msg): self.setupTimer.destroy() self.setupTimer = None self.timer = self.create_timer(self.update_dur, self.timerCallback) + self.timer_laser = self.create_timer(self.scan_publish_dur, self.publishLaserScan) return self.initial_pose = msg.pose.pose @@ -165,29 +202,33 @@ def timerCallback(self): self.publishTransforms(self.t_map_to_odom, self.t_odom_to_base_link) self.publishOdometry(self.t_odom_to_base_link) - self.publishLaserScan() def publishLaserScan(self): # Publish a bogus laser scan for collision monitor - scan = LaserScan() - # scan.header.stamp = (self.get_clock().now()).to_msg() - scan.header.frame_id = self.scan_frame_id - scan.angle_min = -math.pi - scan.angle_max = math.pi - scan.angle_increment = 0.005817705996 # 0.333 degrees - scan.time_increment = 0.0 - scan.scan_time = 0.1 - scan.range_min = 0.1 - scan.range_max = 100.0 - num_samples = int((scan.angle_max - scan.angle_min) / scan.angle_increment) - scan.ranges = [scan.range_max - 0.01] * num_samples - self.scan_pub.publish(scan) + self.scan_msg = LaserScan() + self.scan_msg.header.stamp = (self.get_clock().now()).to_msg() + self.scan_msg.header.frame_id = self.scan_frame_id + self.scan_msg.angle_min = -math.pi + self.scan_msg.angle_max = math.pi + # 1.5 degrees + self.scan_msg.angle_increment = 0.0261799 + self.scan_msg.time_increment = 0.0 + self.scan_msg.scan_time = 0.1 + self.scan_msg.range_min = 0.05 + self.scan_msg.range_max = 30.0 + num_samples = int( + (self.scan_msg.angle_max - self.scan_msg.angle_min) / + self.scan_msg.angle_increment) + self.scan_msg.ranges = [0.0] * num_samples + self.getLaserScan(num_samples) + self.scan_pub.publish(self.scan_msg) def publishTransforms(self, map_to_odom, odom_to_base_link): map_to_odom.header.stamp = \ (self.get_clock().now() + Duration(seconds=self.update_dur)).to_msg() odom_to_base_link.header.stamp = self.get_clock().now().to_msg() - self.tf_broadcaster.sendTransform(map_to_odom) + if self.publish_map_odom_tf: + self.tf_broadcaster.sendTransform(map_to_odom) self.tf_broadcaster.sendTransform(odom_to_base_link) def publishOdometry(self, odom_to_base_link): @@ -209,6 +250,89 @@ def debug(self, msg): self.get_logger().debug(msg) return + def getMap(self): + request = GetMap.Request() + if self.map_client.wait_for_service(timeout_sec=5.0): + # Request to get map + future = self.map_client.call_async(request) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + self.map = future.result().map + self.get_logger().info('Laser scan will be populated using map data') + else: + self.get_logger().warn( + 'Failed to get map, ' + 'Laser scan will be populated using max range' + ) + else: + self.get_logger().warn( + 'Failed to get map, ' + 'Laser scan will be populated using max range' + ) + + def getLaserPose(self): + mat_map_to_odom = transformStampedToMatrix(self.t_map_to_odom) + mat_odom_to_base = transformStampedToMatrix(self.t_odom_to_base_link) + + mat_map_to_laser = tf_transformations.concatenate_matrices( + mat_map_to_odom, + mat_odom_to_base, + self.mat_base_to_laser + ) + transform = matrixToTransform(mat_map_to_laser) + + x = transform.translation.x + y = transform.translation.y + theta = tf_transformations.euler_from_quaternion([ + transform.rotation.x, + transform.rotation.y, + transform.rotation.z, + transform.rotation.w + ])[2] + + return x, y, theta + + def getLaserScan(self, num_samples): + if self.map is None or self.initial_pose is None or self.mat_base_to_laser is None: + self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples + return + + x0, y0, theta = self.getLaserPose() + + mx0, my0 = worldToMap(x0, y0, self.map) + + if not 0 < mx0 < self.map.info.width or not 0 < my0 < self.map.info.height: + # outside map + self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples + return + + for i in range(num_samples): + curr_angle = theta + self.scan_msg.angle_min + i * self.scan_msg.angle_increment + x1 = x0 + self.scan_msg.range_max * math.cos(curr_angle) + y1 = y0 + self.scan_msg.range_max * math.sin(curr_angle) + + mx1, my1 = worldToMap(x1, y1, self.map) + + line_iterator = LineIterator(mx0, my0, mx1, my1, 0.5) + + while line_iterator.isValid(): + mx, my = int(line_iterator.getX()), int(line_iterator.getY()) + + if not 0 < mx < self.map.info.width or not 0 < my < self.map.info.height: + # if outside map then check next ray + break + + point_cost = getMapOccupancy(mx, my, self.map) + + if point_cost >= 60: + self.scan_msg.ranges[i] = math.sqrt( + (int(line_iterator.getX()) - mx0) ** 2 + + (int(line_iterator.getY()) - my0) ** 2 + ) * self.map.info.resolution + break + + line_iterator.advance() + def main(): rclpy.init() diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py index 0ed85689dd..468103e94e 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/utils.py +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -13,6 +13,8 @@ # limitations under the License. +import math + from geometry_msgs.msg import Quaternion, Transform import numpy as np import tf_transformations @@ -63,3 +65,13 @@ def matrixToTransform(matrix): transform.rotation.z = quaternion[2] transform.rotation.w = quaternion[3] return transform + + +def worldToMap(world_x, world_y, map_msg): + map_x = int(math.floor((world_x - map_msg.info.origin.position.x) / map_msg.info.resolution)) + map_y = int(math.floor((world_y - map_msg.info.origin.position.y) / map_msg.info.resolution)) + return map_x, map_y + + +def getMapOccupancy(x, y, map_msg): + return map_msg.data[y * map_msg.info.width + x] From e722563bcad7bc2b9d86380b794055306d102dd2 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 20 Aug 2024 14:50:28 -0400 Subject: [PATCH 13/21] Switch opennav_docking_bt to modern CMake idioms. (#4631) Signed-off-by: Chris Lalancette --- .../opennav_docking_bt/CMakeLists.txt | 82 ++++++++++--------- nav2_docking/opennav_docking_bt/package.xml | 11 +-- .../opennav_docking_bt/test/CMakeLists.txt | 20 ++++- 3 files changed, 66 insertions(+), 47 deletions(-) diff --git a/nav2_docking/opennav_docking_bt/CMakeLists.txt b/nav2_docking/opennav_docking_bt/CMakeLists.txt index c89330ee38..a852c4124b 100644 --- a/nav2_docking/opennav_docking_bt/CMakeLists.txt +++ b/nav2_docking/opennav_docking_bt/CMakeLists.txt @@ -2,56 +2,56 @@ cmake_minimum_required(VERSION 3.5) project(opennav_docking_bt) find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(std_msgs REQUIRED) -find_package(nav2_util REQUIRED) -find_package(nav2_msgs REQUIRED) -find_package(nav2_core REQUIRED) +find_package(behaviortree_cpp REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(nav2_behavior_tree REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav2_util REQUIRED) find_package(nav_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(behaviortree_cpp REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) -# potentially replace with nav2_common, nav2_package() -set(CMAKE_CXX_STANDARD 17) -add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wshadow -Wnull-dereference) +nav2_package() -include_directories( - include +add_library(opennav_dock_action_bt_node SHARED src/dock_robot.cpp) +target_include_directories(opennav_dock_action_bt_node + PUBLIC + "$" + "$" ) - -set(dependencies - rclcpp - rclcpp_action - std_msgs - nav2_util - nav2_msgs - nav2_core - nav2_behavior_tree - nav_msgs - geometry_msgs - behaviortree_cpp +target_compile_definitions(opennav_dock_action_bt_node PRIVATE BT_PLUGIN_EXPORT) +target_link_libraries(opennav_dock_action_bt_node PUBLIC + behaviortree_cpp::behaviortree_cpp + ${geometry_msgs_TARGETS} + nav2_behavior_tree::nav2_behavior_tree + ${nav2_msgs_TARGETS} ) -add_library(opennav_dock_action_bt_node SHARED src/dock_robot.cpp) -list(APPEND plugin_libs opennav_dock_action_bt_node) add_library(opennav_undock_action_bt_node SHARED src/undock_robot.cpp) -list(APPEND plugin_libs opennav_undock_action_bt_node) - -foreach(bt_plugin ${plugin_libs}) - ament_target_dependencies(${bt_plugin} ${dependencies}) - target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) -endforeach() +target_include_directories(opennav_undock_action_bt_node + PUBLIC + "$" + "$" +) +target_compile_definitions(opennav_undock_action_bt_node PRIVATE BT_PLUGIN_EXPORT) +target_link_libraries(opennav_undock_action_bt_node PUBLIC + behaviortree_cpp::behaviortree_cpp + ${geometry_msgs_TARGETS} + nav2_behavior_tree::nav2_behavior_tree + ${nav2_msgs_TARGETS} +) -install(TARGETS ${plugin_libs} +install(TARGETS opennav_dock_action_bt_node opennav_undock_action_bt_node + EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY behavior_trees DESTINATION share/${PROJECT_NAME}) @@ -63,7 +63,13 @@ if(BUILD_TESTING) add_subdirectory(test) endif() -ament_export_include_directories(include) -ament_export_libraries(${plugin_libs}) -ament_export_dependencies(${dependencies}) +ament_export_include_directories(include/${PROJECT_NAME}) +ament_export_libraries(opennav_dock_action_bt_node opennav_undock_action_bt_node) +ament_export_dependencies( + behaviortree_cpp + geometry_msgs + nav2_behavior_tree + nav2_msgs +) +ament_export_targets(${PROJECT_NAME}) ament_package() diff --git a/nav2_docking/opennav_docking_bt/package.xml b/nav2_docking/opennav_docking_bt/package.xml index 85b90b1207..3778555116 100644 --- a/nav2_docking/opennav_docking_bt/package.xml +++ b/nav2_docking/opennav_docking_bt/package.xml @@ -8,16 +8,17 @@ Apache-2.0 ament_cmake + nav2_common - rclcpp - rclcpp_action + behaviortree_cpp + geometry_msgs nav2_behavior_tree - nav2_util nav2_core nav2_msgs + nav2_util nav_msgs - geometry_msgs - behaviortree_cpp + rclcpp + rclcpp_action ament_lint_common ament_lint_auto diff --git a/nav2_docking/opennav_docking_bt/test/CMakeLists.txt b/nav2_docking/opennav_docking_bt/test/CMakeLists.txt index ba0c374f8b..077e3a8581 100644 --- a/nav2_docking/opennav_docking_bt/test/CMakeLists.txt +++ b/nav2_docking/opennav_docking_bt/test/CMakeLists.txt @@ -1,9 +1,21 @@ # Cancel test ament_add_gtest(test_dock_robot test_dock_robot.cpp) -target_link_libraries(test_dock_robot opennav_dock_action_bt_node) -ament_target_dependencies(test_dock_robot ${dependencies}) +target_link_libraries(test_dock_robot + behaviortree_cpp::behaviortree_cpp + ${geometry_msgs_TARGETS} + ${nav2_msgs_TARGETS} + opennav_dock_action_bt_node + rclcpp::rclcpp + rclcpp_action::rclcpp_action +) # Make command test ament_add_gtest(test_undock_robot test_undock_robot.cpp) -target_link_libraries(test_undock_robot opennav_undock_action_bt_node) -ament_target_dependencies(test_undock_robot ${dependencies}) +target_link_libraries(test_undock_robot + behaviortree_cpp::behaviortree_cpp + ${geometry_msgs_TARGETS} + ${nav2_msgs_TARGETS} + opennav_undock_action_bt_node + rclcpp::rclcpp + rclcpp_action::rclcpp_action +) From 24a28d7ffea86a6a9eb301009d1cee603f80d113 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 20 Aug 2024 12:00:15 -0700 Subject: [PATCH 14/21] Making base frame ID for map to base link transform based on base frame ID parameter (#4632) Signed-off-by: Steve Macenski --- nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 62e27e9743..885a6725f5 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -166,7 +166,7 @@ def initialPoseCallback(self, msg): # Adjust map->odom transform based on new initial pose, keeping odom->base_link the same t_map_to_base_link = TransformStamped() t_map_to_base_link.header = msg.header - t_map_to_base_link.child_frame_id = 'base_link' + t_map_to_base_link.child_frame_id = self.base_frame_id t_map_to_base_link.transform.translation.x = self.initial_pose.position.x t_map_to_base_link.transform.translation.y = self.initial_pose.position.y t_map_to_base_link.transform.translation.z = 0.0 From 2e824e9221099a82b1ca0f53139f029f1eb17db5 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 20 Aug 2024 16:40:21 -0400 Subject: [PATCH 15/21] Switch opennav_docking to modern CMake idioms. (#4633) Signed-off-by: Chris Lalancette --- nav2_docking/opennav_docking/CMakeLists.txt | 163 +++++++++++------- .../include/opennav_docking/pose_filter.hpp | 1 - nav2_docking/opennav_docking/package.xml | 21 ++- .../opennav_docking/src/pose_filter.cpp | 1 + .../opennav_docking/test/CMakeLists.txt | 70 +++++--- 5 files changed, 156 insertions(+), 100 deletions(-) diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt index e50590d3e4..d991a25ba9 100644 --- a/nav2_docking/opennav_docking/CMakeLists.txt +++ b/nav2_docking/opennav_docking/CMakeLists.txt @@ -3,65 +3,45 @@ project(opennav_docking) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(std_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(visualization_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_common REQUIRED) find_package(nav2_graceful_controller REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(nav_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(builtin_interfaces REQUIRED) +find_package(opennav_docking_core REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rcl_interfaces REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) -find_package(pluginlib REQUIRED) find_package(yaml_cpp_vendor REQUIRED) find_package(yaml-cpp REQUIRED) -find_package(opennav_docking_core REQUIRED) -# potentially replace with nav2_common, nav2_package() -set(CMAKE_CXX_STANDARD 17) -add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wshadow -Wnull-dereference) - -include_directories( - include -) +nav2_package() set(executable_name opennav_docking) set(library_name ${executable_name}_core) -set(dependencies - angles - rclcpp - rclcpp_action - rclcpp_lifecycle - rclcpp_components - std_msgs - sensor_msgs - visualization_msgs - nav2_graceful_controller - nav2_util - nav2_msgs - nav_msgs - geometry_msgs - builtin_interfaces - tf2_ros - tf2_geometry_msgs - pluginlib - yaml_cpp_vendor - opennav_docking_core -) - add_library(controller SHARED src/controller.cpp ) - -ament_target_dependencies(controller - ${dependencies} +target_include_directories(controller + PUBLIC + "$" + "$" +) +target_link_libraries(controller PUBLIC + ${geometry_msgs_TARGETS} + nav2_graceful_controller::nav2_graceful_controller + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} ) add_library(${library_name} SHARED @@ -69,47 +49,87 @@ add_library(${library_name} SHARED src/dock_database.cpp src/navigator.cpp ) - -ament_target_dependencies(${library_name} - ${dependencies} +target_include_directories(${library_name} + PUBLIC + "$" + "$" ) - -target_link_libraries(${library_name} - yaml-cpp::yaml-cpp +target_link_libraries(${library_name} PUBLIC + angles::angles controller + ${geometry_msgs_TARGETS} + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + opennav_docking_core::opennav_docking_core + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_action::rclcpp_action + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + tf2_ros::tf2_ros + yaml-cpp::yaml-cpp +) +target_link_libraries(${library_name} PRIVATE + rclcpp_components::component + tf2_geometry_msgs::tf2_geometry_msgs ) add_library(pose_filter SHARED src/pose_filter.cpp ) - -ament_target_dependencies(pose_filter - ${dependencies} +target_include_directories(pose_filter + PUBLIC + "$" + "$" +) +target_link_libraries(pose_filter PUBLIC + ${geometry_msgs_TARGETS} +) +target_link_libraries(pose_filter PRIVATE + rclcpp::rclcpp + tf2_geometry_msgs::tf2_geometry_msgs ) add_executable(${executable_name} src/main.cpp ) - -target_link_libraries(${executable_name} ${library_name}) - -ament_target_dependencies(${executable_name} - ${dependencies} +target_include_directories(${executable_name} + PUBLIC + "$" + "$" ) +target_link_libraries(${executable_name} PRIVATE ${library_name} rclcpp::rclcpp) rclcpp_components_register_nodes(${library_name} "opennav_docking::DockingServer") add_library(simple_charging_dock SHARED src/simple_charging_dock.cpp ) -ament_target_dependencies(simple_charging_dock - ${dependencies} +target_include_directories(simple_charging_dock + PUBLIC + "$" + "$" +) +target_link_libraries(simple_charging_dock PUBLIC + ${geometry_msgs_TARGETS} + opennav_docking_core::opennav_docking_core + pose_filter + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${sensor_msgs_TARGETS} + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros +) +target_link_libraries(simple_charging_dock PRIVATE + nav2_util::nav2_util_core + pluginlib::pluginlib + tf2::tf2 ) -target_link_libraries(simple_charging_dock pose_filter) pluginlib_export_plugin_description_file(opennav_docking_core plugins.xml) install(TARGETS ${library_name} controller pose_filter simple_charging_dock + EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -120,7 +140,7 @@ install(TARGETS ${executable_name} ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) install(FILES test/test_dock_file.yaml @@ -131,11 +151,28 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) + find_package(ament_index_cpp REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_find_gtest() add_subdirectory(test) endif() -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(${library_name} controller pose_filter) -ament_export_dependencies(${dependencies} yaml-cpp) +ament_export_dependencies( + geometry_msgs + nav2_graceful_controller + nav2_msgs + nav2_util + opennav_docking_core + pluginlib + rclcpp + rclcpp_action + rclcpp_lifecycle + rcl_interfaces + tf2_ros + yaml-cpp +) +ament_export_targets(${PROJECT_NAME}) ament_package() diff --git a/nav2_docking/opennav_docking/include/opennav_docking/pose_filter.hpp b/nav2_docking/opennav_docking/include/opennav_docking/pose_filter.hpp index 50b538bfd7..99f8359b5e 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/pose_filter.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/pose_filter.hpp @@ -16,7 +16,6 @@ #define OPENNAV_DOCKING__POSE_FILTER_HPP_ #include "geometry_msgs/msg/pose_stamped.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace opennav_docking { diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml index 4c5ac1789b..bb80298041 100644 --- a/nav2_docking/opennav_docking/package.xml +++ b/nav2_docking/opennav_docking/package.xml @@ -8,25 +8,30 @@ Apache-2.0 ament_cmake + nav2_common angles - rclcpp - rclcpp_action - rclcpp_lifecycle + geometry_msgs nav2_graceful_controller nav2_msgs nav2_util nav_msgs - geometry_msgs - builtin_interfaces - sensor_msgs - pluginlib - yaml_cpp_vendor opennav_docking_core + pluginlib + rclcpp + rclcpp_action + rclcpp_components + rclcpp_lifecycle + rcl_interfaces + sensor_msgs + tf2 + tf2_geometry_msgs tf2_ros + yaml_cpp_vendor ament_cmake_gtest ament_cmake_pytest + ament_index_cpp ament_lint_common ament_lint_auto diff --git a/nav2_docking/opennav_docking/src/pose_filter.cpp b/nav2_docking/opennav_docking/src/pose_filter.cpp index 53f91a734c..e010087f63 100644 --- a/nav2_docking/opennav_docking/src/pose_filter.cpp +++ b/nav2_docking/opennav_docking/src/pose_filter.cpp @@ -14,6 +14,7 @@ #include "opennav_docking/pose_filter.hpp" #include "rclcpp/rclcpp.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace opennav_docking { diff --git a/nav2_docking/opennav_docking/test/CMakeLists.txt b/nav2_docking/opennav_docking/test/CMakeLists.txt index a49f245c92..8ffbe18740 100644 --- a/nav2_docking/opennav_docking/test/CMakeLists.txt +++ b/nav2_docking/opennav_docking/test/CMakeLists.txt @@ -2,77 +2,90 @@ ament_add_gtest(test_utils test_utils.cpp ) -ament_target_dependencies(test_utils - ${dependencies} -) target_link_libraries(test_utils + ament_index_cpp::ament_index_cpp + ${geometry_msgs_TARGETS} ${library_name} + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test dock database ament_add_gtest(test_dock_database test_dock_database.cpp ) -ament_target_dependencies(test_dock_database - ${dependencies} -) target_link_libraries(test_dock_database + ament_index_cpp::ament_index_cpp ${library_name} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test navigator ament_add_gtest(test_navigator test_navigator.cpp ) -ament_target_dependencies(test_navigator - ${dependencies} -) target_link_libraries(test_navigator + ament_index_cpp::ament_index_cpp + ${geometry_msgs_TARGETS} ${library_name} + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test Controller ament_add_gtest(test_controller test_controller.cpp ) -ament_target_dependencies(test_controller - ${dependencies} -) target_link_libraries(test_controller + ${geometry_msgs_TARGETS} ${library_name} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test Simple Dock ament_add_gtest(test_simple_charging_dock test_simple_charging_dock.cpp ) -ament_target_dependencies(test_simple_charging_dock - ${dependencies} -) target_link_libraries(test_simple_charging_dock - ${library_name} simple_charging_dock + ament_index_cpp::ament_index_cpp + ${geometry_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${sensor_msgs_TARGETS} + simple_charging_dock + tf2_geometry_msgs::tf2_geometry_msgs ) # Test Pose Filter (unit) ament_add_gtest(test_pose_filter test_pose_filter.cpp ) -ament_target_dependencies(test_pose_filter - ${dependencies} -) target_link_libraries(test_pose_filter - ${library_name} + ${geometry_msgs_TARGETS} pose_filter + rclcpp::rclcpp + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs ) # Test dock pluing for server tests add_library(test_dock SHARED testing_dock.cpp) -ament_target_dependencies(test_dock ${dependencies}) -target_compile_definitions(test_dock PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +target_link_libraries(test_dock PUBLIC + ${geometry_msgs_TARGETS} + opennav_docking_core::opennav_docking_core + pluginlib::pluginlib + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros +) install(TARGETS test_dock - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) ament_export_libraries(test_dock) @@ -80,11 +93,12 @@ ament_export_libraries(test_dock) ament_add_gtest(test_docking_server_unit test_docking_server_unit.cpp ) -ament_target_dependencies(test_docking_server_unit - ${dependencies} -) target_link_libraries(test_docking_server_unit + ${geometry_msgs_TARGETS} ${library_name} + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test docking server (smoke) From c2e4aabdab5a4d3ebbf011c9e28d00ce3e27a4c3 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 20 Aug 2024 18:33:48 -0400 Subject: [PATCH 16/21] Switch nav2_smac_planner to modern CMake idioms. (#4634) Signed-off-by: Chris Lalancette --- nav2_smac_planner/CMakeLists.txt | 224 +++++++++++------- .../include/nav2_smac_planner/a_star.hpp | 8 +- .../nav2_smac_planner/analytic_expansion.hpp | 5 +- .../nav2_smac_planner/collision_checker.hpp | 2 +- .../nav2_smac_planner/costmap_downsampler.hpp | 3 +- .../include/nav2_smac_planner/node_2d.hpp | 10 +- .../include/nav2_smac_planner/node_basic.hpp | 12 - .../include/nav2_smac_planner/node_hybrid.hpp | 7 +- .../nav2_smac_planner/node_lattice.hpp | 11 +- .../include/nav2_smac_planner/smoother.hpp | 8 - .../include/nav2_smac_planner/utils.hpp | 1 - nav2_smac_planner/package.xml | 32 ++- nav2_smac_planner/src/node_lattice.cpp | 13 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 1 - .../src/smac_planner_lattice.cpp | 1 - nav2_smac_planner/src/smoother.cpp | 9 +- nav2_smac_planner/test/CMakeLists.txt | 84 ++++--- .../test/test_collision_checker.cpp | 1 - nav2_smac_planner/test/test_nodebasic.cpp | 3 - 19 files changed, 224 insertions(+), 211 deletions(-) diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index d9da9fd147..e2a90a06ef 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -4,29 +4,23 @@ project(nav2_smac_planner) set(CMAKE_BUILD_TYPE Release) #Debug, Release find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(angles REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(std_msgs REQUIRED) -find_package(visualization_msgs REQUIRED) -find_package(nav2_util REQUIRED) find_package(nav2_core REQUIRED) -find_package(nav2_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(builtin_interfaces REQUIRED) -find_package(tf2_ros REQUIRED) find_package(nav2_costmap_2d REQUIRED) -find_package(pluginlib REQUIRED) -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(angles REQUIRED) +find_package(nav2_util REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(nlohmann_json REQUIRED) find_package(ompl REQUIRED) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rcl_interfaces REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) if(MSVC) add_compile_definitions(_USE_MATH_DEFINES) @@ -34,107 +28,144 @@ else() add_compile_options(-O3 -Wextra -Wdeprecated -fPIC) endif() -include_directories( - include - ${OMPL_INCLUDE_DIRS} -) - set(library_name nav2_smac_planner) -set(dependencies - rclcpp - rclcpp_action - rclcpp_lifecycle - std_msgs - visualization_msgs - nav2_util - nav2_msgs - nav_msgs - geometry_msgs - builtin_interfaces - tf2_ros - nav2_costmap_2d - nav2_core - pluginlib - angles - eigen3_cmake_module -) - -# Hybrid plugin -add_library(${library_name} SHARED - src/smac_planner_hybrid.cpp +# Common library +add_library(${library_name}_common SHARED src/a_star.cpp - src/collision_checker.cpp - src/smoother.cpp src/analytic_expansion.cpp - src/node_hybrid.cpp - src/node_lattice.cpp + src/collision_checker.cpp src/costmap_downsampler.cpp src/node_2d.cpp src/node_basic.cpp + src/node_hybrid.cpp + src/node_lattice.cpp + src/smoother.cpp +) +target_include_directories(${library_name}_common + PUBLIC + "$" + "$" + ${OMPL_INCLUDE_DIRS} +) +target_link_libraries(${library_name}_common PUBLIC + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::layers + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + nlohmann_json::nlohmann_json + ${OMPL_LIBRARIES} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2::tf2 + ${visualization_msgs_TARGETS} +) +target_link_libraries(${library_name}_common PRIVATE + angles::angles ) -target_link_libraries(${library_name} ${OMPL_LIBRARIES}) -target_include_directories(${library_name} PUBLIC ${Eigen3_INCLUDE_DIRS}) - -ament_target_dependencies(${library_name} - ${dependencies} +# Hybrid plugin +add_library(${library_name} SHARED + src/smac_planner_hybrid.cpp +) +target_include_directories(${library_name} + PUBLIC + "$" + "$" + ${OMPL_INCLUDE_DIRS} +) +target_link_libraries(${library_name} PUBLIC + ${library_name}_common + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + nlohmann_json::nlohmann_json + ${OMPL_LIBRARIES} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + tf2::tf2 + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} +) +target_link_libraries(${library_name} PRIVATE + pluginlib::pluginlib ) # 2D plugin add_library(${library_name}_2d SHARED src/smac_planner_2d.cpp - src/a_star.cpp - src/smoother.cpp - src/collision_checker.cpp - src/analytic_expansion.cpp - src/node_hybrid.cpp - src/node_lattice.cpp - src/costmap_downsampler.cpp - src/node_2d.cpp - src/node_basic.cpp ) - -target_link_libraries(${library_name}_2d ${OMPL_LIBRARIES}) -target_include_directories(${library_name}_2d PUBLIC ${Eigen3_INCLUDE_DIRS}) - -ament_target_dependencies(${library_name}_2d - ${dependencies} +target_include_directories(${library_name}_2d + PUBLIC + "$" + "$" + ${OMPL_INCLUDE_DIRS} +) +target_link_libraries(${library_name}_2d PUBLIC + ${library_name}_common + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + nlohmann_json::nlohmann_json + ${OMPL_LIBRARIES} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + tf2::tf2 + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} +) +target_link_libraries(${library_name}_2d PRIVATE + pluginlib::pluginlib ) # Lattice plugin add_library(${library_name}_lattice SHARED src/smac_planner_lattice.cpp - src/a_star.cpp - src/smoother.cpp - src/collision_checker.cpp - src/analytic_expansion.cpp - src/node_hybrid.cpp - src/node_lattice.cpp - src/costmap_downsampler.cpp - src/node_2d.cpp - src/node_basic.cpp ) - -target_link_libraries(${library_name}_lattice ${OMPL_LIBRARIES}) -target_include_directories(${library_name}_lattice PUBLIC ${Eigen3_INCLUDE_DIRS}) - -ament_target_dependencies(${library_name}_lattice - ${dependencies} +target_include_directories(${library_name}_lattice + PUBLIC + "$" + "$" + ${OMPL_INCLUDE_DIRS} +) +target_link_libraries(${library_name}_lattice PUBLIC + ${library_name}_common + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + nlohmann_json::nlohmann_json + ${OMPL_LIBRARIES} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + tf2::tf2 + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} +) +target_link_libraries(${library_name}_lattice PRIVATE + ament_index_cpp::ament_index_cpp + pluginlib::pluginlib ) pluginlib_export_plugin_description_file(nav2_core smac_plugin_hybrid.xml) pluginlib_export_plugin_description_file(nav2_core smac_plugin_2d.xml) pluginlib_export_plugin_description_file(nav2_core smac_plugin_lattice.xml) -install(TARGETS ${library_name} ${library_name}_2d ${library_name}_lattice +install(TARGETS ${library_name}_common ${library_name} ${library_name}_2d ${library_name}_lattice + EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY lattice_primitives/sample_primitives DESTINATION share/${PROJECT_NAME}) @@ -144,10 +175,25 @@ if(BUILD_TESTING) set(AMENT_LINT_AUTO_FILE_EXCLUDE include/nav2_smac_planner/thirdparty/robin_hood.h) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) + ament_find_gtest() add_subdirectory(test) endif() -ament_export_include_directories(include ${OMPL_INCLUDE_DIRS}) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(${library_name} ${library_name}_2d ${library_name}_lattice) -ament_export_dependencies(${dependencies}) +ament_export_dependencies( + geometry_msgs + nav2_core + nav2_costmap_2d + nav_msgs + nlohmann_json + ompl + rclcpp + rclcpp_lifecycle + rcl_interfaces + tf2 + tf2_ros + visualization_msgs +) +ament_export_targets(${PROJECT_NAME}) ament_package() diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index c95d8c82cf..a3ae1f6c7b 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -16,14 +16,12 @@ #ifndef NAV2_SMAC_PLANNER__A_STAR_HPP_ #define NAV2_SMAC_PLANNER__A_STAR_HPP_ -#include -#include -#include +#include #include #include -#include #include -#include "Eigen/Core" +#include +#include #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_core/planner_exceptions.hpp" diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index 76719d7231..c4d3deccda 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -15,10 +15,11 @@ #ifndef NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ #define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ -#include -#include +#include #include #include +#include +#include #include "nav2_smac_planner/node_2d.hpp" #include "nav2_smac_planner/node_hybrid.hpp" diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp index 205c3f0471..5b31ee87cd 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include #include +#include #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" diff --git a/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp b/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp index 332507d7c2..2118917029 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp @@ -15,9 +15,8 @@ #ifndef NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ #define NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ -#include -#include #include +#include #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_smac_planner/constants.hpp" diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp index 3ccadefded..2ef090fe49 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -15,14 +15,10 @@ #ifndef NAV2_SMAC_PLANNER__NODE_2D_HPP_ #define NAV2_SMAC_PLANNER__NODE_2D_HPP_ -#include -#include -#include -#include -#include -#include -#include #include +#include +#include +#include #include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/constants.hpp" diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp index dfb26c5dda..d7b7d72409 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp @@ -15,18 +15,6 @@ #ifndef NAV2_SMAC_PLANNER__NODE_BASIC_HPP_ #define NAV2_SMAC_PLANNER__NODE_BASIC_HPP_ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ompl/base/StateSpace.h" - #include "nav2_smac_planner/constants.hpp" #include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/node_lattice.hpp" diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index b67ed978ec..cf6a9a6e89 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -15,15 +15,10 @@ #ifndef NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_ #define NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_ -#include -#include -#include -#include #include -#include #include #include -#include +#include #include "ompl/base/StateSpace.h" diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index f31ccb9b0d..f3e3ff37f9 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -15,21 +15,12 @@ #ifndef NAV2_SMAC_PLANNER__NODE_LATTICE_HPP_ #define NAV2_SMAC_PLANNER__NODE_LATTICE_HPP_ -#include - -#include -#include -#include #include -#include #include -#include -#include #include +#include -#include "nlohmann/json.hpp" #include "ompl/base/StateSpace.h" -#include "angles/angles.h" #include "nav2_smac_planner/constants.hpp" #include "nav2_smac_planner/types.hpp" diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp index d896cd90b5..89ecc0b092 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp @@ -15,22 +15,14 @@ #ifndef NAV2_SMAC_PLANNER__SMOOTHER_HPP_ #define NAV2_SMAC_PLANNER__SMOOTHER_HPP_ -#include #include -#include -#include -#include -#include #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/constants.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav_msgs/msg/path.hpp" -#include "angles/angles.h" -#include "tf2/utils.h" #include "ompl/base/StateSpace.h" -#include "ompl/base/spaces/DubinsStateSpace.h" 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 fb020a729a..89cb2f8dc7 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp @@ -21,7 +21,6 @@ #include #include "nlohmann/json.hpp" -#include "Eigen/Core" #include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/pose.hpp" #include "tf2/utils.h" diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 0a1303bd68..d73d553496 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -8,31 +8,29 @@ Apache-2.0 ament_cmake + nav2_common - rclcpp - rclcpp_action - rclcpp_lifecycle - visualization_msgs - nav2_util - nav2_msgs - nav_msgs + ament_index_cpp + angles + eigen geometry_msgs - builtin_interfaces - nav2_common - tf2_ros - nav2_costmap_2d nav2_core - pluginlib - eigen3_cmake_module - eigen - ompl + nav2_costmap_2d + nav2_util + nav_msgs nlohmann-json-dev - angles + ompl + pluginlib + rclcpp + rclcpp_lifecycle + rcl_interfaces + tf2_ros + tf2 + visualization_msgs ament_lint_common ament_lint_auto ament_cmake_gtest - ament_cmake_pytest ament_cmake diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index f3ddc9adb8..6c794b9a98 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -12,16 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include +#include #include -#include +#include +#include +#include #include -#include #include -#include #include -#include -#include +#include + +#include "angles/angles.h" #include "ompl/base/ScopedState.h" #include "ompl/base/spaces/DubinsStateSpace.h" diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 5ba10d7d9a..15491c051e 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -19,7 +19,6 @@ #include #include -#include "Eigen/Core" #include "nav2_smac_planner/smac_planner_hybrid.hpp" // #define BENCHMARK_TESTING diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index c5a9ecdff6..ebeead9c5c 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -18,7 +18,6 @@ #include #include -#include "Eigen/Core" #include "nav2_smac_planner/smac_planner_lattice.hpp" // #define BENCHMARK_TESTING diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp index a69e14011f..fb43f4a121 100644 --- a/nav2_smac_planner/src/smoother.cpp +++ b/nav2_smac_planner/src/smoother.cpp @@ -14,8 +14,15 @@ #include #include -#include + +#include #include +#include + +#include "angles/angles.h" + +#include "tf2/utils.h" + #include "nav2_smac_planner/smoother.hpp" namespace nav2_smac_planner diff --git a/nav2_smac_planner/test/CMakeLists.txt b/nav2_smac_planner/test/CMakeLists.txt index 8039cf9c51..fc4abf20c7 100644 --- a/nav2_smac_planner/test/CMakeLists.txt +++ b/nav2_smac_planner/test/CMakeLists.txt @@ -2,128 +2,136 @@ ament_add_gtest(test_utils test_utils.cpp ) -ament_target_dependencies(test_utils - ${dependencies} -) target_link_libraries(test_utils ${library_name} + ${geometry_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + nav2_util::nav2_util_core ) # Test costmap downsampler ament_add_gtest(test_costmap_downsampler test_costmap_downsampler.cpp ) -ament_target_dependencies(test_costmap_downsampler - ${dependencies} -) target_link_libraries(test_costmap_downsampler ${library_name} + nav2_costmap_2d::nav2_costmap_2d_core + nav2_util::nav2_util_core + rclcpp::rclcpp ) # Test Node2D ament_add_gtest(test_node2d test_node2d.cpp ) -ament_target_dependencies(test_node2d - ${dependencies} -) target_link_libraries(test_node2d ${library_name} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test NodeHybrid ament_add_gtest(test_nodehybrid test_nodehybrid.cpp ) -ament_target_dependencies(test_nodehybrid - ${dependencies} -) target_link_libraries(test_nodehybrid ${library_name} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test NodeBasic ament_add_gtest(test_nodebasic test_nodebasic.cpp ) -ament_target_dependencies(test_nodebasic - ${dependencies} -) target_link_libraries(test_nodebasic ${library_name} + rclcpp::rclcpp ) # Test collision checker ament_add_gtest(test_collision_checker test_collision_checker.cpp ) -ament_target_dependencies(test_collision_checker - ${dependencies} -) target_link_libraries(test_collision_checker ${library_name} + ${geometry_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test A* ament_add_gtest(test_a_star test_a_star.cpp ) -ament_target_dependencies(test_a_star - ${dependencies} -) target_link_libraries(test_a_star ${library_name} + ament_index_cpp::ament_index_cpp + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test SMAC Hybrid ament_add_gtest(test_smac_hybrid test_smac_hybrid.cpp ) -ament_target_dependencies(test_smac_hybrid - ${dependencies} -) target_link_libraries(test_smac_hybrid ${library_name} + ${geometry_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test SMAC 2D ament_add_gtest(test_smac_2d test_smac_2d.cpp ) -ament_target_dependencies(test_smac_2d - ${dependencies} -) target_link_libraries(test_smac_2d ${library_name}_2d + ${geometry_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test SMAC lattice ament_add_gtest(test_smac_lattice test_smac_lattice.cpp ) -ament_target_dependencies(test_smac_lattice - ${dependencies} -) target_link_libraries(test_smac_lattice ${library_name}_lattice + ${geometry_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test SMAC Smoother ament_add_gtest(test_smoother test_smoother.cpp ) -ament_target_dependencies(test_smoother - ${dependencies} -) target_link_libraries(test_smoother ${library_name}_lattice ${library_name} ${library_name}_2d + ament_index_cpp::ament_index_cpp + ${geometry_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) -#Test Lattice node +# Test Lattice node ament_add_gtest(test_lattice_node test_nodelattice.cpp) - -ament_target_dependencies(test_lattice_node ${dependencies}) - -target_link_libraries(test_lattice_node ${library_name}) +target_link_libraries(test_lattice_node + ${library_name} + ament_index_cpp::ament_index_cpp + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/nav2_smac_planner/test/test_collision_checker.cpp b/nav2_smac_planner/test/test_collision_checker.cpp index dd4e032a51..92590bd945 100644 --- a/nav2_smac_planner/test/test_collision_checker.cpp +++ b/nav2_smac_planner/test/test_collision_checker.cpp @@ -19,7 +19,6 @@ #include "gtest/gtest.h" #include "nav2_smac_planner/collision_checker.hpp" -#include "nav2_util/lifecycle_node.hpp" using namespace nav2_costmap_2d; // NOLINT diff --git a/nav2_smac_planner/test/test_nodebasic.cpp b/nav2_smac_planner/test/test_nodebasic.cpp index 00aea82465..544842cec6 100644 --- a/nav2_smac_planner/test/test_nodebasic.cpp +++ b/nav2_smac_planner/test/test_nodebasic.cpp @@ -19,9 +19,6 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_costmap_2d/costmap_subscriber.hpp" -#include "nav2_util/lifecycle_node.hpp" #include "nav2_smac_planner/node_basic.hpp" #include "nav2_smac_planner/node_2d.hpp" #include "nav2_smac_planner/node_hybrid.hpp" From a63090f0fecea2c2af2d2cfc8f61c1e04080fcef Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 21 Aug 2024 13:34:37 -0700 Subject: [PATCH 17/21] Update Smac Planner for rounding to closest bin rather than flooring (#4636) --- nav2_smac_planner/src/node_hybrid.cpp | 4 ++-- nav2_smac_planner/src/smac_planner_hybrid.cpp | 10 ++++------ nav2_smac_planner/test/test_nodehybrid.cpp | 6 +++--- 3 files changed, 9 insertions(+), 11 deletions(-) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 5314cd68c1..1d3d36d520 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -330,8 +330,8 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) unsigned int HybridMotionTable::getClosestAngularBin(const double & theta) { - return static_cast(floor(theta / static_cast(bin_size))) % - num_angle_quantization; + auto bin = static_cast(round(static_cast(theta) / bin_size)); + return bin < num_angle_quantization ? bin : 0u; } float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 15491c051e..8a3d8f56a6 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -367,7 +367,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( std::to_string(start.pose.position.y) + ") was outside bounds"); } - double orientation_bin = tf2::getYaw(start.pose.orientation) / _angle_bin_size; + double orientation_bin = std::round(tf2::getYaw(start.pose.orientation) / _angle_bin_size); while (orientation_bin < 0.0) { orientation_bin += static_cast(_angle_quantizations); } @@ -375,8 +375,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( if (orientation_bin >= static_cast(_angle_quantizations)) { orientation_bin -= static_cast(_angle_quantizations); } - unsigned int orientation_bin_id = static_cast(floor(orientation_bin)); - _a_star->setStart(mx, my, orientation_bin_id); + _a_star->setStart(mx, my, static_cast(orientation_bin)); // Set goal point, in A* bin search coordinates if (!costmap->worldToMapContinuous(goal.pose.position.x, goal.pose.position.y, mx, my)) { @@ -384,7 +383,7 @@ 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 = tf2::getYaw(goal.pose.orientation) / _angle_bin_size; + orientation_bin = std::round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size); while (orientation_bin < 0.0) { orientation_bin += static_cast(_angle_quantizations); } @@ -392,8 +391,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( if (orientation_bin >= static_cast(_angle_quantizations)) { orientation_bin -= static_cast(_angle_quantizations); } - orientation_bin_id = static_cast(floor(orientation_bin)); - _a_star->setGoal(mx, my, orientation_bin_id); + _a_star->setGoal(mx, my, static_cast(orientation_bin)); // Setup message nav_msgs::msg::Path plan; diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index fb17dad520..d98c8336f6 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -396,7 +396,7 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test) { motion_table.bin_size = M_PI; motion_table.num_angle_quantization = 2; - double test_theta = M_PI; + double test_theta = M_PI / 2.0 - 0.000001; unsigned int expected_angular_bin = 0; unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); EXPECT_EQ(expected_angular_bin, calculated_angular_bin); @@ -414,8 +414,8 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test) { motion_table.bin_size = 0.0872664675; motion_table.num_angle_quantization = 72; - double test_theta = 6.28318526567925; - unsigned int expected_angular_bin = 71; + double test_theta = 6.28317530718; // 0.0001 less than 2 pi + unsigned int expected_angular_bin = 0; // should be closer to wrap around unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); EXPECT_EQ(expected_angular_bin, calculated_angular_bin); } From 6471f02b6bd73f06b5c96a20d35cb6db0c9f2f6e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 21 Aug 2024 13:51:22 -0700 Subject: [PATCH 18/21] adding stamped option for loopbacks im (#4637) Signed-off-by: Steve Macenski --- nav2_loopback_sim/README.md | 1 + .../nav2_loopback_sim/loopback_simulator.py | 24 +++++++++++++++---- 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/nav2_loopback_sim/README.md b/nav2_loopback_sim/README.md index 1e1aec6569..7339904090 100644 --- a/nav2_loopback_sim/README.md +++ b/nav2_loopback_sim/README.md @@ -38,6 +38,7 @@ ros2 launch nav2_bringup tb4_loopback_simulation.launch.py # Nav2 integrated na - `odom_frame_id`: The odom frame to use (default `odom`) - `map_frame_id`: The map frame to use (default `map`) - `scan_frame_id`: The can frame to use to publish a scan to keep the collision monitor fed and happy (default `base_scan` for TB3, `rplidar_link` for TB4) +- `enable_stamped_cmd_vel`: Whether cmd_vel is stamped or unstamped (i.e. Twist or TwistStamped). Default `false` for `Twist`. ### Topics diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 885a6725f5..7902affb13 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -15,7 +15,7 @@ import math -from geometry_msgs.msg import PoseWithCovarianceStamped, Twist +from geometry_msgs.msg import PoseWithCovarianceStamped, Twist, TwistStamped from geometry_msgs.msg import Quaternion, TransformStamped, Vector3 from nav2_simple_commander.line_iterator import LineIterator from nav_msgs.msg import Odometry @@ -73,6 +73,9 @@ def __init__(self): self.declare_parameter('scan_frame_id', 'base_scan') self.scan_frame_id = self.get_parameter('scan_frame_id').get_parameter_value().string_value + self.declare_parameter('enable_stamped_cmd_vel', False) + use_stamped = self.get_parameter('enable_stamped_cmd_vel').get_parameter_value().bool_value + self.declare_parameter('scan_publish_dur', 0.1) self.scan_publish_dur = self.get_parameter( 'scan_publish_dur').get_parameter_value().double_value @@ -93,9 +96,14 @@ def __init__(self): self.initial_pose_sub = self.create_subscription( PoseWithCovarianceStamped, 'initialpose', self.initialPoseCallback, 10) - self.cmd_vel_sub = self.create_subscription( - Twist, - 'cmd_vel', self.cmdVelCallback, 10) + if not use_stamped: + self.cmd_vel_sub = self.create_subscription( + Twist, + 'cmd_vel', self.cmdVelCallback, 10) + else: + self.cmd_vel_sub = self.create_subscription( + TwistStamped, + 'cmd_vel', self.cmdVelStampedCallback, 10) self.odom_pub = self.create_publisher(Odometry, 'odom', 10) sensor_qos = QoSProfile( @@ -139,6 +147,14 @@ def cmdVelCallback(self, msg): self.curr_cmd_vel = msg self.curr_cmd_vel_time = self.get_clock().now() + def cmdVelStampedCallback(self, msg): + self.debug('Received cmd_vel') + if self.initial_pose is None: + # Don't consider velocities before the initial pose is set + return + self.curr_cmd_vel = msg.twist + self.curr_cmd_vel_time = rclpy.time.Time.from_msg(msg.header.stamp) + def initialPoseCallback(self, msg): self.info('Received initial pose!') if self.initial_pose is None: From 226b196a84989d1dffb0d32070d63c823079861d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 21 Aug 2024 16:57:25 -0400 Subject: [PATCH 19/21] Fixes for nav2_smac_planner to build with -Werror. (#4635) * Fixes for nav2_smac_planner to build with -Werror. Signed-off-by: Chris Lalancette * Feedback from review. Signed-off-by: Chris Lalancette --------- Signed-off-by: Chris Lalancette --- nav2_smac_planner/CMakeLists.txt | 2 ++ .../include/nav2_smac_planner/constants.hpp | 10 +++---- .../include/nav2_smac_planner/node_basic.hpp | 6 ++-- .../nav2_smac_planner/thirdparty/robin_hood.h | 2 +- .../include/nav2_smac_planner/types.hpp | 2 +- nav2_smac_planner/src/analytic_expansion.cpp | 20 ++++++------- nav2_smac_planner/src/collision_checker.cpp | 16 +++++----- nav2_smac_planner/src/costmap_downsampler.cpp | 2 +- nav2_smac_planner/src/node_2d.cpp | 4 +-- nav2_smac_planner/src/node_hybrid.cpp | 30 +++++++++---------- nav2_smac_planner/src/smac_planner_hybrid.cpp | 1 - nav2_smac_planner/src/smoother.cpp | 4 +-- .../deprecated/smoother_cost_function.hpp | 4 +-- nav2_smac_planner/test/test_a_star.cpp | 1 - nav2_smac_planner/test/test_node2d.cpp | 6 ++-- nav2_smac_planner/test/test_nodehybrid.cpp | 6 ++-- nav2_smac_planner/test/test_nodelattice.cpp | 6 ++-- 17 files changed, 58 insertions(+), 64 deletions(-) diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index e2a90a06ef..a8789af62d 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -22,6 +22,8 @@ find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(visualization_msgs REQUIRED) +nav2_package() + if(MSVC) add_compile_definitions(_USE_MATH_DEFINES) else() diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp index a7ff03fdca..af44ce5365 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp @@ -59,11 +59,11 @@ inline MotionModel fromString(const std::string & n) } } -const float UNKNOWN = 255.0; -const float OCCUPIED = 254.0; -const float INSCRIBED = 253.0; -const float MAX_NON_OBSTACLE = 252.0; -const float FREE = 0; +const float UNKNOWN_COST = 255.0; +const float OCCUPIED_COST = 254.0; +const float INSCRIBED_COST = 253.0; +const float MAX_NON_OBSTACLE_COST = 252.0; +const float FREE_COST = 0; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp index d7b7d72409..0532e1fea3 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp @@ -37,9 +37,9 @@ class NodeBasic * @brief A constructor for nav2_smac_planner::NodeBasic * @param index The index of this node for self-reference */ - explicit NodeBasic(const uint64_t index) - : index(index), - graph_node_ptr(nullptr) + explicit NodeBasic(const uint64_t new_index) + : graph_node_ptr(nullptr), + index(new_index) { } diff --git a/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h b/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h index 4d3977937b..978a3728a8 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h +++ b/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h @@ -147,7 +147,7 @@ static Counts& counts() { # pragma intrinsic(ROBIN_HOOD(BITSCANFORWARD)) # define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x) \ [](size_t mask) noexcept -> int { \ - unsigned long index; // NOLINT \ + unsigned long index; \ // NOLINT return ROBIN_HOOD(BITSCANFORWARD)(&index, mask) ? static_cast(index) \ : ROBIN_HOOD(BITNESS); \ }(x) diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp index 18c2e1315f..cc6c9975c5 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -110,7 +110,7 @@ struct SmootherParams * @struct nav2_smac_planner::TurnDirection * @brief A struct with the motion primitive's direction embedded */ -enum struct TurnDirection +enum class TurnDirection { UNKNOWN = 0, FORWARD = 1, diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index f36b152583..73084859da 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -337,28 +337,28 @@ void AnalyticExpansion::cleanNode(const NodePtr & /*expanded_nodes*/) template<> typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion:: getAnalyticPath( - const NodePtr & node, - const NodePtr & goal, - const NodeGetter & node_getter, - const ompl::base::StateSpacePtr & state_space) + const NodePtr &, + const NodePtr &, + const NodeGetter &, + const ompl::base::StateSpacePtr &) { return AnalyticExpansionNodes(); } template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( - const NodePtr & node, - const NodePtr & goal_node, - const AnalyticExpansionNodes & expanded_nodes) + const NodePtr &, + const NodePtr &, + const AnalyticExpansionNodes &) { return NodePtr(nullptr); } template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, const NodePtr & goal_node, - const NodeGetter & getter, int & analytic_iterations, - int & closest_distance) + const NodePtr &, const NodePtr &, + const NodeGetter &, int &, + int &) { return NodePtr(nullptr); } diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 8b9e69aa77..385f484615 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -130,11 +130,11 @@ bool GridCollisionChecker::inCollision( // If its inscribed, in collision, or unknown in the middle, // no need to even check the footprint, its invalid - if (footprint_cost_ == UNKNOWN && !traverse_unknown) { + if (footprint_cost_ == UNKNOWN_COST && !traverse_unknown) { return true; } - if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { + if (footprint_cost_ == INSCRIBED_COST || footprint_cost_ == OCCUPIED_COST) { return true; } @@ -153,23 +153,23 @@ bool GridCollisionChecker::inCollision( footprint_cost_ = static_cast(footprintCost(current_footprint)); - if (footprint_cost_ == UNKNOWN && traverse_unknown) { + if (footprint_cost_ == UNKNOWN_COST && traverse_unknown) { return false; } // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= OCCUPIED; + return footprint_cost_ >= OCCUPIED_COST; } else { // if radius, then we can check the center of the cost assuming inflation is used footprint_cost_ = static_cast(costmap_->getCost( static_cast(x + 0.5f), static_cast(y + 0.5f))); - if (footprint_cost_ == UNKNOWN && traverse_unknown) { + if (footprint_cost_ == UNKNOWN_COST && traverse_unknown) { return false; } // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= INSCRIBED; + return footprint_cost_ >= INSCRIBED_COST; } } @@ -178,12 +178,12 @@ bool GridCollisionChecker::inCollision( const bool & traverse_unknown) { footprint_cost_ = costmap_->getCost(i); - if (footprint_cost_ == UNKNOWN && traverse_unknown) { + if (footprint_cost_ == UNKNOWN_COST && traverse_unknown) { return false; } // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= INSCRIBED; + return footprint_cost_ >= INSCRIBED_COST; } float GridCollisionChecker::getCost() diff --git a/nav2_smac_planner/src/costmap_downsampler.cpp b/nav2_smac_planner/src/costmap_downsampler.cpp index d0581daaf8..216aa5c70f 100644 --- a/nav2_smac_planner/src/costmap_downsampler.cpp +++ b/nav2_smac_planner/src/costmap_downsampler.cpp @@ -48,7 +48,7 @@ void CostmapDownsampler::on_configure( _downsampled_costmap = std::make_unique( _downsampled_size_x, _downsampled_size_y, _downsampled_resolution, - _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN); + _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN_COST); if (!node.expired()) { _downsampled_costmap_pub = std::make_unique( diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index 746268b56a..e0e23840ae 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -128,7 +128,7 @@ void Node2D::getNeighbors( uint64_t index; NodePtr neighbor; uint64_t node_i = this->getIndex(); - const Coordinates parent = getCoords(this->getIndex()); + const Coordinates coord_parent = getCoords(this->getIndex()); Coordinates child; for (unsigned int i = 0; i != _neighbors_grid_offsets.size(); ++i) { @@ -136,7 +136,7 @@ void Node2D::getNeighbors( // Check for wrap around conditions child = getCoords(index); - if (fabs(parent.x - child.x) > 1 || fabs(parent.y - child.y) > 1) { + if (fabs(coord_parent.x - child.x) > 1 || fabs(coord_parent.y - child.y) > 1) { continue; } diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 1d3d36d520..0ec6da7a73 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -169,9 +169,9 @@ void HybridMotionTable::initDubin( const TurnDirection turn_dir = projections[i]._turn_dir; if (turn_dir != TurnDirection::FORWARD && turn_dir != TurnDirection::REVERSE) { // Turning, so length is the arc length - const float angle = projections[i]._theta * bin_size; - const float turning_rad = delta_dist / (2.0f * sin(angle / 2.0f)); - travel_costs[i] = turning_rad * angle; + const float arc_angle = projections[i]._theta * bin_size; + const float turning_rad = delta_dist / (2.0f * sin(arc_angle / 2.0f)); + travel_costs[i] = turning_rad * arc_angle; } else { travel_costs[i] = delta_dist; } @@ -290,9 +290,9 @@ void HybridMotionTable::initReedsShepp( const TurnDirection turn_dir = projections[i]._turn_dir; if (turn_dir != TurnDirection::FORWARD && turn_dir != TurnDirection::REVERSE) { // Turning, so length is the arc length - const float angle = projections[i]._theta * bin_size; - const float turning_rad = delta_dist / (2.0f * sin(angle / 2.0f)); - travel_costs[i] = turning_rad * angle; + const float arc_angle = projections[i]._theta * bin_size; + const float turning_rad = delta_dist / (2.0f * sin(arc_angle / 2.0f)); + travel_costs[i] = turning_rad * arc_angle; } else { travel_costs[i] = delta_dist; } @@ -305,11 +305,11 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) projection_list.reserve(projections.size()); for (unsigned int i = 0; i != projections.size(); i++) { - const MotionPose & motion_model = projections[i]; + const MotionPose & proj_motion_model = projections[i]; // normalize theta, I know its overkill, but I've been burned before... const float & node_heading = node->pose.theta; - float new_heading = node_heading + motion_model._theta; + float new_heading = node_heading + proj_motion_model._theta; if (new_heading < 0.0) { new_heading += num_angle_quantization_float; @@ -322,7 +322,7 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) projection_list.emplace_back( delta_xs[i][node_heading] + node->pose.x, delta_ys[i][node_heading] + node->pose.y, - new_heading, motion_model._turn_dir); + new_heading, proj_motion_model._turn_dir); } return projection_list; @@ -564,7 +564,7 @@ float NodeHybrid::adjustedFootprintCost(const float & cost) float NodeHybrid::getObstacleHeuristic( const Coordinates & node_coords, - const Coordinates & goal_coords, + const Coordinates &, const float & cost_penalty) { // If already expanded, return the cost @@ -651,8 +651,8 @@ float NodeHybrid::getObstacleHeuristic( unsigned int y_offset = (new_idx / size_x) * 2; unsigned int x_offset = (new_idx - ((new_idx / size_x) * size_x)) * 2; cost = costmap->getCost(x_offset, y_offset); - for (unsigned int i = 0; i < 2u; ++i) { - unsigned int mxd = x_offset + i; + for (unsigned int k = 0; k < 2u; ++k) { + unsigned int mxd = x_offset + k; if (mxd >= costmap->getSizeInCellsX()) { continue; } @@ -661,7 +661,7 @@ float NodeHybrid::getObstacleHeuristic( if (myd >= costmap->getSizeInCellsY()) { continue; } - if (i == 0 && j == 0) { + if (k == 0 && j == 0) { continue; } cost = std::min(cost, static_cast(costmap->getCost(mxd, myd))); @@ -674,10 +674,10 @@ float NodeHybrid::getObstacleHeuristic( if (!is_circular) { // Adjust cost value if using SE2 footprint checks cost = adjustedFootprintCost(cost); - if (cost >= OCCUPIED) { + if (cost >= OCCUPIED_COST) { continue; } - } else if (cost >= INSCRIBED) { + } else if (cost >= INSCRIBED_COST) { continue; } diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 8a3d8f56a6..02b85fef7a 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -288,7 +288,6 @@ void SmacPlannerHybrid::activate() // Special case handling to obtain resolution changes in global costmap auto resolution_remote_cb = [this](const rclcpp::Parameter & p) { - auto node = _node.lock(); dynamicParametersCallback( {rclcpp::Parameter("resolution", rclcpp::ParameterValue(p.as_double()))}); }; diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp index fb43f4a121..66e0d58446 100644 --- a/nav2_smac_planner/src/smoother.cpp +++ b/nav2_smac_planner/src/smoother.cpp @@ -170,7 +170,7 @@ bool Smoother::smoothImpl( cost = static_cast(costmap->getCost(mx, my)); } - if (cost > MAX_NON_OBSTACLE && cost != UNKNOWN) { + if (cost > MAX_NON_OBSTACLE_COST && cost != UNKNOWN_COST) { RCLCPP_DEBUG( rclcpp::get_logger("SmacPlannerSmoother"), "Smoothing process resulted in an infeasible collision. " @@ -374,7 +374,7 @@ void Smoother::findBoundaryExpansion( // Check for collision unsigned int mx, my; costmap->worldToMap(x, y, mx, my); - if (static_cast(costmap->getCost(mx, my)) >= INSCRIBED) { + if (static_cast(costmap->getCost(mx, my)) >= INSCRIBED_COST) { expansion.in_collision = true; } diff --git a/nav2_smac_planner/test/deprecated/smoother_cost_function.hpp b/nav2_smac_planner/test/deprecated/smoother_cost_function.hpp index acd6414567..e627bc84ed 100644 --- a/nav2_smac_planner/test/deprecated/smoother_cost_function.hpp +++ b/nav2_smac_planner/test/deprecated/smoother_cost_function.hpp @@ -398,7 +398,7 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction double & r, Eigen::Vector2d & xi) const { - if (value == FREE) { + if (value == FREE_COST) { return; } @@ -432,7 +432,7 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction double & j0, double & j1) const { - if (value == FREE) { + if (value == FREE_COST) { return; } diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 5cae8b38f3..32ff915825 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -301,7 +301,6 @@ TEST(AStarTest, test_a_star_lattice) nav2_smac_planner::MotionModel::STATE_LATTICE, info); int max_iterations = 10000; float tolerance = 10.0; - int it_on_approach = 10; int terminal_checking_interval = 5000; double max_planning_time = 120.0; int num_it = 0; diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index 7b99858500..4aa66b04a1 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -142,10 +142,8 @@ TEST(Node2DTest, test_node_2d_neighbors) unsigned char cost = static_cast(1); nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(1); node->setCost(cost); - std::function neighborGetter = - [&, this](const uint64_t & index, - nav2_smac_planner::Node2D * & neighbor_rtn) -> bool + std::function neighborGetter = + [](const uint64_t &, nav2_smac_planner::Node2D * &) -> bool { return false; }; diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index d98c8336f6..43fdc5e516 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -362,10 +362,8 @@ TEST(NodeHybridTest, test_node_reeds_neighbors) std::make_unique(costmap_ros, 72, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); nav2_smac_planner::NodeHybrid * node = new nav2_smac_planner::NodeHybrid(49); - std::function neighborGetter = - [&, this](const uint64_t & index, - nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool + std::function neighborGetter = + [](const uint64_t &, nav2_smac_planner::NodeHybrid * &) -> bool { // because we don't return a real object return false; diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp index e118388a07..98a921d175 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -303,10 +303,8 @@ TEST(NodeLatticeTest, test_get_neighbors) std::make_unique(costmap_ros, 72, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); - std::function neighborGetter = - [&, this](const uint64_t & index, - nav2_smac_planner::NodeLattice * & neighbor_rtn) -> bool + std::function neighborGetter = + [](const uint64_t &, nav2_smac_planner::NodeLattice * &) -> bool { // because we don't return a real object return false; From 8990c05992e7f1d7ea02fc32a5ab4355ed77d3ca Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 22 Aug 2024 13:08:11 -0400 Subject: [PATCH 20/21] Switch nav2_system_tests to modern CMake idioms. (#4638) Signed-off-by: Chris Lalancette --- nav2_system_tests/CMakeLists.txt | 176 +++++++++--------- nav2_system_tests/package.xml | 73 +++----- .../src/behavior_tree/CMakeLists.txt | 18 +- .../behavior_tree/test_behavior_tree_node.cpp | 35 ++-- .../behaviors/assisted_teleop/CMakeLists.txt | 14 +- .../assisted_teleop_behavior_tester.hpp | 1 - .../src/behaviors/wait/CMakeLists.txt | 12 +- .../behaviors/wait/wait_behavior_tester.hpp | 1 - .../src/dummy_controller/CMakeLists.txt | 15 +- .../src/dummy_planner/CMakeLists.txt | 13 +- .../src/localization/CMakeLists.txt | 6 +- .../localization/test_localization_node.cpp | 6 +- nav2_system_tests/src/planning/CMakeLists.txt | 68 +++++-- .../src/planning/planner_tester.hpp | 3 - .../planning/test_planner_is_path_valid.cpp | 1 + .../src/planning/test_planner_plugins.cpp | 2 + nav2_system_tests/src/updown/CMakeLists.txt | 8 +- nav2_system_tests/src/updown/test_updown.cpp | 2 +- 18 files changed, 230 insertions(+), 224 deletions(-) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 127986f464..851a1e2db4 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -1,114 +1,110 @@ cmake_minimum_required(VERSION 3.5) project(nav2_system_tests) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 23) -endif() - find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(nav2_util REQUIRED) -find_package(nav2_map_server REQUIRED) -find_package(nav2_behavior_tree REQUIRED) -find_package(nav2_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(visualization_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(std_msgs REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(nav2_amcl REQUIRED) -find_package(nav2_lifecycle_manager REQUIRED) -find_package(rclpy REQUIRED) -find_package(nav2_navfn_planner REQUIRED) -find_package(nav2_planner REQUIRED) -find_package(navigation2) -find_package(angles REQUIRED) -find_package(behaviortree_cpp REQUIRED) -find_package(pluginlib REQUIRED) -find_package(nav2_minimal_tb3_sim REQUIRED) nav2_package() -set(dependencies - rclcpp - nav2_util - nav2_map_server - nav2_msgs - nav_msgs - visualization_msgs - nav2_amcl - nav2_lifecycle_manager - nav2_behavior_tree - geometry_msgs - std_msgs - tf2_geometry_msgs - rclpy - nav2_planner - nav2_navfn_planner - angles - behaviortree_cpp - pluginlib -) - -set(local_controller_plugin_lib local_controller) - -add_library(${local_controller_plugin_lib} SHARED src/error_codes/controller/controller_error_plugins.cpp) -ament_target_dependencies(${local_controller_plugin_lib} ${dependencies}) -target_compile_definitions(${local_controller_plugin_lib} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -pluginlib_export_plugin_description_file(nav2_core src/error_codes/controller_plugins.xml) - -install(TARGETS ${local_controller_plugin_lib} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} -) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + find_package(ament_index_cpp REQUIRED) + find_package(ament_lint_auto REQUIRED) + find_package(angles REQUIRED) + find_package(behaviortree_cpp REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(nav2_amcl REQUIRED) + find_package(nav2_behavior_tree REQUIRED) + find_package(nav2_lifecycle_manager REQUIRED) + find_package(nav2_map_server REQUIRED) + find_package(nav2_minimal_tb3_sim REQUIRED) + find_package(nav2_msgs REQUIRED) + find_package(nav2_navfn_planner REQUIRED) + find_package(nav2_planner REQUIRED) + find_package(nav2_util REQUIRED) + find_package(nav_msgs REQUIRED) + find_package(navigation2 REQUIRED) + find_package(pluginlib REQUIRED) + find_package(rclcpp REQUIRED) + find_package(rclcpp_lifecycle REQUIRED) + find_package(std_msgs REQUIRED) + find_package(tf2 REQUIRED) + find_package(tf2_geometry_msgs REQUIRED) + find_package(tf2_ros REQUIRED) + find_package(visualization_msgs REQUIRED) -install(FILES src/error_codes/controller_plugins.xml - DESTINATION share/${PROJECT_NAME} -) + ament_lint_auto_find_test_dependencies() + + ament_find_gtest() -set(global_planner_plugin_lib global_planner) + set(local_controller_plugin_lib local_controller) -add_library(${global_planner_plugin_lib} SHARED src/error_codes/planner/planner_error_plugin.cpp) -ament_target_dependencies(${global_planner_plugin_lib} ${dependencies}) -target_compile_definitions(${global_planner_plugin_lib} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -pluginlib_export_plugin_description_file(nav2_core src/error_codes/planner_plugins.xml) + add_library(${local_controller_plugin_lib} SHARED src/error_codes/controller/controller_error_plugins.cpp) + target_link_libraries(${local_controller_plugin_lib} PRIVATE + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + pluginlib::pluginlib + ) + pluginlib_export_plugin_description_file(nav2_core src/error_codes/controller_plugins.xml) -install(TARGETS ${global_planner_plugin_lib} + install(TARGETS ${local_controller_plugin_lib} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} -) + ) -install(FILES src/error_codes/planner_plugins.xml + install(FILES src/error_codes/controller_plugins.xml DESTINATION share/${PROJECT_NAME} -) - -set(smoother_plugin_lib smoother) - -add_library(${smoother_plugin_lib} SHARED src/error_codes/smoother/smoother_error_plugin.cpp) -ament_target_dependencies(${smoother_plugin_lib} ${dependencies}) -target_compile_definitions(${smoother_plugin_lib} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -pluginlib_export_plugin_description_file(nav2_core src/error_codes/smoother_plugins.xml) - -install(TARGETS ${smoother_plugin_lib} + ) + + set(global_planner_plugin_lib global_planner) + + add_library(${global_planner_plugin_lib} SHARED src/error_codes/planner/planner_error_plugin.cpp) + target_link_libraries(${global_planner_plugin_lib} PRIVATE + ${geometry_msgs_TARGETS} + nav2_core::nav2_core ${nav_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros + ) + pluginlib_export_plugin_description_file(nav2_core src/error_codes/planner_plugins.xml) + + install(TARGETS ${global_planner_plugin_lib} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} -) + ) -install(FILES src/error_codes/smoother_plugins.xml + install(FILES src/error_codes/planner_plugins.xml DESTINATION share/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() + ) + + set(smoother_plugin_lib smoother) + + add_library(${smoother_plugin_lib} SHARED src/error_codes/smoother/smoother_error_plugin.cpp) + target_link_libraries(${smoother_plugin_lib} PRIVATE + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros + ) + pluginlib_export_plugin_description_file(nav2_core src/error_codes/smoother_plugins.xml) + + install(TARGETS ${smoother_plugin_lib} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} + ) - find_package(ament_cmake_gtest REQUIRED) - find_package(ament_cmake_pytest REQUIRED) + install(FILES src/error_codes/smoother_plugins.xml + DESTINATION share/${PROJECT_NAME} + ) add_subdirectory(src/behavior_tree) add_subdirectory(src/planning) @@ -126,10 +122,6 @@ if(BUILD_TESTING) add_subdirectory(src/costmap_filters) add_subdirectory(src/error_codes) install(DIRECTORY maps models DESTINATION share/${PROJECT_NAME}) - endif() -ament_export_libraries(${local_controller_plugin_lib}) -ament_export_libraries(${global_planner_plugin_lib}) -ament_export_libraries(${smoother_plugin_lib}) ament_package() diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index e3472e3e94..05046c8d8a 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -10,58 +10,37 @@ ament_cmake nav2_common - rclcpp - rclpy - nav2_util - nav2_map_server - nav2_msgs - nav2_lifecycle_manager - nav2_navfn_planner - nav2_behavior_tree - nav_msgs - visualization_msgs - nav2_amcl - launch_ros - launch_testing - geometry_msgs - std_msgs - tf2_geometry_msgs - launch_ros - launch_testing - nav2_planner - nav2_minimal_tb3_sim - - launch_ros - launch_testing - rclcpp - rclpy - nav2_bringup - nav2_util - nav2_map_server - nav2_msgs - nav2_lifecycle_manager - nav2_navfn_planner - nav2_behavior_tree - nav_msgs - visualization_msgs - geometry_msgs - nav2_amcl - std_msgs - tf2_geometry_msgs - nav2_minimal_tb3_sim - navigation2 - lcov - robot_state_publisher - nav2_planner - - ament_lint_common - ament_lint_auto ament_cmake_gtest ament_cmake_pytest + ament_index_cpp + ament_lint_auto + ament_lint_common + geometry_msgs launch launch_ros launch_testing - python3-zmq + lcov + nav2_amcl + nav2_behavior_tree + nav2_bringup + nav2_lifecycle_manager + nav2_map_server + nav2_minimal_tb3_sim + nav2_msgs + nav2_navfn_planner + nav2_planner + nav2_util + nav_msgs + navigation2 + rclcpp + rclcpp_lifecycle + rclpy + robot_state_publisher + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros + visualization_msgs ament_cmake diff --git a/nav2_system_tests/src/behavior_tree/CMakeLists.txt b/nav2_system_tests/src/behavior_tree/CMakeLists.txt index 19eca116e0..66d4fb1e1f 100644 --- a/nav2_system_tests/src/behavior_tree/CMakeLists.txt +++ b/nav2_system_tests/src/behavior_tree/CMakeLists.txt @@ -1,16 +1,14 @@ -find_package(Boost COMPONENTS system filesystem REQUIRED) - ament_add_gtest(test_behavior_tree_node test_behavior_tree_node.cpp server_handler.cpp ) - -ament_target_dependencies(test_behavior_tree_node - ${dependencies} -) - -target_include_directories(test_behavior_tree_node PUBLIC ${Boost_INCLUDE_DIRS}) target_link_libraries(test_behavior_tree_node - ${Boost_FILESYSTEM_LIBRARY} - ${Boost_SYSTEM_LIBRARY} + ament_index_cpp::ament_index_cpp + behaviortree_cpp::behaviortree_cpp + ${geometry_msgs_TARGETS} + nav2_behavior_tree::nav2_behavior_tree + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + rclcpp::rclcpp + tf2_ros::tf2_ros ) 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 100e5dad93..d8c0070d13 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 @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include -#include +#include #include +#include #include +#include #include -#include -#include +#include #include "gtest/gtest.h" @@ -31,16 +31,18 @@ #include "tf2_ros/create_timer_ros.h" #include "nav2_util/odometry_utils.hpp" +#include "nav2_util/string_utils.hpp" #include "nav2_behavior_tree/plugins_list.hpp" #include "rclcpp/rclcpp.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + #include "server_handler.hpp" using namespace std::chrono_literals; -namespace fs = boost::filesystem; namespace nav2_system_tests { @@ -61,8 +63,7 @@ class BehaviorTreeHandler odom_smoother_ = std::make_shared(node_); - std::vector plugin_libs; - boost::split(plugin_libs, nav2::details::BT_BUILTIN_PLUGINS, boost::is_any_of(";")); + nav2_util::Tokens plugin_libs = nav2_util::split(nav2::details::BT_BUILTIN_PLUGINS, ';'); for (const auto & p : plugin_libs) { factory_.registerFromPlugin(BT::SharedLibrary::getOSName(p)); @@ -194,12 +195,12 @@ std::shared_ptr BehaviorTreeTestFixture::bt_handler = nullp TEST_F(BehaviorTreeTestFixture, TestBTXMLFiles) { - fs::path root = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + std::filesystem::path root = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); root /= "behavior_trees/"; - if (boost::filesystem::exists(root) && boost::filesystem::is_directory(root)) { - for (auto const & entry : boost::filesystem::recursive_directory_iterator(root)) { - if (boost::filesystem::is_regular_file(entry) && entry.path().extension() == ".xml") { + if (std::filesystem::exists(root) && std::filesystem::is_directory(root)) { + for (auto const & entry : std::filesystem::recursive_directory_iterator(root)) { + if (std::filesystem::is_regular_file(entry) && entry.path().extension() == ".xml") { std::cout << entry.path().string() << std::endl; EXPECT_EQ(bt_handler->loadBehaviorTree(entry.path().string()), true); } @@ -216,7 +217,7 @@ TEST_F(BehaviorTreeTestFixture, TestBTXMLFiles) TEST_F(BehaviorTreeTestFixture, TestAllSuccess) { // Load behavior tree from file - fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); bt_file /= "behavior_trees/"; bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); @@ -258,7 +259,7 @@ TEST_F(BehaviorTreeTestFixture, TestAllSuccess) TEST_F(BehaviorTreeTestFixture, TestAllFailure) { // Load behavior tree from file - fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); bt_file /= "behavior_trees/"; bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); @@ -310,7 +311,7 @@ TEST_F(BehaviorTreeTestFixture, TestAllFailure) TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries) { // Load behavior tree from file - fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); bt_file /= "behavior_trees/"; bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); @@ -364,7 +365,7 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries) TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple) { // Load behavior tree from file - fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); bt_file /= "behavior_trees/"; bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); @@ -458,7 +459,7 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple) TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex) { // Load behavior tree from file - fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); bt_file /= "behavior_trees/"; bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); @@ -522,7 +523,7 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex) TEST_F(BehaviorTreeTestFixture, TestRecoverySubtreeGoalUpdated) { // Load behavior tree from file - fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); bt_file /= "behavior_trees/"; bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt b/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt index 740bb77491..0bc3240cc8 100644 --- a/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt +++ b/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt @@ -4,9 +4,17 @@ ament_add_gtest_executable(${test_assisted_teleop_behavior} test_assisted_teleop_behavior_node.cpp assisted_teleop_behavior_tester.cpp ) - -ament_target_dependencies(${test_assisted_teleop_behavior} - ${dependencies} +target_link_libraries(${test_assisted_teleop_behavior} + ${geometry_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + nav2_costmap_2d::nav2_costmap_2d_client + nav2_util::nav2_util_core + ${nav2_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_action::rclcpp_action + ${std_msgs_TARGETS} + tf2::tf2 + tf2_ros::tf2_ros ) ament_add_test(test_assisted_teleop_behavior 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 2cf46827e8..37bdd790c2 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 @@ -22,7 +22,6 @@ #include #include -#include "angles/angles.h" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "geometry_msgs/msg/twist.hpp" diff --git a/nav2_system_tests/src/behaviors/wait/CMakeLists.txt b/nav2_system_tests/src/behaviors/wait/CMakeLists.txt index 36bf50dec8..5e2d4d656b 100644 --- a/nav2_system_tests/src/behaviors/wait/CMakeLists.txt +++ b/nav2_system_tests/src/behaviors/wait/CMakeLists.txt @@ -4,9 +4,15 @@ ament_add_gtest_executable(${test_wait_behavior_exec} test_wait_behavior_node.cpp wait_behavior_tester.cpp ) - -ament_target_dependencies(${test_wait_behavior_exec} - ${dependencies} +target_link_libraries(${test_wait_behavior_exec} + angles::angles + ${geometry_msgs_TARGETS} + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_action::rclcpp_action + tf2::tf2 + tf2_ros::tf2_ros ) ament_add_test(test_wait_behavior 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 71181e24d3..18e131de88 100644 --- a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp +++ b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp @@ -25,7 +25,6 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" -#include "angles/angles.h" #include "nav2_msgs/action/wait.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/node_thread.hpp" diff --git a/nav2_system_tests/src/dummy_controller/CMakeLists.txt b/nav2_system_tests/src/dummy_controller/CMakeLists.txt index c552f861c9..688234b2a8 100644 --- a/nav2_system_tests/src/dummy_controller/CMakeLists.txt +++ b/nav2_system_tests/src/dummy_controller/CMakeLists.txt @@ -2,12 +2,9 @@ add_executable(dummy_controller_node src/dummy_controller/main.cpp src/dummy_controller/dummy_controller.cpp ) - -ament_target_dependencies(dummy_controller_node - rclcpp - std_msgs - nav2_util - nav2_behavior_tree - nav2_msgs - nav_msgs -) \ No newline at end of file +target_link_libraries(dummy_controller_node PRIVATE + ${geometry_msgs_TARGETS} + nav2_behavior_tree::nav2_behavior_tree + nav2_util::nav2_util_core + rclcpp::rclcpp +) diff --git a/nav2_system_tests/src/dummy_planner/CMakeLists.txt b/nav2_system_tests/src/dummy_planner/CMakeLists.txt index b1b4c66a76..14fada8fcc 100644 --- a/nav2_system_tests/src/dummy_planner/CMakeLists.txt +++ b/nav2_system_tests/src/dummy_planner/CMakeLists.txt @@ -2,12 +2,7 @@ add_executable(dummy_planner_node src/dummy_planner/main.cpp src/dummy_planner/dummy_planner.cpp ) - -ament_target_dependencies(dummy_planner_node - rclcpp - std_msgs - nav2_util - nav2_behavior_tree - nav2_msgs - nav_msgs -) \ No newline at end of file +target_link_libraries(dummy_planner_node PRIVATE + nav2_behavior_tree::nav2_behavior_tree + rclcpp::rclcpp +) diff --git a/nav2_system_tests/src/localization/CMakeLists.txt b/nav2_system_tests/src/localization/CMakeLists.txt index 7fa35172c3..9b5a920141 100644 --- a/nav2_system_tests/src/localization/CMakeLists.txt +++ b/nav2_system_tests/src/localization/CMakeLists.txt @@ -1,11 +1,11 @@ ament_add_gtest_executable(test_localization_node test_localization_node.cpp ) -ament_target_dependencies(test_localization_node - ${dependencies} +target_link_libraries(test_localization_node + ${geometry_msgs_TARGETS} + rclcpp::rclcpp ) - ament_add_test(test_localization GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_localization_launch.py" diff --git a/nav2_system_tests/src/localization/test_localization_node.cpp b/nav2_system_tests/src/localization/test_localization_node.cpp index 092b5e9e84..75c69ca506 100644 --- a/nav2_system_tests/src/localization/test_localization_node.cpp +++ b/nav2_system_tests/src/localization/test_localization_node.cpp @@ -15,12 +15,10 @@ #include #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "nav2_amcl/amcl_node.hpp" -#include "std_msgs/msg/string.hpp" #include "geometry_msgs/msg/pose_array.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -using std::placeholders::_1; using namespace std::chrono_literals; // rclcpp::init can only be called once per process, so this needs to be a global variable @@ -52,7 +50,7 @@ class TestAmclPose : public ::testing::Test "initialpose", rclcpp::SystemDefaultsQoS()); subscription_ = node->create_subscription( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&TestAmclPose::amcl_pose_callback, this, _1)); + std::bind(&TestAmclPose::amcl_pose_callback, this, std::placeholders::_1)); initial_pose_pub_->publish(testPose_); } diff --git a/nav2_system_tests/src/planning/CMakeLists.txt b/nav2_system_tests/src/planning/CMakeLists.txt index ed150c7040..2a0e9137e0 100644 --- a/nav2_system_tests/src/planning/CMakeLists.txt +++ b/nav2_system_tests/src/planning/CMakeLists.txt @@ -4,12 +4,18 @@ ament_add_gtest_executable(${test_planner_costmaps_exec} test_planner_costmaps_node.cpp planner_tester.cpp ) - target_link_libraries(${test_planner_costmaps_exec} - ${nav2_map_server_LIBRARIES}) - -ament_target_dependencies(${test_planner_costmaps_exec} - ${dependencies} + ${geometry_msgs_TARGETS} + nav2_map_server::map_io + nav2_map_server::map_server_core + ${nav2_msgs_TARGETS} + nav2_planner::planner_server_core + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} ) set(test_planner_random_exec test_planner_random_node) @@ -18,13 +24,19 @@ ament_add_gtest_executable(${test_planner_random_exec} test_planner_random_node.cpp planner_tester.cpp ) - -ament_target_dependencies(${test_planner_random_exec} - ${dependencies} -) - target_link_libraries(${test_planner_random_exec} - ${nav2_map_server_LIBRARIES}) + ${geometry_msgs_TARGETS} + nav2_map_server::map_io + nav2_map_server::map_server_core + ${nav2_msgs_TARGETS} + nav2_planner::planner_server_core + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} +) ament_add_test(test_planner_costmaps GENERATE_RESULT_FOR_RETURN_CODE_ZERO @@ -47,14 +59,36 @@ ament_add_test(test_planner_random ) ament_add_gtest(test_planner_plugins + planner_tester.cpp test_planner_plugins.cpp TIMEOUT 10 ) - -ament_target_dependencies(test_planner_plugins rclcpp geometry_msgs nav2_msgs ${dependencies}) +target_link_libraries(test_planner_plugins + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_map_server::map_io + nav2_map_server::map_server_core + ${nav2_msgs_TARGETS} + nav2_planner::planner_server_core + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros +) ament_add_gtest(test_planner_is_path_valid - test_planner_is_path_valid.cpp - planner_tester.cpp) - -ament_target_dependencies(test_planner_is_path_valid rclcpp geometry_msgs nav2_msgs ${dependencies}) + planner_tester.cpp + test_planner_is_path_valid.cpp +) +target_link_libraries(test_planner_is_path_valid + ${geometry_msgs_TARGETS} + nav2_map_server::map_io + nav2_map_server::map_server_core + ${nav2_msgs_TARGETS} + nav2_planner::planner_server_core + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros +) diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index 421a00f4b7..0308172600 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -22,18 +22,15 @@ #include #include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" #include "nav2_msgs/action/compute_path_to_pose.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_msgs/msg/costmap.hpp" #include "nav2_msgs/srv/get_costmap.hpp" #include "nav2_msgs/srv/is_path_valid.hpp" -#include "visualization_msgs/msg/marker.hpp" #include "nav2_util/costmap.hpp" #include "nav2_util/node_thread.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2_msgs/msg/tf_message.hpp" #include "nav2_planner/planner_server.hpp" #include "tf2_ros/transform_broadcaster.h" diff --git a/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp b/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp index 958891e02a..cf4b7c90a8 100644 --- a/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp +++ b/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp @@ -16,6 +16,7 @@ #include #include +#include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_msgs/srv/is_path_valid.hpp" #include "rclcpp/rclcpp.hpp" #include "planner_tester.hpp" diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index 0ae08f4fa9..188db2474d 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -17,6 +17,8 @@ #include #include +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" #include "rclcpp/rclcpp.hpp" #include "planner_tester.hpp" #include "nav2_util/lifecycle_utils.hpp" diff --git a/nav2_system_tests/src/updown/CMakeLists.txt b/nav2_system_tests/src/updown/CMakeLists.txt index 273e1828ea..a6260ff35e 100644 --- a/nav2_system_tests/src/updown/CMakeLists.txt +++ b/nav2_system_tests/src/updown/CMakeLists.txt @@ -1,11 +1,11 @@ add_executable(test_updown test_updown.cpp ) - -ament_target_dependencies(test_updown - ${dependencies} +target_link_libraries(test_updown PRIVATE + ${geometry_msgs_TARGETS} + nav2_lifecycle_manager::nav2_lifecycle_manager_core + rclcpp::rclcpp ) install(TARGETS test_updown RUNTIME DESTINATION lib/${PROJECT_NAME}) install(FILES test_updown_launch.py DESTINATION share/${PROJECT_NAME}) - diff --git a/nav2_system_tests/src/updown/test_updown.cpp b/nav2_system_tests/src/updown/test_updown.cpp index 092e0edf46..219a217cb6 100644 --- a/nav2_system_tests/src/updown/test_updown.cpp +++ b/nav2_system_tests/src/updown/test_updown.cpp @@ -19,7 +19,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_lifecycle_manager/lifecycle_manager_client.hpp" -#include "rcutils/cmdline_parser.h" +#include "rclcpp/rclcpp.hpp" using namespace std::chrono_literals; From 6bf9d68af4c485c11947114e0e91ea5b0b5efc27 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 22 Aug 2024 11:30:14 -0700 Subject: [PATCH 21/21] Fix rolling build issue: message filter to hpp from h (#4642) * Fix rolling build issue: message filter to hpp from h Signed-off-by: Steve Macenski * Update amcl_node.hpp Signed-off-by: Steve Macenski * Update static_layer.hpp Signed-off-by: Steve Macenski * Update obstacle_layer.hpp Signed-off-by: Steve Macenski * Update range_sensor_layer.hpp Signed-off-by: Steve Macenski * Update voxel_layer.hpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 2 +- nav2_amcl/src/amcl_node.cpp | 2 +- nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp | 2 +- nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp | 2 +- nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp | 2 +- nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index d8f35b5c7f..0dd6166324 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -29,7 +29,7 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" -#include "message_filters/subscriber.h" +#include "message_filters/subscriber.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_amcl/motion_model/motion_model.hpp" #include "nav2_amcl/sensors/laser/laser.hpp" diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 595ffb7626..5bf17738bd 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -28,7 +28,7 @@ #include #include -#include "message_filters/subscriber.h" +#include "message_filters/subscriber.hpp" #include "nav2_amcl/angleutils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_amcl/pf/pf.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 4b8409f467..858d89b0ac 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -48,7 +48,7 @@ #pragma GCC diagnostic ignored "-Wreorder" #include "tf2_ros/message_filter.h" #pragma GCC diagnostic pop -#include "message_filters/subscriber.h" +#include "message_filters/subscriber.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "sensor_msgs/msg/point_cloud.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp index a4383f1477..8d39116c60 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp @@ -42,7 +42,7 @@ #include #include "map_msgs/msg/occupancy_grid_update.hpp" -#include "message_filters/subscriber.h" +#include "message_filters/subscriber.hpp" #include "nav2_costmap_2d/costmap_layer.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" 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 967a6c3789..f0735904a9 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -43,7 +43,7 @@ #include #include "map_msgs/msg/occupancy_grid_update.hpp" -#include "message_filters/subscriber.h" +#include "message_filters/subscriber.hpp" #include "nav2_costmap_2d/costmap_layer.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp index 888e401439..ed6ee8f7f0 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp @@ -39,7 +39,7 @@ #define NAV2_COSTMAP_2D__VOXEL_LAYER_HPP_ #include -#include "message_filters/subscriber.h" +#include "message_filters/subscriber.hpp" #include #include