diff --git a/.circleci/config.yml b/.circleci/config.yml index d6766347816..841d60042d1 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v36\ + - "<< parameters.key >>-v39\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v36\ + - "<< parameters.key >>-v39\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v36\ + key: "<< parameters.key >>-v39\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ @@ -532,7 +532,7 @@ jobs: _parameters: release_parameters: &release_parameters - packages_skip_regex: "nav2_system_tests" + packages_skip_regex: "'(nav2_system_tests|nav2_smac_planner|nav2_mppi_controller|nav2_route|nav2_rviz_plugins|nav2_rotation_shim_controller|nav2_waypoint_follower|nav2_smoother|opennav_docking|nav2_behaviors|nav2_bringup|navigation2)'" workflows: version: 2 diff --git a/.github/workflows/build_main_against_distros.yml b/.github/workflows/build_main_against_distros.yml index 5b4aa08f1e1..95941856d3e 100644 --- a/.github/workflows/build_main_against_distros.yml +++ b/.github/workflows/build_main_against_distros.yml @@ -17,7 +17,7 @@ jobs: steps: - name: Checkout repository - uses: actions/checkout@v4 + uses: actions/checkout@v5 - name: Set up Docker build context run: | diff --git a/.github/workflows/claude.yml b/.github/workflows/claude.yml index 3ffbfa392fd..73467b184cc 100644 --- a/.github/workflows/claude.yml +++ b/.github/workflows/claude.yml @@ -34,7 +34,7 @@ jobs: id-token: write steps: - name: Checkout repository - uses: actions/checkout@v4 + uses: actions/checkout@v5 with: fetch-depth: 1 diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index 62645e662aa..895bc2152c3 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -13,7 +13,7 @@ jobs: matrix: linter: [xmllint, cpplint, uncrustify, pep257, flake8, mypy] steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@v5 - name: Install typeshed for mypy if: matrix.linter == 'mypy' @@ -30,8 +30,8 @@ jobs: name: pre-commit runs-on: ubuntu-latest steps: - - uses: actions/checkout@v4 - - uses: actions/setup-python@v5 + - uses: actions/checkout@v5 + - uses: actions/setup-python@v6 - uses: pre-commit/action@v3.0.1 env: SKIP: >- diff --git a/.github/workflows/update_ci_image.yaml b/.github/workflows/update_ci_image.yaml index ef39facf09c..3c7743f1ad3 100644 --- a/.github/workflows/update_ci_image.yaml +++ b/.github/workflows/update_ci_image.yaml @@ -66,7 +66,7 @@ jobs: - check_ci_image runs-on: ubuntu-latest steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@v5 - name: Set up Docker Buildx uses: docker/setup-buildx-action@v3 - name: Login to Docker Hub diff --git a/README.md b/README.md index 4ef84c7c30f..d58ef3a8b6b 100644 --- a/README.md +++ b/README.md @@ -8,16 +8,16 @@

For detailed instructions on how to: -- [Getting Started](https://docs.nav2.org/getting_started/index.html) -- [Concepts](https://docs.nav2.org/concepts/index.html) -- [Build](https://docs.nav2.org/development_guides/build_docs/index.html#build) -- [Install](https://docs.nav2.org/development_guides/build_docs/index.html#install) +- [Concepts](https://docs.nav2.org/concepts/index.html) and [Getting Started](https://docs.nav2.org/getting_started/index.html) +- [First Time Setup Guide](https://docs.nav2.org/setup_guides/index.html) +- [ROS Distribution Statuses](https://docs.nav2.org/#distributions) +- [Build & Install](https://docs.nav2.org/development_guides/build_docs/index.html#build) and [Docker Containers](https://github.com/orgs/ros-navigation/packages/container/package/navigation2) - [General Tutorials](https://docs.nav2.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://docs.nav2.org/plugin_tutorials/index.html) -- [Configure](https://docs.nav2.org/configuration/index.html) +- [Configuration Guide](https://docs.nav2.org/configuration/index.html) - [Navigation Plugins](https://docs.nav2.org/plugins/index.html) -- [ROSCon Talks](https://docs.nav2.org/about/roscon.html) +- [API Docs](https://api.nav2.org/) +- [ROSCon Talks](https://docs.nav2.org/about/roscon.html) and [Citations](https://docs.nav2.org/citations.html) - [Migration Guides](https://docs.nav2.org/migration/index.html) -- [Container Images for Building Nav2](https://github.com/orgs/ros-navigation/packages/container/package/navigation2) - [Contribute](https://docs.nav2.org/development_guides/involvement_docs/index.html) Please visit our [documentation site](https://docs.nav2.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-uj428p0x-jKx8U7OzK1IOWp5TnDS2rA) (if this link does not work, please contact maintainers to reactivate). @@ -29,7 +29,7 @@ Please visit our [documentation site](https://docs.nav2.org/). [Please visit our Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community.

- +

### [Dexory](https://www.dexory.com/) develops robotics and AI logistics solutions to drive better business decisions using a digital twin of warehouses to provide inventory insights. @@ -40,6 +40,8 @@ Please thank our amazing sponsors for their generous support of Nav2 on behalf o ### [Stereolabs](https://www.stereolabs.com/) produces the high-quality ZED stereo cameras with a complete vision pipeline from neural depth to SLAM, 3D object tracking, AI and more. +### [3Laws Robotics](https://3laws.io/) provide Supervisor ROS and Pro, easy-to-use dynamic collision avoidance solutions to improve safety and application throughput. + ## Citation If you use the navigation framework, an algorithm from this repository, or ideas from it diff --git a/doc/sponsors_sept_2025.png b/doc/sponsors_sept_2025.png new file mode 100644 index 00000000000..655ad1ce3cd Binary files /dev/null and b/doc/sponsors_sept_2025.png differ diff --git a/nav2_amcl/CMakeLists.txt b/nav2_amcl/CMakeLists.txt index aa6cf1a4ed7..e29f641cb84 100644 --- a/nav2_amcl/CMakeLists.txt +++ b/nav2_amcl/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_amcl) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(message_filters REQUIRED) find_package(nav_msgs REQUIRED) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 0be08c39db6..65787e82b00 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -44,9 +44,9 @@ #include "sensor_msgs/msg/laser_scan.hpp" #include "nav2_ros_common/service_server.hpp" #include "std_srvs/srv/empty.hpp" -#include "tf2_ros/message_filter.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/message_filter.hpp" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/transform_listener.hpp" #define NEW_UNIFORM_SAMPLING 1 diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index c1a2cbc2cea..69ac2d324b5 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -24,6 +24,7 @@ ament_cmake nav2_common + backward_ros geometry_msgs message_filters nav_msgs diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index e7910c583b1..b3b21631a59 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -38,11 +38,11 @@ #include "tf2/utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/LinearMath/Transform.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/message_filter.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/message_filter.hpp" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_amcl/portable_utils.hpp" #include "nav2_ros_common/validate_messages.hpp" @@ -1339,7 +1339,7 @@ AmclNode::initTransforms() get_node_timers_interface(), callback_group_); tf_buffer_->setCreateTimerInterface(timer_interface); - tf_listener_ = std::make_shared(*tf_buffer_); + tf_listener_ = std::make_shared(*tf_buffer_, this, true); tf_broadcaster_ = std::make_shared(shared_from_this()); sent_first_transform_ = false; diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 03339b19cdc..6aa40c5ed05 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_behavior_tree CXX) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(action_msgs REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(geometry_msgs REQUIRED) @@ -18,6 +19,7 @@ find_package(std_srvs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(nav2_ros_common REQUIRED) +find_package(angles REQUIRED) nav2_package() @@ -32,6 +34,7 @@ target_include_directories(${library_name} "$") target_link_libraries(${library_name} PUBLIC ${action_msgs_TARGETS} + angles::angles behaviortree_cpp::behaviortree_cpp ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} @@ -122,8 +125,8 @@ list(APPEND plugin_libs nav2_is_path_valid_condition_bt_node) add_library(nav2_time_expired_condition_bt_node SHARED plugins/condition/time_expired_condition.cpp) list(APPEND plugin_libs nav2_time_expired_condition_bt_node) -add_library(nav2_path_expiring_timer_condition SHARED plugins/condition/path_expiring_timer_condition.cpp) -list(APPEND plugin_libs nav2_path_expiring_timer_condition) +add_library(nav2_path_expiring_timer_condition_bt_node SHARED plugins/condition/path_expiring_timer_condition.cpp) +list(APPEND plugin_libs nav2_path_expiring_timer_condition_bt_node) add_library(nav2_distance_traveled_condition_bt_node SHARED plugins/condition/distance_traveled_condition.cpp) list(APPEND plugin_libs nav2_distance_traveled_condition_bt_node) @@ -218,6 +221,12 @@ list(APPEND plugin_libs nav2_nonblocking_sequence_bt_node) add_library(nav2_round_robin_node_bt_node SHARED plugins/control/round_robin_node.cpp) list(APPEND plugin_libs nav2_round_robin_node_bt_node) +add_library(nav2_pause_resume_controller_bt_node SHARED plugins/control/pause_resume_controller.cpp) +list(APPEND plugin_libs nav2_pause_resume_controller_bt_node) + +add_library(nav2_persistent_sequence_bt_node SHARED plugins/control/persistent_sequence.cpp) +list(APPEND plugin_libs nav2_persistent_sequence_bt_node) + add_library(nav2_single_trigger_bt_node SHARED plugins/decorator/single_trigger_node.cpp) list(APPEND plugin_libs nav2_single_trigger_bt_node) @@ -268,6 +277,7 @@ foreach(bt_plugin ${plugin_libs}) ${std_msgs_TARGETS} ${std_srvs_TARGETS} nav2_ros_common::nav2_ros_common + angles::angles ) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) endforeach() diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 96fb5df0dce..b17a914a541 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -98,6 +98,12 @@ class BehaviorTreeEngine */ void resetGrootMonitor(); + /** + * @brief Function to register a BT from an XML file + * @param file_path Path to BT XML file + */ + void registerTreeFromFile(const std::string & file_path); + /** * @brief Function to explicitly reset all BT nodes to initial state * @param tree Tree to halt diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index d5d1fa4c3a4..924d1ebc28a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -56,7 +56,8 @@ class BtActionServer OnGoalReceivedCallback on_goal_received_callback, OnLoopCallback on_loop_callback, OnPreemptCallback on_preempt_callback, - OnCompletionCallback on_completion_callback); + OnCompletionCallback on_completion_callback, + const std::vector & search_directories = std::vector{}); /** * @brief A destructor for nav2_behavior_tree::BtActionServer class @@ -102,7 +103,8 @@ class BtActionServer * @return bool true if the resulting BT correspond to the one in bt_xml_filename. false * if something went wrong, and previous BT is maintained */ - bool loadBehaviorTree(const std::string & bt_xml_filename = ""); + bool loadBehaviorTree( + const std::string & bt_xml_filename = ""); /** * @brief Getter function for BT Blackboard @@ -245,6 +247,7 @@ class BtActionServer // The XML file that contains the Behavior Tree to create std::string current_bt_xml_filename_; std::string default_bt_xml_filename_; + std::vector search_directories_; // The wrapper class for the BT functionality std::unique_ptr bt_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 184f3d94aca..10a8289d5f4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -42,9 +42,11 @@ BtActionServer::BtActionServer( OnGoalReceivedCallback on_goal_received_callback, OnLoopCallback on_loop_callback, OnPreemptCallback on_preempt_callback, - OnCompletionCallback on_completion_callback) + OnCompletionCallback on_completion_callback, + const std::vector & search_directories) : action_name_(action_name), default_bt_xml_filename_(default_bt_xml_filename), + search_directories_(search_directories), plugin_lib_names_(plugin_lib_names), node_(parent), on_goal_received_callback_(on_goal_received_callback), @@ -246,6 +248,8 @@ void BtActionServer::setGrootMonitoring( template bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filename) { + namespace fs = std::filesystem; + // Empty filename is default for backward compatibility auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename; @@ -255,19 +259,38 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml return true; } - // if a new tree is created, than the Groot2 Publisher must be destroyed + // Reset any existing Groot2 monitoring bt_->resetGrootMonitor(); - // Read the input BT XML from the specified file into a string std::ifstream xml_file(filename); - if (!xml_file.good()) { setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, - "Couldn't open input XML file: " + filename); + "Couldn't open BT XML file: " + filename); return false; } - // Create the Behavior Tree from the XML input + const auto canonical_main_bt = fs::canonical(filename); + + // Register all XML behavior Subtrees found in the given directories + for (const auto & directory : search_directories_) { + try { + for (const auto & entry : fs::directory_iterator(directory)) { + if (entry.path().extension() == ".xml") { + // Skip registering the main tree file + if (fs::equivalent(fs::canonical(entry.path()), canonical_main_bt)) { + continue; + } + bt_->registerTreeFromFile(entry.path().string()); + } + } + } catch (const std::exception & e) { + setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, + "Exception reading behavior tree directory: " + std::string(e.what())); + return false; + } + } + + // Try to load the main BT tree try { tree_ = bt_->createTreeFromFile(filename, blackboard_); for (auto & subtree : tree_.subtrees) { @@ -281,15 +304,15 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml } } catch (const std::exception & e) { setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, - std::string("Exception when loading BT: ") + e.what()); + std::string("Exception when creating BT tree from file: ") + e.what()); return false; } + // Optional logging and monitoring topic_logger_ = std::make_unique(client_node_, tree_); current_bt_xml_filename_ = filename; - // Enable monitoring with Groot2 if (enable_groot_monitoring_) { bt_->addGrootMonitoring(&tree_, groot_server_port_); RCLCPP_DEBUG( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp index ebc29fa9721..cd52705f197 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp @@ -18,6 +18,7 @@ #include #include +#include #include "std_msgs/msg/string.hpp" @@ -102,6 +103,7 @@ class ControllerSelector : public BT::SyncActionNode rclcpp::executors::SingleThreadedExecutor callback_group_executor_; std::string topic_name_; + std::chrono::milliseconds bt_loop_duration_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp index 820a1a7f71f..0b9d9f6b43c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp @@ -18,6 +18,7 @@ #include #include +#include #include "std_msgs/msg/string.hpp" @@ -102,6 +103,7 @@ class GoalCheckerSelector : public BT::SyncActionNode rclcpp::executors::SingleThreadedExecutor callback_group_executor_; std::string topic_name_; + std::chrono::milliseconds bt_loop_duration_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp index 07026ac64f9..269ad96b13b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp @@ -18,6 +18,7 @@ #include #include +#include #include "std_msgs/msg/string.hpp" @@ -103,6 +104,7 @@ class PlannerSelector : public BT::SyncActionNode rclcpp::executors::SingleThreadedExecutor callback_group_executor_; std::string topic_name_; + std::chrono::milliseconds bt_loop_duration_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp index c58f615dbf7..3ff8e397313 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "std_msgs/msg/string.hpp" @@ -101,6 +102,7 @@ class ProgressCheckerSelector : public BT::SyncActionNode rclcpp::executors::SingleThreadedExecutor callback_group_executor_; std::string topic_name_; + std::chrono::milliseconds bt_loop_duration_; }; } // namespace nav2_behavior_tree 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 index a76394e2ffe..d402d37fde5 100644 --- 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 @@ -73,6 +73,9 @@ class RemoveInCollisionGoals : public BtServiceNode BT::InputPort( "consider_unknown_as_obstacle", false, "Whether to consider unknown cost as obstacle"), + BT::InputPort( + "nb_goals_to_consider", 5, + "Number of goals to consider for collision checking"), BT::OutputPort("output_goals", "Goals with in-collision goals removed"), BT::InputPort>("input_waypoint_statuses", @@ -85,6 +88,7 @@ class RemoveInCollisionGoals : public BtServiceNode private: bool use_footprint_; bool consider_unknown_as_obstacle_; + int nb_goals_to_consider_; double cost_threshold_; nav_msgs::msg::Goals input_goals_; }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp index a651318379f..d7271c3fbe1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -62,6 +62,9 @@ class RemovePassedGoals : public BT::ActionNodeBase "Goals with passed viapoints removed"), BT::InputPort("radius", 0.5, "radius to goal for it to be considered for removal"), BT::InputPort("robot_base_frame", "Robot base frame"), + BT::InputPort("nb_goals_to_consider", 1, "Number of waypoints to consider"), + BT::InputPort("yaw", 1.57, + "yaw threshold to goal for it to be considered for removal"), BT::InputPort>("input_waypoint_statuses", "Original waypoint_statuses to mark waypoint status from"), BT::OutputPort>("output_waypoint_statuses", @@ -74,10 +77,12 @@ class RemovePassedGoals : public BT::ActionNodeBase BT::NodeStatus tick() override; double viapoint_achieved_radius_; + double viapoint_achieved_yaw_; double transform_tolerance_; nav2::LifecycleNode::SharedPtr node_; std::shared_ptr tf_; std::string robot_base_frame_; + int nb_goals_to_consider_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp index f752fafb913..6d438262a58 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp @@ -18,6 +18,7 @@ #include #include +#include #include "std_msgs/msg/string.hpp" @@ -102,6 +103,7 @@ class SmootherSelector : public BT::SyncActionNode rclcpp::executors::SingleThreadedExecutor callback_group_executor_; std::string topic_name_; + std::chrono::milliseconds bt_loop_duration_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp index 6ce8a76fd64..9ffc5aa773f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp @@ -25,7 +25,7 @@ #include "nav_msgs/msg/path.hpp" #include "nav2_behavior_tree/bt_utils.hpp" #include "nav2_behavior_tree/json_utils.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_ros_common/lifecycle_node.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp index db615c06181..eb099f329b0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp @@ -20,7 +20,7 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "behaviortree_cpp/condition_node.h" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp index abcf722b872..c75f8b78d98 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp @@ -23,7 +23,7 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp index aeebfcb3634..db3c56f24b7 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp @@ -23,7 +23,7 @@ #include "behaviortree_cpp/json_export.h" #include "nav2_behavior_tree/bt_utils.hpp" #include "nav2_behavior_tree/json_utils.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp index 2c95e1e880e..d456d8092c1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "nav2_ros_common/lifecycle_node.hpp" #include "sensor_msgs/msg/battery_state.hpp" @@ -85,6 +86,7 @@ class IsBatteryChargingCondition : public BT::ConditionNode nav2::Subscription::SharedPtr battery_sub_; std::string battery_topic_; bool is_battery_charging_; + std::chrono::milliseconds bt_loop_duration_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp index d66eb5eea72..de31aa1c4e0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "nav2_ros_common/lifecycle_node.hpp" #include "sensor_msgs/msg/battery_state.hpp" @@ -93,6 +94,7 @@ class IsBatteryLowCondition : public BT::ConditionNode double min_battery_; bool is_voltage_; bool is_battery_low_; + std::chrono::milliseconds bt_loop_duration_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.hpp new file mode 100644 index 00000000000..b434e75f1d3 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.hpp @@ -0,0 +1,81 @@ +// Copyright (c) 2024 Jakub Chudziński +// +// 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__CONDITION__IS_GOAL_NEARBY_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_GOAL_NEARBY_CONDITION_HPP_ + +#include +#include "nav_msgs/msg/path.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp/condition_node.h" + + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ConditionNode that returns SUCCESS when the IsGoalNearby + * service returns true and FAILURE otherwise + */ +class IsGoalNearbyCondition : public BT::ConditionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::IsGoalNearbyCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ + IsGoalNearbyCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + IsGoalNearbyCondition() = delete; + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort("path", "Planned Path"), + BT::InputPort( + "proximity_threshold", 3.0, + "Proximity length (m) of the remaining path considered as a nearby"), + }; + } + +private: + + /** + * @brief Checks if the robot is in the goal proximity + * @param goal_path current planned path to the goal + * @param prox_thr proximity length (m) of the remaining path considered as a nearby + * @return whether the robot is in the goal proximity + */ + bool isRobotInGoalProximity( + const nav_msgs::msg::Path& goal_path, + const double& prox_thr); +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_GOAL_NEARBY_CONDITION_HPP_ \ No newline at end of file diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp index 334eaefd6f0..f13ed225aaf 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp @@ -73,7 +73,7 @@ class IsPathValidCondition : public BT::ConditionNode return { BT::InputPort("path", "Path to Check"), BT::InputPort("server_timeout"), - BT::InputPort("max_cost", 253, "Maximum cost of the path"), + BT::InputPort("max_cost", 254, "Maximum cost of the path"), BT::InputPort( "consider_unknown_as_obstacle", false, "Whether to consider unknown cost as obstacle") diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp index 41a86ee813d..be1bcd6ee18 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp @@ -21,7 +21,7 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "behaviortree_cpp/condition_node.h" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp new file mode 100644 index 00000000000..fa5bfa35f2f --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp @@ -0,0 +1,153 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_ + +// Other includes +#include +#include +#include + +// ROS includes +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/callback_group.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "behaviortree_cpp/control_node.h" +#include "nav2_ros_common/service_server.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" + +// Interface definitions +#include "std_srvs/srv/trigger.hpp" + + +namespace nav2_behavior_tree +{ + +using Trigger = std_srvs::srv::Trigger; + +enum state_t {RESUMED, PAUSED, PAUSE_REQUESTED, ON_PAUSE, RESUME_REQUESTED, ON_RESUME}; +const std::map state_names = { + {RESUMED, "RESUMED"}, + {PAUSED, "PAUSED"}, + {PAUSE_REQUESTED, "PAUSE_REQUESTED"}, + {ON_PAUSE, "ON_PAUSE"}, + {RESUME_REQUESTED, "RESUME_REQUESTED"}, + {ON_RESUME, "ON_RESUME"} +}; +const std::map child_indices = { + {RESUMED, 0}, + {PAUSED, 1}, + {ON_PAUSE, 2}, + {ON_RESUME, 3} +}; + +/* @brief Controlled through service calls to pause and resume the execution of the tree + * It has one mandatory child for the RESUMED, and three optional for the PAUSED state, + * the ON_PAUSE event and the ON_RESUME event. + * It has two input ports: + * - pause_service_name: name of the service to pause + * - resume_service_name: name of the service to resume + * + * Usage: + * + * + * + * + * + * + * + * + * + * + * The controller starts in RESUMED state, and ticks it until it returns success. + * When the pause service is called, ON_PAUSE is ticked until completion, + * then the controller switches to PAUSED state. + * When the resume service is called, ON_RESUME is ticked until completion, + * then the controller switches back to RESUMED state. + * + * The controller only returns success when the RESUMED child returns success. + * The controller returns failure if any child returns failure. + * In any other case, it returns running. + */ + + +class PauseResumeController : public BT::ControlNode +{ +public: + //! @brief Constructor + PauseResumeController( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + //! @brief Reset state and go to Idle + void halt() override; + + //! @brief Handle transitions if requested and tick child related to the actual state + BT::NodeStatus tick() override; + + //! @brief Declare ports + static BT::PortsList providedPorts() + { + return { + BT::InputPort( + "pause_service_name", + "Name of the service to pause"), + BT::InputPort( + "resume_service_name", + "Name of the service to resume"), + }; + } + + [[nodiscard]] inline state_t getState() const + { + return state_; + } + +private: + //! @brief Set state to PAUSE_REQUESTED + void pauseServiceCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response); + + //! @brief Set state to RESUME_REQUESTED + void resumeServiceCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response); + + /** @brief Switch to the next state based on the current state + * + * PAUSE_REQUESTED -> ON_PAUSE + * ON_PAUSE -> PAUSED + * + * RESUME_REQUESTED -> ON_RESUME + * ON_RESUME -> RESUMED + * + * Do nothing if in end state + */ + void switchToNextState(); + + rclcpp::Logger logger_{rclcpp::get_logger("PauseResumeController")}; + rclcpp::CallbackGroup::SharedPtr cb_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + nav2::ServiceServer::SharedPtr pause_srv_; + nav2::ServiceServer::SharedPtr resume_srv_; + state_t state_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/persistent_sequence.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/persistent_sequence.hpp new file mode 100644 index 00000000000..79d494eb8e6 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/persistent_sequence.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PERSISTENT_SEQUENCE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PERSISTENT_SEQUENCE_HPP_ + +#include +#include "behaviortree_cpp/control_node.h" +#include "behaviortree_cpp/bt_factory.h" + +namespace nav2_behavior_tree +{ +/** + * @brief The PersistentSequenceNode is similar to the SequenceNode, but it + * stores the index of the last running child in the blackboard (key: "current_child_idx"), + * and it does not reset the index when it got halted. + * It used to tick children in an ordered sequence. If any child returns RUNNING, previous + * children will NOT be ticked again. + * + * - If all the children return SUCCESS, this node returns SUCCESS. + * + * - If a child returns RUNNING, this node returns RUNNING. + * Loop is NOT restarted, the same running child will be ticked again. + * + * - If a child returns FAILURE, stop the loop and return FAILURE. + * Restart the loop only if (reset_on_failure == true) + * + */ +class PersistentSequenceNode : public BT::ControlNode +{ +public: + PersistentSequenceNode(const std::string & name, const BT::NodeConfiguration & conf); + + ~PersistentSequenceNode() override = default; + + //! @brief Declare ports + static BT::PortsList providedPorts() + { + return { + BT::BidirectionalPort("current_child_idx", "The index of the current child"), + }; + } + +private: + BT::NodeStatus tick() override; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PERSISTENT_SEQUENCE_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp index 64e4a7c4a75..782e89a8f89 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp @@ -20,7 +20,7 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "behaviortree_cpp/decorator_node.h" #include "nav2_behavior_tree/bt_utils.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index 2c6d69895e6..61a909c53c6 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -19,6 +19,7 @@ #include #include +#include #include "behaviortree_cpp/decorator_node.h" #include "behaviortree_cpp/json_export.h" @@ -110,6 +111,7 @@ class GoalUpdater : public BT::DecoratorNode rclcpp::executors::SingleThreadedExecutor callback_group_executor_; std::string goal_updater_topic_; std::string goals_updater_topic_; + std::chrono::milliseconds bt_loop_duration_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp index a8d27ecd96f..57ca3d4ffc3 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp @@ -24,7 +24,7 @@ #include "nav2_msgs/msg/behavior_tree_log.hpp" #include "nav2_msgs/msg/behavior_tree_status_change.hpp" #include "tf2/time.hpp" -#include "tf2_ros/buffer_interface.h" +#include "tf2_ros/buffer_interface.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index a9798c0c64c..230ce02f70c 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -404,6 +404,16 @@ + + + Service to call to pause + Service to call to resume + + + + Index of the child to execute + + Rate diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index e931df421e2..69bcb1466f7 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -12,6 +12,7 @@ ament_cmake action_msgs + backward_ros behaviortree_cpp geometry_msgs nav2_common diff --git a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp index 860f1ed12de..eee98bd7861 100644 --- a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp @@ -33,9 +33,8 @@ ControllerSelector::ControllerSelector( : BT::SyncActionNode(name, conf) { initialize(); - - // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin - callback_group_executor_.spin_some(std::chrono::nanoseconds(1)); + bt_loop_duration_ = + config().blackboard->template get("bt_loop_duration"); } void ControllerSelector::initialize() @@ -69,7 +68,7 @@ BT::NodeStatus ControllerSelector::tick() initialize(); } - callback_group_executor_.spin_some(); + callback_group_executor_.spin_all(bt_loop_duration_); // This behavior always use the last selected controller received from the topic input. // When no input is specified it uses the default controller. diff --git a/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp b/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp index 20df1bcca9c..0b1fbae15c8 100644 --- a/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp @@ -33,6 +33,8 @@ GoalCheckerSelector::GoalCheckerSelector( : BT::SyncActionNode(name, conf) { initialize(); + bt_loop_duration_ = + config().blackboard->template get("bt_loop_duration"); } void GoalCheckerSelector::initialize() @@ -66,7 +68,7 @@ BT::NodeStatus GoalCheckerSelector::tick() initialize(); } - callback_group_executor_.spin_some(); + callback_group_executor_.spin_all(bt_loop_duration_); // This behavior always use the last selected goal checker received from the topic input. // When no input is specified it uses the default goal checker. diff --git a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp index 6dd1806d67a..8dc248d032e 100644 --- a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp @@ -33,9 +33,8 @@ PlannerSelector::PlannerSelector( : BT::SyncActionNode(name, conf) { initialize(); - - // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin - callback_group_executor_.spin_some(std::chrono::nanoseconds(1)); + bt_loop_duration_ = + config().blackboard->template get("bt_loop_duration"); } void PlannerSelector::initialize() @@ -69,7 +68,7 @@ BT::NodeStatus PlannerSelector::tick() initialize(); } - callback_group_executor_.spin_some(); + callback_group_executor_.spin_all(bt_loop_duration_); // This behavior always use the last selected planner received from the topic input. // When no input is specified it uses the default planner. diff --git a/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp index 71d0b308e2a..458c0253e41 100644 --- a/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp @@ -32,6 +32,8 @@ ProgressCheckerSelector::ProgressCheckerSelector( : BT::SyncActionNode(name, conf) { initialize(); + bt_loop_duration_ = + config().blackboard->template get("bt_loop_duration"); } void ProgressCheckerSelector::initialize() @@ -65,7 +67,7 @@ BT::NodeStatus ProgressCheckerSelector::tick() initialize(); } - callback_group_executor_.spin_some(); + callback_group_executor_.spin_all(bt_loop_duration_); // This behavior always use the last selected progress checker received from the topic input. // When no input is specified it uses the default goaprogressl checker. diff --git a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp index 113c3a2f514..9c52804205b 100644 --- a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp @@ -39,6 +39,7 @@ void RemoveInCollisionGoals::on_tick() getInput("cost_threshold", cost_threshold_); getInput("input_goals", input_goals_); getInput("consider_unknown_as_obstacle", consider_unknown_as_obstacle_); + getInput("nb_goals_to_consider", nb_goals_to_consider_); if (input_goals_.goals.empty()) { setOutput("output_goals", input_goals_); @@ -48,8 +49,11 @@ void RemoveInCollisionGoals::on_tick() request_ = std::make_shared(); request_->use_footprint = use_footprint_; - for (const auto & goal : input_goals_.goals) { - request_->poses.push_back(goal); + for (size_t i = + 0; i < static_cast(nb_goals_to_consider_) && i < input_goals_.goals.size(); + ++i) + { + request_->poses.push_back(input_goals_.goals[i]); } } @@ -67,35 +71,31 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion( // get the `waypoint_statuses` vector std::vector waypoint_statuses; auto waypoint_statuses_get_res = getInput("input_waypoint_statuses", waypoint_statuses); - if (!waypoint_statuses_get_res) { - RCLCPP_ERROR(node_->get_logger(), "Missing [input_waypoint_statuses] port input!"); - } - nav_msgs::msg::Goals valid_goal_poses; - for (size_t i = 0; i < response->costs.size(); ++i) { - if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) || - response->costs[i] < cost_threshold_) + for (int i = static_cast(response->costs.size()) - 1; i >= 0; --i) { + if ((response->costs[i] != 255 || consider_unknown_as_obstacle_) && + response->costs[i] >= cost_threshold_) { - valid_goal_poses.goals.push_back(input_goals_.goals[i]); - } else if (waypoint_statuses_get_res) { - using namespace nav2_util::geometry_utils; // NOLINT - auto cur_waypoint_index = - find_next_matching_goal_in_waypoint_statuses(waypoint_statuses, input_goals_.goals[i]); - if (cur_waypoint_index == -1) { - RCLCPP_ERROR(node_->get_logger(), "Failed to find matching goal in waypoint_statuses"); - return BT::NodeStatus::FAILURE; + if (waypoint_statuses_get_res) { + const int cur_waypoint_index = + nav2_util::geometry_utils::find_next_matching_goal_in_waypoint_statuses( + waypoint_statuses, input_goals_.goals[i]); + + if (cur_waypoint_index >= 0 && + static_cast(cur_waypoint_index) < waypoint_statuses.size()) { + waypoint_statuses[static_cast(cur_waypoint_index)].waypoint_status = + nav2_msgs::msg::WaypointStatus::SKIPPED; + } else { + RCLCPP_WARN( + node_->get_logger(), + "RemoveInCollisionGoals: No matching waypoint found for goal index %d; " + "skipping waypoint_statuses update.", i); + } } - waypoint_statuses[cur_waypoint_index].waypoint_status = - nav2_msgs::msg::WaypointStatus::SKIPPED; + input_goals_.goals.erase(input_goals_.goals.begin() + i); } } - // Inform if all goals have been removed - if (valid_goal_poses.goals.empty()) { - RCLCPP_INFO( - node_->get_logger(), - "All goals are in collision and have been removed from the list"); - } - setOutput("output_goals", valid_goal_poses); + setOutput("output_goals", input_goals_); // set `waypoint_statuses` output setOutput("output_waypoint_statuses", waypoint_statuses); 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 335d5e60c37..1fe5a5bd3f6 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -18,6 +18,8 @@ #include "nav_msgs/msg/path.hpp" #include "nav2_util/geometry_utils.hpp" +#include "tf2/utils.h" +#include "angles/angles.h" #include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp" @@ -28,12 +30,14 @@ RemovePassedGoals::RemovePassedGoals( const std::string & name, const BT::NodeConfiguration & conf) : BT::ActionNodeBase(name, conf), - viapoint_achieved_radius_(0.5) + viapoint_achieved_radius_(0.5), + viapoint_achieved_yaw_(1.57) {} void RemovePassedGoals::initialize() { getInput("radius", viapoint_achieved_radius_); + getInput("yaw", viapoint_achieved_yaw_); // new input for yaw threshold tf_ = config().blackboard->get>("tf_buffer"); node_ = config().blackboard->get("node"); @@ -56,6 +60,9 @@ inline BT::NodeStatus RemovePassedGoals::tick() setOutput("output_goals", goal_poses); return BT::NodeStatus::SUCCESS; } + getInput("nb_goals_to_consider", nb_goals_to_consider_); + nb_goals_to_consider_ = std::min(nb_goals_to_consider_, + static_cast(goal_poses.goals.size())); using namespace nav2_util::geometry_utils; // NOLINT @@ -70,33 +77,47 @@ inline BT::NodeStatus RemovePassedGoals::tick() // get the `waypoint_statuses` vector std::vector waypoint_statuses; auto waypoint_statuses_get_res = getInput("input_waypoint_statuses", waypoint_statuses); - if (!waypoint_statuses_get_res) { - RCLCPP_ERROR_ONCE(node_->get_logger(), "Missing [input_waypoint_statuses] port input!"); - } - - double dist_to_goal; - while (goal_poses.goals.size() > 1) { - dist_to_goal = euclidean_distance(goal_poses.goals[0].pose, current_pose.pose); - if (dist_to_goal > viapoint_achieved_radius_) { + bool goal_reached = false; + int reached_goal_index = -1; + + // Iterate over the first `nb_goals_to_consider` goals + for (int i = 0; i < nb_goals_to_consider_; ++i) { + double dist_to_goal = euclidean_distance(goal_poses.goals[i].pose, current_pose.pose); + double yaw_goal = tf2::getYaw(goal_poses.goals[i].pose.orientation); + double yaw_current = tf2::getYaw(current_pose.pose.orientation); + double yaw_diff = std::fabs(angles::shortest_angular_distance(yaw_current, yaw_goal)); + if ((dist_to_goal <= viapoint_achieved_radius_ && yaw_diff <= viapoint_achieved_yaw_) + || dist_to_goal <= 0.2) // Added a small tolerance without yaw check to avoid robot stucked near collision (allow to achieve pose without orientation) + { + reached_goal_index = i; + goal_reached = true; break; } - - // mark waypoint statuses before the goal is erased from goals + } + if (goal_reached) { if (waypoint_statuses_get_res) { - auto cur_waypoint_index = - find_next_matching_goal_in_waypoint_statuses(waypoint_statuses, goal_poses.goals[0]); - if (cur_waypoint_index == -1) { - RCLCPP_ERROR_ONCE(node_->get_logger(), "Failed to find matching goal in waypoint_statuses"); - return BT::NodeStatus::FAILURE; + for (int i = 0; i <= reached_goal_index; ++i) { + auto cur_waypoint_index = + find_next_matching_goal_in_waypoint_statuses(waypoint_statuses, goal_poses.goals[i]); + if (cur_waypoint_index >= 0 && + cur_waypoint_index < static_cast(waypoint_statuses.size())) + { + waypoint_statuses.at(cur_waypoint_index).waypoint_status = + (i == reached_goal_index) ? nav2_msgs::msg::WaypointStatus::COMPLETED : + nav2_msgs::msg::WaypointStatus::SKIPPED; + } else { + RCLCPP_WARN(node_->get_logger(), + "RemovePassedGoals: No matching waypoint found for goal %d", i); + } } - waypoint_statuses[cur_waypoint_index].waypoint_status = - nav2_msgs::msg::WaypointStatus::COMPLETED; } - - goal_poses.goals.erase(goal_poses.goals.begin()); + // If the reached goal is NOT the last one, erase all COMPLETED and SKIPPED goals + if (reached_goal_index < static_cast(goal_poses.goals.size()) - 1) { + goal_poses.goals.erase(goal_poses.goals.begin(), + goal_poses.goals.begin() + reached_goal_index + 1); + } } - setOutput("output_goals", goal_poses); // set `waypoint_statuses` output setOutput("output_waypoint_statuses", waypoint_statuses); diff --git a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp index 1e7768e7020..7c17a97561f 100644 --- a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp @@ -34,9 +34,8 @@ SmootherSelector::SmootherSelector( : BT::SyncActionNode(name, conf) { initialize(); - - // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin - callback_group_executor_.spin_some(std::chrono::nanoseconds(1)); + bt_loop_duration_ = + config().blackboard->template get("bt_loop_duration"); } void SmootherSelector::initialize() @@ -70,7 +69,7 @@ BT::NodeStatus SmootherSelector::tick() initialize(); } - callback_group_executor_.spin_some(); + callback_group_executor_.spin_all(bt_loop_duration_); // This behavior always use the last selected smoother received from the topic input. // When no input is specified it uses the default smoother. diff --git a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp index bf8e58b9184..7cae783bb9f 100644 --- a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp +++ b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp @@ -25,8 +25,8 @@ #include "nav_msgs/msg/path.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/LinearMath/Quaternion.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp" diff --git a/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp index 571cf10d64f..dab7e019dd0 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp @@ -27,9 +27,8 @@ IsBatteryChargingCondition::IsBatteryChargingCondition( is_battery_charging_(false) { initialize(); - - // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin - callback_group_executor_.spin_some(std::chrono::nanoseconds(1)); + bt_loop_duration_ = + config().blackboard->template get("bt_loop_duration"); } void IsBatteryChargingCondition::initialize() @@ -65,7 +64,7 @@ BT::NodeStatus IsBatteryChargingCondition::tick() initialize(); } - callback_group_executor_.spin_some(); + callback_group_executor_.spin_all(bt_loop_duration_); if (is_battery_charging_) { return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp index 628b31145b4..57898da658b 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -30,9 +30,8 @@ IsBatteryLowCondition::IsBatteryLowCondition( is_battery_low_(false) { initialize(); - - // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin - callback_group_executor_.spin_some(std::chrono::nanoseconds(1)); + bt_loop_duration_ = + config().blackboard->template get("bt_loop_duration"); } void IsBatteryLowCondition::initialize() @@ -71,7 +70,7 @@ BT::NodeStatus IsBatteryLowCondition::tick() initialize(); } - callback_group_executor_.spin_some(); + callback_group_executor_.spin_all(bt_loop_duration_); if (is_battery_low_) { return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.cpp b/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.cpp new file mode 100644 index 00000000000..5b2855ebefd --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.cpp @@ -0,0 +1,42 @@ +#include "nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.hpp" +#include "nav2_util/geometry_utils.hpp" + +namespace nav2_behavior_tree +{ + +IsGoalNearbyCondition::IsGoalNearbyCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf) +{ +} + +bool IsGoalNearbyCondition::isRobotInGoalProximity( + const nav_msgs::msg::Path& goal_path, + const double& prox_thr) +{ + return nav2_util::geometry_utils::calculate_path_length(goal_path, 0) < prox_thr; +} + + +BT::NodeStatus IsGoalNearbyCondition::tick() +{ + nav_msgs::msg::Path path; + double prox_thr; + getInput("path", path); + getInput("proximity_threshold", prox_thr); + if (!path.poses.empty()){ + if(isRobotInGoalProximity(path,prox_thr)){ + return BT::NodeStatus::SUCCESS; + } + } + return BT::NodeStatus::FAILURE; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("IsGoalNearby"); +} diff --git a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp index b377d47c585..914a2be547a 100644 --- a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp @@ -18,7 +18,7 @@ #include "rclcpp/rclcpp.hpp" #include "tf2/time.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_behavior_tree/plugins/condition/transform_available_condition.hpp" diff --git a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp new file mode 100644 index 00000000000..60b269fa2e0 --- /dev/null +++ b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp @@ -0,0 +1,188 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Other includes +#include + +// ROS includes +#include "nav2_behavior_tree/plugins/control/pause_resume_controller.hpp" + +// Other includes +#include "behaviortree_cpp/bt_factory.h" + +namespace nav2_behavior_tree +{ + +using namespace std::placeholders; + +PauseResumeController::PauseResumeController( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf) +: BT::ControlNode(xml_tag_name, conf) +{ + auto node = config().blackboard->get("node"); + state_ = RESUMED; + + // Create a separate cb group with a separate executor to spin + cb_group_ = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + + executor_ = + std::make_shared(); + + executor_->add_callback_group(cb_group_, node->get_node_base_interface()); + + std::string pause_service_name; + getInput("pause_service_name", pause_service_name); + pause_srv_ = node->create_service( + pause_service_name, + std::bind(&PauseResumeController::pauseServiceCallback, this, _1, _2, _3), + cb_group_); + + std::string resume_service_name; + getInput("resume_service_name", resume_service_name); + resume_srv_ = node->create_service( + resume_service_name, + std::bind(&PauseResumeController::resumeServiceCallback, this, _1, _2, _3), + cb_group_); +} + +BT::NodeStatus PauseResumeController::tick() +{ + unsigned int children_count = children_nodes_.size(); + if (children_count < 1 || children_count > 4) { + throw std::runtime_error( + "PauseNode must have at least one and at most four children " + "(currently has " + std::to_string(children_count) + ")"); + } + + if (status() == BT::NodeStatus::IDLE) { + state_ = RESUMED; + } + setStatus(BT::NodeStatus::RUNNING); + + executor_->spin_some(); + + // If pause / resume requested, reset children and switch to transient child + if (state_ == PAUSE_REQUESTED || state_ == RESUME_REQUESTED) { + resetChildren(); + switchToNextState(); + } + + // Return RUNNING and do nothing if specific child is not used + const uint16_t child_idx = child_indices.at(state_); + if (children_nodes_.size() <= child_idx) { + switchToNextState(); + return BT::NodeStatus::RUNNING; + } + + // If child is used, tick it + const BT::NodeStatus child_status = + children_nodes_[child_indices.at(state_)]->executeTick(); + + switch (child_status) { + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; + case BT::NodeStatus::SUCCESS: + case BT::NodeStatus::SKIPPED: + if (state_ == RESUMED) { + // Resumed child returned SUCCESS, we're done + return BT::NodeStatus::SUCCESS; + } + switchToNextState(); + // If any branch other than RESUMED returned SUCCESS, keep ticking + return BT::NodeStatus::RUNNING; + case BT::NodeStatus::FAILURE: + RCLCPP_ERROR( + logger_, "%s child returned FAILURE", state_names.at(state_).c_str()); + return BT::NodeStatus::FAILURE; + default: + throw std::runtime_error("A child node must never return IDLE"); + } +} + +void PauseResumeController::switchToNextState() +{ + static const std::map next_states = { + {PAUSE_REQUESTED, ON_PAUSE}, + {ON_PAUSE, PAUSED}, + {RESUME_REQUESTED, ON_RESUME}, + {ON_RESUME, RESUMED} + }; + + if (state_ == PAUSED || state_ == RESUMED) { + // No next state, do nothing + return; + } + + state_ = next_states.at(state_); + RCLCPP_INFO(logger_, "Switched to state: %s", state_names.at(state_).c_str()); +} + +void PauseResumeController::pauseServiceCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response) +{ + if (state_ != PAUSED) { + RCLCPP_INFO(logger_, "Received pause request"); + response->success = true; + state_ = PAUSE_REQUESTED; + return; + } + + std::string warn_message = "PauseResumeController BT node already in state PAUSED"; + RCLCPP_WARN(logger_, "%s", warn_message.c_str()); + response->success = false; + response->message = warn_message; +} + +void PauseResumeController::resumeServiceCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response) +{ + if (state_ == PAUSED) { + RCLCPP_INFO(logger_, "Received resume request"); + response->success = true; + state_ = RESUME_REQUESTED; + return; + } + + std::string warn_message = "PauseResumeController BT node not in state PAUSED"; + RCLCPP_WARN(logger_, "%s", warn_message.c_str()); + response->success = false; + response->message = warn_message; +} + +void PauseResumeController::halt() +{ + state_ = RESUMED; + ControlNode::halt(); +} + +} // namespace nav2_behavior_tree + +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory.registerBuilder( + "PauseResumeController", builder); +} diff --git a/nav2_behavior_tree/plugins/control/persistent_sequence.cpp b/nav2_behavior_tree/plugins/control/persistent_sequence.cpp new file mode 100644 index 00000000000..1886deb3d5c --- /dev/null +++ b/nav2_behavior_tree/plugins/control/persistent_sequence.cpp @@ -0,0 +1,84 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_behavior_tree/plugins/control/persistent_sequence.hpp" +#include "behaviortree_cpp/action_node.h" +#include "behaviortree_cpp/bt_factory.h" + +namespace nav2_behavior_tree +{ + +PersistentSequenceNode::PersistentSequenceNode( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ControlNode::ControlNode(name, conf) {} + +BT::NodeStatus PersistentSequenceNode::tick() +{ + const int children_count = children_nodes_.size(); + + int current_child_idx; + getInput("current_child_idx", current_child_idx); + + setStatus(BT::NodeStatus::RUNNING); + + while (current_child_idx < children_count) { + TreeNode * current_child_node = children_nodes_[current_child_idx]; + const BT::NodeStatus child_status = current_child_node->executeTick(); + + switch (child_status) { + case BT::NodeStatus::RUNNING: + return child_status; + + case BT::NodeStatus::FAILURE: + // Reset on failure + resetChildren(); + current_child_idx = 0; + setOutput("current_child_idx", 0); + return child_status; + + case BT::NodeStatus::SUCCESS: + case BT::NodeStatus::SKIPPED: + // Skip the child node + current_child_idx++; + setOutput("current_child_idx", current_child_idx); + break; + + case BT::NodeStatus::IDLE: + throw std::runtime_error("A child node must never return IDLE"); + } // end switch + } // end while loop + + // The entire while loop completed. This means that all the children returned SUCCESS. + if (current_child_idx >= children_count) { + resetChildren(); + setOutput("current_child_idx", 0); + } + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory.registerBuilder( + "PersistentSequence", builder); +} diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp index 0fdd47e7ffb..b560cddf2e7 100644 --- a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp @@ -21,7 +21,7 @@ #include "nav2_util/robot_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "behaviortree_cpp/decorator_node.h" diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index 2cf6031ce19..d5b67155d30 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -36,9 +36,8 @@ GoalUpdater::GoalUpdater( goals_updater_topic_("goals_update") { initialize(); - - // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin - callback_group_executor_.spin_all(std::chrono::milliseconds(1)); + bt_loop_duration_ = + config().blackboard->template get("bt_loop_duration"); } void GoalUpdater::initialize() @@ -91,7 +90,7 @@ inline BT::NodeStatus GoalUpdater::tick() getInput("input_goal", goal); getInput("input_goals", goals); - callback_group_executor_.spin_all(std::chrono::milliseconds(49)); + callback_group_executor_.spin_all(bt_loop_duration_); if (last_goal_received_set_) { if (last_goal_received_.header.stamp == rclcpp::Time(0)) { diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 21131be010a..a2b93f7d086 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -102,6 +102,13 @@ BehaviorTreeEngine::createTreeFromFile( return factory_.createTreeFromFile(file_path, blackboard); } +/// @brief Register a tree from an XML file and return the tree +void BehaviorTreeEngine::registerTreeFromFile( + const std::string & file_path) +{ + factory_.registerBehaviorTreeFromFile(file_path); +} + void BehaviorTreeEngine::addGrootMonitoring( BT::Tree * tree, diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp index ec1abd18e59..9f686ad2ed4 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp @@ -44,6 +44,9 @@ class ControllerSelectorTestFixture : public ::testing::Test config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard config_->blackboard->set("node", node_); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { return std::make_unique(name, config); diff --git a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp index fb4a40c0ffc..0067a5bd7ed 100644 --- a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp @@ -44,6 +44,9 @@ class GoalCheckerSelectorTestFixture : public ::testing::Test config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard config_->blackboard->set("node", node_); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { return std::make_unique(name, config); diff --git a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp index 3308abe5efa..7043d193789 100644 --- a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp @@ -44,6 +44,9 @@ class PlannerSelectorTestFixture : public ::testing::Test config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard config_->blackboard->set("node", node_); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { return std::make_unique(name, config); diff --git a/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp index c7c70e742e9..eebb6c193b4 100644 --- a/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp @@ -43,6 +43,9 @@ class ProgressCheckerSelectorTestFixture : public ::testing::Test config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard config_->blackboard->set("node", node_); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { return std::make_unique(name, config); diff --git a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp index 002610472f4..50238346741 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp @@ -44,6 +44,9 @@ class SmootherSelectorTestFixture : public ::testing::Test config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard config_->blackboard->set("node", node_); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { return std::make_unique(name, config); diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt index d7d32012733..51f7a908801 100644 --- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt @@ -2,7 +2,7 @@ plugin_add_test(test_condition_distance_traveled test_distance_traveled.cpp nav2 plugin_add_test(test_condition_time_expired test_time_expired.cpp nav2_time_expired_condition_bt_node) -plugin_add_test(test_condition_path_expiring_timer test_path_expiring_timer.cpp nav2_path_expiring_timer_condition) +plugin_add_test(test_condition_path_expiring_timer test_path_expiring_timer.cpp nav2_path_expiring_timer_condition_bt_node) plugin_add_test(test_condition_goal_reached test_goal_reached.cpp nav2_goal_reached_condition_bt_node) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp index 18aaf99f8ec..fb27568c275 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp @@ -39,6 +39,9 @@ class IsBatteryChargingConditionTestFixture : public ::testing::Test config_->blackboard->set( "node", node_); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); factory_->registerNodeType("IsBatteryCharging"); diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp index 0af03b9afaa..a5080593117 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp @@ -40,6 +40,9 @@ class IsBatteryLowConditionTestFixture : public ::testing::Test config_->blackboard->set( "node", node_); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); factory_->registerNodeType("IsBatteryLow"); diff --git a/nav2_behavior_tree/test/plugins/control/CMakeLists.txt b/nav2_behavior_tree/test/plugins/control/CMakeLists.txt index 1a8b0cf781a..a625bdad792 100644 --- a/nav2_behavior_tree/test/plugins/control/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/control/CMakeLists.txt @@ -5,3 +5,8 @@ plugin_add_test(test_control_pipeline_sequence test_pipeline_sequence.cpp nav2_p plugin_add_test(test_control_round_robin_node test_round_robin_node.cpp nav2_round_robin_node_bt_node) plugin_add_test(test_control_nonblocking_sequence test_nonblocking_sequence.cpp nav2_nonblocking_sequence_bt_node) + +plugin_add_test(test_control_pause_resume_controller + test_pause_resume_controller.cpp nav2_pause_resume_controller_bt_node) + +plugin_add_test(test_persistent_sequence test_persistent_sequence.cpp nav2_persistent_sequence_bt_node) diff --git a/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp b/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp new file mode 100644 index 00000000000..a546bcfb565 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp @@ -0,0 +1,253 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "utils/test_behavior_tree_fixture.hpp" +#include "utils/test_dummy_tree_node.hpp" +#include "utils/get_node_from_tree.hpp" +#include "nav2_behavior_tree/plugins/control/pause_resume_controller.hpp" +#include "std_srvs/srv/trigger.hpp" + +class PauseResumeControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + static void SetUpTestCase() + { + nav2_behavior_tree::BehaviorTreeTestFixture::SetUpTestCase(); + + ASSERT_TRUE(node_); + pause_client_ = node_->create_client("pause", true); + resume_client_ = node_->create_client("resume", true); + + ASSERT_TRUE(factory_); + factory_->registerNodeType("PauseResumeController"); + factory_->registerNodeType("DummyNode"); + } + + static void TearDownTestCase() + { + tree_.reset(); + pause_client_.reset(); + resume_client_.reset(); + } + + static void CheckServiceResult( + const nav2::ServiceClient::SharedPtr & client, + std::shared_future> future, + bool success = true) + { + const auto res = client->spin_until_complete(future, std::chrono::seconds(1)); + ASSERT_EQ(res, rclcpp::FutureReturnCode::SUCCESS); + ASSERT_EQ(future.wait_for(std::chrono::seconds(0)), std::future_status::ready); + EXPECT_EQ(future.get()->success, success); + } + +protected: + static std::shared_ptr tree_; + static nav2::ServiceClient::SharedPtr pause_client_; + static nav2::ServiceClient::SharedPtr resume_client_; +}; + +std::shared_ptr PauseResumeControllerTestFixture::tree_ = nullptr; +nav2::ServiceClient::SharedPtr +PauseResumeControllerTestFixture::pause_client_ = nullptr; +nav2::ServiceClient::SharedPtr +PauseResumeControllerTestFixture::resume_client_ = nullptr; + +TEST_F(PauseResumeControllerTestFixture, test_incorrect_num_children) +{ + // create tree with incorrect number of children + std::string xml_txt = + R"( + + + + + + )"; + EXPECT_THROW( + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard), + BT::RuntimeError); +} + +TEST_F(PauseResumeControllerTestFixture, test_unused_children) +{ + // create tree with only RESUMED child + std::string xml_txt = + R"( + + + + + + + )"; + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // get pause_resume_controller so we can check state + auto pause_bt_node = + nav2_behavior_tree::get_node_from_tree(tree_); + ASSERT_NE(pause_bt_node, nullptr); + using state_t = nav2_behavior_tree::state_t; + + // get dummy nodes so we can change their status + auto resumed_child = + nav2_behavior_tree::get_node_from_tree(tree_, 0); + ASSERT_NE(resumed_child, nullptr); + resumed_child->changeStatus(BT::NodeStatus::RUNNING); + + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + + // Call pause service, expect RUNNING and PAUSED + auto req = std::make_shared(); + auto future = pause_client_->async_call(req); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + CheckServiceResult(pause_client_, future); + + // Tick again, expect RUNNING and PAUSED + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + + // Call resume service, expect RUNNING and ON_RESUME + future = resume_client_->async_call(req); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + CheckServiceResult(resume_client_, future); + + // Tick again, expect RUNNING and RESUMED + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); +} + +TEST_F(PauseResumeControllerTestFixture, test_behavior) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + + + + )"; + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // get pause_resume_controller so we can check state + auto pause_bt_node = + nav2_behavior_tree::get_node_from_tree(tree_); + ASSERT_NE(pause_bt_node, nullptr); + using state_t = nav2_behavior_tree::state_t; + + // get dummy nodes so we can change their status + auto resumed_child = + nav2_behavior_tree::get_node_from_tree(tree_, 0); + auto paused_child = + nav2_behavior_tree::get_node_from_tree(tree_, 1); + auto on_pause_child = + nav2_behavior_tree::get_node_from_tree(tree_, 2); + auto on_resume_child = + nav2_behavior_tree::get_node_from_tree(tree_, 3); + ASSERT_NE(resumed_child, nullptr); + ASSERT_NE(paused_child, nullptr); + ASSERT_NE(on_pause_child, nullptr); + ASSERT_NE(on_resume_child, nullptr); + + resumed_child->changeStatus(BT::NodeStatus::RUNNING); + paused_child->changeStatus(BT::NodeStatus::RUNNING); + on_pause_child->changeStatus(BT::NodeStatus::RUNNING); + on_resume_child->changeStatus(BT::NodeStatus::RUNNING); + + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + + // Call pause service, set ON_PAUSE child to RUNNING, expect RUNNING and ON_PAUSE + auto req = std::make_shared(); + auto future = pause_client_->async_call(req); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::ON_PAUSE); + CheckServiceResult(pause_client_, future); + + // Change ON_PAUSE child to SUCCESS, expect RUNNING and PAUSED + on_pause_child->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + + // Set PAUSED child to SUCCESS, expect RUNNING and PAUSED + // (should keep ticking unless RESUMED branch succeeds) + paused_child->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + + // Set PAUSED child to SKIPPED, expect RUNNING and PAUSED + paused_child->changeStatus(BT::NodeStatus::SKIPPED); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + + // Call pause service again, expect RUNNING and PAUSED + future = pause_client_->async_call(req); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED); + CheckServiceResult(pause_client_, future, false); + + // Call resume service, change ON_RESUME child to FAILURE, expect FAILURE + future = resume_client_->async_call(req); + on_resume_child->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::FAILURE); + CheckServiceResult(resume_client_, future); + + // Halt the tree, expect RUNNING and RESUMED + tree_->haltTree(); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + + // Set resumed child to SUCCESS, expect SUCCESS and RESUMED + resumed_child->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + + // Call resume service again, expect RUNNING and RESUMED + future = resume_client_->async_call(req); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED); + CheckServiceResult(resume_client_, future, false); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/control/test_persistent_sequence.cpp b/nav2_behavior_tree/test/plugins/control/test_persistent_sequence.cpp new file mode 100644 index 00000000000..ebf6aa0d6b8 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/control/test_persistent_sequence.cpp @@ -0,0 +1,157 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "utils/test_behavior_tree_fixture.hpp" +#include "utils/test_dummy_tree_node.hpp" +#include "utils/get_node_from_tree.hpp" +#include "nav2_behavior_tree/plugins/control/persistent_sequence.hpp" + +class PersistentSequenceTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + static void SetUpTestCase() + { + factory_ = std::make_shared(); + config_ = new BT::NodeConfiguration(); + config_->blackboard = BT::Blackboard::create(); + config_->blackboard->set("seq_child_idx", 0); + + factory_->registerNodeType( + "PersistentSequence"); + + // Register dummy node for testing + factory_->registerNodeType("DummyNode"); + } + + static void TearDownTestCase() + { + if (config_) { + delete config_; + config_ = nullptr; + } + tree_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +BT::NodeConfiguration * PersistentSequenceTestFixture::config_ = nullptr; +std::shared_ptr +PersistentSequenceTestFixture::factory_ = nullptr; +std::shared_ptr PersistentSequenceTestFixture::tree_ = nullptr; + +TEST_F(PersistentSequenceTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + )"; + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + config_->blackboard->set("seq_child_idx", 0); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); +} + +TEST_F(PersistentSequenceTestFixture, test_behavior) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + + + )"; + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // get dummy nodes so we can change their status + auto child0 = + nav2_behavior_tree::get_node_from_tree(tree_, 0); + auto child1 = + nav2_behavior_tree::get_node_from_tree(tree_, 1); + auto child2 = + nav2_behavior_tree::get_node_from_tree(tree_, 2); + ASSERT_NE(child0, nullptr); + ASSERT_NE(child1, nullptr); + ASSERT_NE(child2, nullptr); + child0->changeStatus(BT::NodeStatus::RUNNING); + child1->changeStatus(BT::NodeStatus::RUNNING); + child2->changeStatus(BT::NodeStatus::RUNNING); + + // Set child index to 0 and child0 to RUNNING, expect RUNNING and idx = 0 + config_->blackboard->set("seq_child_idx", 0); + child0->changeStatus(BT::NodeStatus::RUNNING); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(config_->blackboard->get("seq_child_idx"), 0); + + // Set child0 to SUCCESS, expect SUCCESS and idx = 1 + child0->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(config_->blackboard->get("seq_child_idx"), 1); + + // Set child1 to FAILURE, expect FAILURE and idx = 0 + child1->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_EQ(config_->blackboard->get("seq_child_idx"), 0); + + // Set idx to 1, child0 to FAILURE, child1 to SKIPPED and child2 to RUNNING, + // expect RUNNING and idx = 2 + config_->blackboard->set("seq_child_idx", 1); + child0->changeStatus(BT::NodeStatus::FAILURE); + child1->changeStatus(BT::NodeStatus::SKIPPED); + child2->changeStatus(BT::NodeStatus::RUNNING); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(config_->blackboard->get("seq_child_idx"), 2); + + // Set child2 to SUCCESS, expect SUCCESS and idx = 0 + child2->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(config_->blackboard->get("seq_child_idx"), 0); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index 1766dfbd418..33cfeffc5e7 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -43,6 +43,9 @@ class GoalUpdaterTestFixture : public ::testing::Test config_->blackboard->set( "node", node_); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) diff --git a/nav2_behavior_tree/test/utils/get_node_from_tree.hpp b/nav2_behavior_tree/test/utils/get_node_from_tree.hpp new file mode 100644 index 00000000000..5ea720872d4 --- /dev/null +++ b/nav2_behavior_tree/test/utils/get_node_from_tree.hpp @@ -0,0 +1,66 @@ +// Copyright (c) 2025 Enjoy Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILS__GET_NODE_FROM_TREE_HPP_ +#define UTILS__GET_NODE_FROM_TREE_HPP_ + +#include +#include + +#include "behaviortree_cpp/bt_factory.h" +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief Get node from tree by type and index, casted to NodeT type, const casted away + * Returns node if it was found, nullptr otherwise. + */ +template +NodeT * get_node_from_tree( + const std::shared_ptr tree, + const size_t index = 0) +{ + static rclcpp::Logger logger = rclcpp::get_logger("nav2_behavior_tree::get_node_from_tree"); + + std::vector nodes = tree->getNodesByPath("*"); + if (nodes.empty()) { + RCLCPP_ERROR(logger, "No nodes of given type found"); + return nullptr; + } + if (nodes.size() <= index) { + RCLCPP_ERROR(logger, "Out of bounds (found %zu < %zu nodes)", nodes.size(), index); + return nullptr; + } + + const NodeT * const_bt_node = + dynamic_cast(nodes[index]); + if (const_bt_node == nullptr) { + RCLCPP_ERROR(logger, "Failed to cast node to given type"); + return nullptr; + } + + NodeT * bt_node = const_cast(const_bt_node); + if (bt_node == nullptr) { + RCLCPP_ERROR(logger, "Failed to cast away const from node"); + return nullptr; + } + + return bt_node; +} + +} // namespace nav2_behavior_tree + +#endif // UTILS__GET_NODE_FROM_TREE_HPP_ diff --git a/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp b/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp index dcfa52927ac..b4a64d7c78f 100644 --- a/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp +++ b/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp @@ -18,6 +18,7 @@ #include #include +#include namespace nav2_behavior_tree { @@ -29,17 +30,20 @@ namespace nav2_behavior_tree class DummyNode : public BT::ActionNodeBase { public: - DummyNode() + DummyNode( + const std::string & /*xml_tag_name*/ = "dummy", + const BT::NodeConfiguration & /*conf*/ = BT::NodeConfiguration()) : BT::ActionNodeBase("dummy", {}) { } void changeStatus(BT::NodeStatus status) { - if (status == BT::NodeStatus::IDLE) { + requested_status = status; + if (requested_status == BT::NodeStatus::IDLE) { resetStatus(); } else { - setStatus(status); + setStatus(requested_status); } } @@ -50,12 +54,26 @@ class DummyNode : public BT::ActionNodeBase BT::NodeStatus tick() override { + if (requested_status == BT::NodeStatus::IDLE) { + resetStatus(); + } else { + setStatus(requested_status); + } return status(); } void halt() override { + resetStatus(); } + + static BT::PortsList providedPorts() + { + return {}; + } + +protected: + BT::NodeStatus requested_status = BT::NodeStatus::IDLE; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/test/utils/test_transform_handler.hpp b/nav2_behavior_tree/test/utils/test_transform_handler.hpp index 19cfcdfa222..cf272d132cd 100644 --- a/nav2_behavior_tree/test/utils/test_transform_handler.hpp +++ b/nav2_behavior_tree/test/utils/test_transform_handler.hpp @@ -29,9 +29,9 @@ #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_msgs/msg/tf_message.hpp" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/buffer.hpp" using namespace std::chrono_literals; // NOLINT using namespace std::chrono; // NOLINT diff --git a/nav2_behaviors/CMakeLists.txt b/nav2_behaviors/CMakeLists.txt index c67f9af628b..292191ea013 100644 --- a/nav2_behaviors/CMakeLists.txt +++ b/nav2_behaviors/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_behaviors) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) diff --git a/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp b/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp index 7301cef3836..82b55c0786f 100644 --- a/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp +++ b/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp @@ -19,8 +19,8 @@ #include #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" #include "nav2_core/behavior.hpp" diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 70d0f30fa30..22db8701700 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -26,8 +26,8 @@ #include #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" #include "geometry_msgs/msg/twist.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/twist_publisher.hpp" diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index c3739a3461e..04f20f80782 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -11,6 +11,7 @@ ament_cmake nav2_common + backward_ros geometry_msgs nav2_core nav2_costmap_2d diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index 54ace7bac0a..0f4db4b3bf6 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -87,7 +87,7 @@ BehaviorServer::on_configure(const rclcpp_lifecycle::State & state) get_node_base_interface(), get_node_timers_interface()); tf_->setCreateTimerInterface(timer_interface); - transform_listener_ = std::make_shared(*tf_); + transform_listener_ = std::make_shared(*tf_, this, true); behavior_types_.resize(behavior_ids_.size()); if (!loadBehaviorPlugins()) { diff --git a/nav2_bringup/CMakeLists.txt b/nav2_bringup/CMakeLists.txt index 5537046d246..af08ad0265b 100644 --- a/nav2_bringup/CMakeLists.txt +++ b/nav2_bringup/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_bringup) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(nav2_common REQUIRED) find_package(navigation2 REQUIRED) diff --git a/nav2_bringup/launch/cbf_supervisor_launch.py b/nav2_bringup/launch/cbf_supervisor_launch.py new file mode 100644 index 00000000000..74dc2f951f9 --- /dev/null +++ b/nav2_bringup/launch/cbf_supervisor_launch.py @@ -0,0 +1,27 @@ +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, ExecuteProcess +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution, EnvironmentVariable +import os + +def generate_launch_description(): + launchDesc = LaunchDescription() + # If the Supervisor package is available on system (debian install) + # then include the supervisor launch file + pkg = get_package_share_directory("lll_supervisor") + launchDesc.add_action( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("lll_supervisor"), + "launch", + "supervisor.launch.py", + ] + ) + ) + ) + ) + + return launchDesc diff --git a/nav2_bringup/launch/tb4_simulation_launch.py b/nav2_bringup/launch/tb4_simulation_launch.py index 2dcde8ec82f..e369d8bad7b 100644 --- a/nav2_bringup/launch/tb4_simulation_launch.py +++ b/nav2_bringup/launch/tb4_simulation_launch.py @@ -31,7 +31,7 @@ # Define local map types MAP_POSES_DICT = { 'depot': { - 'x': -8.00, 'y': 0.00, 'z': 0.01, + 'x': -7.54, 'y': -0.3, 'z': 0.01, 'R': 0.00, 'P': 0.00, 'Y': 0.00 }, 'warehouse': { diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index dc5ebc7bb19..d17f705cd1e 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -12,9 +12,11 @@ ament_cmake nav2_common + backward_ros navigation2 launch_ros + backward_ros diff_drive_controller joint_state_broadcaster launch_ros diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 23e509760cc..a1c33435c18 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -6,6 +6,7 @@ amcl: alpha4: 0.2 alpha5: 0.2 base_frame_id: "base_footprint" + introspection_mode: "disabled" beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 beam_skip_threshold: 0.3 @@ -30,7 +31,6 @@ amcl: sigma_hit: 0.2 tf_broadcast: true transform_tolerance: 1.0 - introspection_mode: "disabled" update_min_a: 0.2 update_min_d: 0.25 z_hit: 0.5 @@ -38,6 +38,15 @@ amcl: z_rand: 0.5 z_short: 0.05 scan_topic: scan + set_initial_pose: true + always_reset_initial_pose: true + first_map_only: false + initial_pose: + x: 7.65 + y: 7.65 + z: 0.0 + yaw: 0.0 + bt_navigator: ros__parameters: @@ -45,8 +54,10 @@ bt_navigator: robot_base_frame: base_link odom_topic: odom bt_loop_duration: 10 + #enable_stamped_cmd_vel: False filter_duration: 0.3 default_server_timeout: 20 + goal_reached_tol: 0.8 wait_for_service_timeout: 1000 introspection_mode: "disabled" navigators: ["navigate_to_pose", "navigate_through_poses"] @@ -58,14 +69,26 @@ bt_navigator: plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" enable_groot_monitoring: false groot_server_port: 1669 + bt_search_directories: + - $(find-pkg-share nav2_bt_navigator)/behavior_trees # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - + + #default_nav_through_poses_bt_xml: $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy.xml + default_nav_through_poses_bt_xml: $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy_test_replanning.xml + #default_nav_through_poses_bt_xml: $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy.xml # Pelna obsluga przeszkod + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). # Built-in plugins are added automatically - # plugin_lib_names: [] + plugin_lib_names: + - nav2_publish_goals_action_bt_node + - nav2_is_collision_free_condition_bt_node + - nav2_remove_next_goal_action_bt_node + - nav2_pose_reached_condition_bt_node + - nav2_goals_truncation_action_bt_node + - nav2_goals_has_at_most_point_count_bt_node error_code_name_prefixes: - assisted_teleop @@ -84,7 +107,8 @@ bt_navigator: controller_server: ros__parameters: - controller_frequency: 20.0 + controller_frequency: 30.0 + #enable_stamped_cmd_vel: False costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 @@ -98,8 +122,9 @@ controller_server: # Progress checker parameters progress_checker: - plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.5 + plugin: "nav2_controller::PoseProgressChecker" + required_movement_radius: 0.1 + required_movement_angle: 0.3 movement_time_allowance: 10.0 # Goal checker parameters #precise_goal_checker: @@ -110,11 +135,237 @@ controller_server: general_goal_checker: stateful: True plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 - FollowPath: + xy_goal_tolerance: 0.25 #0.0 #0.25 #0.0 + yaw_goal_tolerance: 0.25 #0.0 #0.25 #0.0 + + # change general goal checker for path following + # FollowPath: + # plugin: "nav2_mppi_controller::MPPIController" + # time_steps: 56 + # model_dt: 0.05 + # batch_size: 2000 + # ax_max: 3.0 + # ax_min: -3.0 + # ay_max: 3.0 + # ay_min: -3.0 + # az_max: 3.5 + # vx_std: 0.2 + # vy_std: 0.2 + # wz_std: 0.4 #0.9 + # vx_max: 0.3 + # vx_min: -0.1 + # vy_max: 0.5 + # wz_max: 0.3 + # iteration_count: 1 + # prune_distance: 1.7 + # enforce_path_inversion: True + # transform_tolerance: 0.1 + # temperature: 0.3 + # gamma: 0.015 + # motion_model: "DiffDrive" + # #publish_critics_stats: True + # #visualize: true + # #publish_optimal_trajectory: true + # regenerate_noises: true + # Visualization: + # publish_critics_stats: false + # publish_optimal_trajectory_msg: false + # publish_optimal_trajectory: false + # publish_transformed_path: false + # publish_trajectories_with_total_cost: true + # publish_trajectories_with_individual_cost: true + # publish_optimal_footprints: true + # publish_optimal_path: false + # trajectory_step: 5 + # time_step: 3 + # footprint_downsample_factor: 3 + # TrajectoryValidator: + # # The validator can be configured with additional parameters if needed. + # plugin: "mppi::DefaultOptimalTrajectoryValidator" + # collision_lookahead_time: 0.0 # Time to look ahead to check for collisions + # consider_footprint: false # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks + # AckermannConstraints: + # min_turning_r: 0.2 + # critics: [ #"ConstraintCritic", + # "CostCritic", "GoalCritic", + # "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + # "PathAngleCritic", "PreferForwardCritic"] + # GoalCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 5.0 + # threshold_to_consider: 1.4 + # GoalAngleCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 3.0 + # threshold_to_consider: 0.5 + # PreferForwardCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 5.0 + # threshold_to_consider: 0.5 + # CostCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 3.81 + # near_collision_cost: 253 + # critical_cost: 300.0 + # consider_footprint: true + # collision_cost: 1000000.0 + # near_goal_distance: 0.2 + # trajectory_point_step: 2 + # PathAlignCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 14.0 + # max_path_occupancy_ratio: 0.05 + # trajectory_point_step: 4 + # threshold_to_consider: 0.5 + # offset_from_furthest: 20 + # use_path_orientations: false + # PathFollowCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 5.0 + # offset_from_furthest: 5 + # threshold_to_consider: 1.4 + # PathAngleCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 2.0 + # offset_from_furthest: 4 + # threshold_to_consider: 0.5 + # max_angle_to_furthest: 1.0 + # mode: 0 + # WaypointCritic: + # enabled: false + # cost_power: 1 + # cost_weight: 5.0 + # position_tolerance: 0.1 + # yaw_tolerance: 0.1 + # waypoint_topic: "truncated_path" + # use_orientation: false + # max_waypoint_distance: 10.0 + MPPI: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 50 + model_dt: 0.1 + batch_size: 2000 + ax_max: 1.0 + ax_min: -1.0 + ay_max: 0.0 + ay_min: 0.0 + az_max: 1.0 + vx_std: 0.2 + vy_std: 0.0 + wz_std: 0.9 + vx_max: 0.3 + vx_min: -0.1 + vy_max: 0.0 + wz_max: 0.3 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + enforce_path_inversion: True + enforce_path_rotation: True + minimum_rotation_angle: 1.0 + inversion_xy_tolerance: 0.5 + inversion_yaw_tolerance: 0.785 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + reset_period: 1.0 # (only in Humble) + regenerate_noises: true + Visualization: + publish_critics_stats: true + publish_optimal_trajectory_msg: false + publish_optimal_trajectory: false + publish_transformed_path: true + publish_trajectories_with_total_cost: false + publish_trajectories_with_individual_cost: false + publish_optimal_footprints: false + publish_optimal_path: false + trajectory_step: 10 + time_step: 5 + footprint_downsample_factor: 3 + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + # ObstaclesCritic: + # enabled: true + # cost_power: 1 + # repulsion_weight: 1.5 + # critical_weight: 20.0 + # consider_footprint: false + # collision_cost: 10000.0 + # collision_margin_distance: 0.1 + # near_goal_distance: 0.5 + # inflation_radius: 0.55 # (only in Humble) + # cost_scaling_factor: 10.0 # (only in Humble) + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 2.5 # PODNIESC WAGE COST CRITISC + near_collision_cost: 253 + critical_cost: 250.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 + + FollowPathA: + # plugin: "nav2_rotation_shim_controller::RotationShimController" + # primary_controller: "nav2_mppi_controller::MPPIController" + # angular_dist_threshold: 2.4434 # 140 degrees + # forward_sampling_distance: 0.5 + # angular_disengage_threshold: 0.3925 + # rotate_to_heading_angular_vel: 0.3 + # max_angular_accel: 1.0 + # simulate_ahead_time: 1.0 + # rotate_to_goal_heading: false plugin: "nav2_mppi_controller::MPPIController" - time_steps: 56 + time_steps: 35 model_dt: 0.05 batch_size: 2000 ax_max: 3.0 @@ -124,55 +375,389 @@ controller_server: az_max: 3.5 vx_std: 0.2 vy_std: 0.2 - wz_std: 0.4 - vx_max: 0.5 - vx_min: -0.35 + wz_std: 0.9 + vx_max: 0.3 + vx_min: -0.1 vy_max: 0.5 - wz_max: 1.9 + wz_max: 0.3 iteration_count: 1 - prune_distance: 1.7 + prune_distance: 0.8 + enforce_path_inversion: True transform_tolerance: 0.1 temperature: 0.3 gamma: 0.015 motion_model: "DiffDrive" - visualize: true - publish_optimal_trajectory: true - regenerate_noises: true - TrajectoryVisualizer: - trajectory_step: 5 - time_step: 3 + publish_critics_stats: True + #visualize: false + #publish_optimal_trajectory: true + regenerate_noises: false + Visualization: + publish_critics_stats: false + publish_optimal_trajectory_msg: false + publish_optimal_trajectory: false + publish_transformed_path: false + publish_trajectories_with_total_cost: true + publish_trajectories_with_individual_cost: false + publish_optimal_footprints: true + publish_optimal_path: false + trajectory_step: 10 + time_step: 5 + footprint_downsample_factor: 3 TrajectoryValidator: # The validator can be configured with additional parameters if needed. + # plugin: "mppi::AutoFixingTrajectoryValidator" + # #consider_footprint: true + # consider_footprint: true + # collision_lookahead_time: 0.8 + # auto_fix_ttc_window: 0.8 + # auto_fix_safe_clearance: 0.04 # ≈ 0.8 * resolution (np. 5cm → 0.04) + # auto_fix_step_gain: 0.6 + # auto_fix_max_step: 0.05 + # auto_fix_max_total_displacement: 0.35 + # auto_fix_max_iters: 6 + # auto_fix_smooth_iters: 2 plugin: "mppi::DefaultOptimalTrajectoryValidator" - collision_lookahead_time: 2.0 # Time to look ahead to check for collisions - consider_footprint: false # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks + collision_lookahead_time: 0.0 # Time to look ahead to check for collisions + consider_footprint: true # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks + + #collision_lookahead_time: 0.1 # Time to look ahead to check for collisions + #consider_footprint: true # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks AckermannConstraints: min_turning_r: 0.2 - critics: - [ - "ConstraintCritic", - "CostCritic", - "GoalCritic", - "GoalAngleCritic", - "PathAlignCritic", - "PathFollowCritic", - "PathAngleCritic", - "PreferForwardCritic", - ] + critics: [ #"ConstraintCritic", + "ObstaclesCritic", "GoalCritic", + "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + "PathAngleCritic", "PreferForwardCritic"] #,"WaypointCritic"] ConstraintCritic: + enabled: false + cost_power: 1 + cost_weight: 4.0 + GoalCritic: enabled: true cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 #0.5 # zmienione z 0.2 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.3 #0.3 # zmienione z 0.1 + PreferForwardCritic: + enabled: false + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 2.5 # PODNIESC WAGE COST CRITISC + near_collision_cost: 253 + critical_cost: 250.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 0.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: false + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + offset_from_furthest: 3 + threshold_to_consider: 0.0 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + offset_from_furthest: 4 + threshold_to_consider: 0.0 + max_angle_to_furthest: 0.5 #1.2 + mode: 0 + ObstaclesCritic: + enabled: true + cost_power: 1 + repulsion_weight: 0.5 #0.5 0.8 + critical_weight: 15.0 + consider_footprint: true # FALSE # avoid obstacles based on radius not the footprint + collision_cost: 10000.0 + collision_margin_distance: 0.1 + near_goal_distance: 0.2 + cost_scaling_factor: 2.5 + inflation_radius: 1.0 + WaypointCritic: + enabled: false + cost_power: 1 + cost_weight: 5.0 + position_tolerance: 0.1 + yaw_tolerance: 0.1 + waypoint_topic: "truncated_path" + use_orientation: True + max_waypoint_distance: 10.0 + TwirlingCritic: + enabled: false + twirling_cost_power: 1 + twirling_cost_weight: 10.0 + + FollowPathB: + # plugin: "nav2_rotation_shim_controller::RotationShimController" + # primary_controller: "nav2_mppi_controller::MPPIController" + # angular_dist_threshold: 2.4434 # 140 degrees + # forward_sampling_distance: 0.5 + # angular_disengage_threshold: 0.3925 + # rotate_to_heading_angular_vel: 0.3 + # max_angular_accel: 1.0 + # simulate_ahead_time: 1.0 + # rotate_to_goal_heading: false + plugin: "nav2_mppi_controller::MPPIController" + # Dobrac prune distance, time_stemps i model_dt PRZYSPIESZENIA NIE DZIALA DLA NIZSZEGO MODEL DT + time_steps: 35 + model_dt: 0.15 #0.05 + batch_size: 2000 + ax_max: 1.0 + ax_min: -1.0 + ay_max: 1.0 + ay_min: -1.0 + az_max: 1.0 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.9 + vx_max: 0.3 + vx_min: -0.1 + vy_max: 0.5 + wz_max: 0.3 + iteration_count: 1 + prune_distance: 1.0 + enforce_path_inversion: True + enforce_path_rotation: True + minimum_rotation_angle: 1.0 + inversion_xy_tolerance: 0.5 + inversion_yaw_tolerance: 0.785 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + publish_critics_stats: false + #visualize: false + #publish_optimal_trajectory: true + regenerate_noises: true + Visualization: + publish_critics_stats: true + publish_optimal_trajectory_msg: false + publish_optimal_trajectory: false + publish_transformed_path: true + publish_trajectories_with_total_cost: false + publish_trajectories_with_individual_cost: false + publish_optimal_footprints: false + publish_optimal_path: false + trajectory_step: 10 + time_step: 5 + footprint_downsample_factor: 3 + TrajectoryValidator: + # The validator can be configured with additional parameters if needed. + # plugin: "mppi::AutoFixingTrajectoryValidator" + # #consider_footprint: true + # consider_footprint: true + # collision_lookahead_time: 0.8 + # auto_fix_ttc_window: 0.8 + # auto_fix_safe_clearance: 0.04 # ≈ 0.8 * resolution (np. 5cm → 0.04) + # auto_fix_step_gain: 0.6 + # auto_fix_max_step: 0.05 + # auto_fix_max_total_displacement: 0.35 + # auto_fix_max_iters: 6 + # auto_fix_smooth_iters: 2 + plugin: "mppi::DefaultOptimalTrajectoryValidator" + collision_lookahead_time: 0.0 # Time to look ahead to check for collisions + consider_footprint: true # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks + + #collision_lookahead_time: 0.1 # Time to look ahead to check for collisions + #consider_footprint: true # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks + AckermannConstraints: + min_turning_r: 0.2 + critics: [ #"ConstraintCritic", + "CostCritic", "GoalCritic", + "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + "PathAngleCritic", "PreferForwardCritic"] #,"WaypointCritic"] + ConstraintCritic: + enabled: false + cost_power: 1 cost_weight: 4.0 GoalCritic: enabled: true cost_power: 1 cost_weight: 5.0 - threshold_to_consider: 1.4 + threshold_to_consider: 0.5 #0.5 # zmienione z 0.2 GoalAngleCritic: enabled: true cost_power: 1 cost_weight: 3.0 + threshold_to_consider: 0.3 #0.3 # zmienione z 0.1 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 2.5 # PODNIESC WAGE COST CRITISC + near_collision_cost: 253 + critical_cost: 250.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 0.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.3 + offset_from_furthest: 10 + use_path_orientations: false + visualize_furthest_point: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + offset_from_furthest: 3 + threshold_to_consider: 0.5 + visualize_furthest_point: false + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + offset_from_furthest: 4 + threshold_to_consider: 0.3 + max_angle_to_furthest: 0.5 #1.2 + visualize_furthest_point: false + forward_preference: true + mode: 0 + ObstaclesCritic: + enabled: true + cost_power: 1 + repulsion_weight: 0.5 #0.5 0.8 + critical_weight: 15.0 + consider_footprint: true # FALSE # avoid obstacles based on radius not the footprint + collision_cost: 10000.0 + collision_margin_distance: 0.1 + near_goal_distance: 0.2 + cost_scaling_factor: 2.5 + inflation_radius: 1.0 + WaypointCritic: + enabled: false + cost_power: 1 + cost_weight: 5.0 + position_tolerance: 0.1 + yaw_tolerance: 0.1 + waypoint_topic: "truncated_path" + use_orientation: True + max_waypoint_distance: 10.0 + TwirlingCritic: + enabled: false + twirling_cost_power: 1 + twirling_cost_weight: 10.0 + + FollowPathC: + # plugin: "nav2_rotation_shim_controller::RotationShimController" + # primary_controller: "nav2_mppi_controller::MPPIController" + # angular_dist_threshold: 2.4434 # 140 degrees + # forward_sampling_distance: 0.5 + # angular_disengage_threshold: 0.3925 + # rotate_to_heading_angular_vel: 0.3 + # max_angular_accel: 1.0 + # simulate_ahead_time: 1.0 + # rotate_to_goal_heading: false + plugin: "nav2_mppi_controller::MPPIController" + # Dobrac prune distance, time_stemps i model_dt PRZYSPIESZENIA NIE DZIALA DLA NIZSZEGO MODEL DT + time_steps: 35 #35 # 18 + model_dt: 0.15 #0.15 #0.05 + batch_size: 2000 + ax_max: 1.0 + ax_min: -1.0 + ay_max: 1.0 + ay_min: -1.0 + az_max: 1.0 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.3 + vx_min: -0.1 + vy_max: 0.5 + wz_max: 0.3 + iteration_count: 1 + prune_distance: 1.7 #1.7 #1.0 + enforce_path_inversion: True + enforce_path_rotation: True + minimum_rotation_angle: 1.0 + inversion_xy_tolerance: 0.5 + inversion_yaw_tolerance: 0.785 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + publish_critics_stats: false + #visualize: false + #publish_optimal_trajectory: true + regenerate_noises: true + Visualization: + publish_critics_stats: true + publish_optimal_trajectory_msg: false + publish_optimal_trajectory: false + publish_transformed_path: true + publish_trajectories_with_total_cost: false + publish_trajectories_with_individual_cost: false + publish_optimal_footprints: false + publish_optimal_path: false + trajectory_step: 10 + time_step: 5 + footprint_downsample_factor: 3 + TrajectoryValidator: + # The validator can be configured with additional parameters if needed. + # plugin: "mppi::AutoFixingTrajectoryValidator" + # #consider_footprint: true + # consider_footprint: true + # collision_lookahead_time: 0.8 + # auto_fix_ttc_window: 0.8 + # auto_fix_safe_clearance: 0.04 # ≈ 0.8 * resolution (np. 5cm → 0.04) + # auto_fix_step_gain: 0.6 + # auto_fix_max_step: 0.05 + # auto_fix_max_total_displacement: 0.35 + # auto_fix_max_iters: 6 + # auto_fix_smooth_iters: 2 + plugin: "mppi::DefaultOptimalTrajectoryValidator" + collision_lookahead_time: 0.0 # Time to look ahead to check for collisions + consider_footprint: true # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks + + #collision_lookahead_time: 0.1 # Time to look ahead to check for collisions + #consider_footprint: true # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks + AckermannConstraints: + min_turning_r: 0.2 + critics: [ #"ConstraintCritic", + "CostCritic", "GoalCritic", + "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + "PathAngleCritic", "PreferForwardCritic"] #,"WaypointCritic"] + ConstraintCritic: + enabled: false + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 #0.5 # zmienione z 0.2 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.3 #0.3 # zmienione z 0.1 PreferForwardCritic: enabled: true cost_power: 1 @@ -181,10 +766,10 @@ controller_server: CostCritic: enabled: true cost_power: 1 - cost_weight: 3.81 + cost_weight: 2.5 # PODNIESC WAGE COST CRITISC near_collision_cost: 253 - critical_cost: 300.0 - consider_footprint: false + critical_cost: 250.0 + consider_footprint: true collision_cost: 1000000.0 near_goal_distance: 1.0 trajectory_point_step: 2 @@ -194,27 +779,51 @@ controller_server: cost_weight: 14.0 max_path_occupancy_ratio: 0.05 trajectory_point_step: 4 - threshold_to_consider: 0.5 - offset_from_furthest: 20 + threshold_to_consider: 0.0 + offset_from_furthest: 10 use_path_orientations: false + visualize_furthest_point: false PathFollowCritic: enabled: true cost_power: 1 - cost_weight: 5.0 - offset_from_furthest: 5 - threshold_to_consider: 1.4 + cost_weight: 4.0 + offset_from_furthest: 3 + threshold_to_consider: 0.0 + visualize_furthest_point: false PathAngleCritic: enabled: true cost_power: 1 - cost_weight: 2.0 + cost_weight: 3.0 offset_from_furthest: 4 - threshold_to_consider: 0.5 - max_angle_to_furthest: 1.0 + threshold_to_consider: 0.0 + max_angle_to_furthest: 0.5 #1.2 + visualize_furthest_point: false + forward_preference: true mode: 0 - # TwirlingCritic: - # enabled: true - # twirling_cost_power: 1 - # twirling_cost_weight: 10.0 + ObstaclesCritic: + enabled: true + cost_power: 1 + repulsion_weight: 0.5 #0.5 0.8 + critical_weight: 15.0 + consider_footprint: true # FALSE # avoid obstacles based on radius not the footprint + collision_cost: 10000.0 + collision_margin_distance: 0.1 + near_goal_distance: 0.2 + cost_scaling_factor: 2.5 + inflation_radius: 1.0 + WaypointCritic: + enabled: false + cost_power: 1 + cost_weight: 5.0 + position_tolerance: 0.1 + yaw_tolerance: 0.1 + waypoint_topic: "truncated_path" + use_orientation: True + max_waypoint_distance: 10.0 + TwirlingCritic: + enabled: false + twirling_cost_power: 1 + twirling_cost_weight: 10.0 local_costmap: local_costmap: @@ -227,9 +836,10 @@ local_costmap: width: 3 height: 3 resolution: 0.05 - robot_radius: 0.22 - plugins: ["voxel_layer", "inflation_layer"] - filters: ["keepout_filter"] + #robot_radius: 0.22 + footprint: "[ [0.28638, 0.274], [0.28638, -0.274], [-0.39762, -0.274], [-0.39762, 0.274] ]" + plugins: ["obstacle_layer", "inflation_layer"] + #filters: ["keepout_filter"] keepout_filter: plugin: "nav2_costmap_2d::KeepoutFilter" enabled: KEEPOUT_ZONE_ENABLED @@ -238,8 +848,31 @@ local_costmap: lethal_override_cost: 200 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.70 + cost_scaling_factor: 2.5 + inflation_radius: 1.0 + + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + footprint_clearing_enabled: True + observation_sources: scan + scan: + # A relative topic will be appended to the parent of the global_costmap namespace. + # For example: + # * User chosen namespace is `tb4`. + # * User chosen topic is `scan`. + # * Topic will be remapped to `/tb4/scan` without `global_costmap`. + # * Use global topic `/scan` if you do not wish the node namespace to apply + topic: scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -269,6 +902,8 @@ local_costmap: static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True + footprint_clearing_enabled: True + restore_cleared_footprint: True always_send_full_costmap: True introspection_mode: "disabled" @@ -279,11 +914,12 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - robot_radius: 0.22 + #robot_radius: 0.22 + footprint: "[ [0.28638, 0.274], [0.28638, -0.274], [-0.39762, -0.274], [-0.39762, 0.274] ]" resolution: 0.05 track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - filters: ["keepout_filter", "speed_filter"] + #filters: ["keepout_filter", "speed_filter"] keepout_filter: plugin: "nav2_costmap_2d::KeepoutFilter" enabled: KEEPOUT_ZONE_ENABLED @@ -298,6 +934,7 @@ global_costmap: obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True + footprint_clearing_enabled: True observation_sources: scan scan: # A relative topic will be appended to the parent of the global_costmap namespace. @@ -321,7 +958,7 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.7 + inflation_radius: 0.55 always_send_full_costmap: True introspection_mode: "disabled" @@ -367,18 +1004,81 @@ map_saver: map_subscribe_transient_local: True introspection_mode: "disabled" +# planner_server: +# ros__parameters: +# expected_planner_frequency: 20.0 +# planner_plugins: ["GridBased"] +# costmap_update_timeout: 1.0 +# introspection_mode: "disabled" +# GridBased: +# plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::" +# tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters +# downsample_costmap: false # whether or not to downsample the map +# downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) +# allow_unknown: true # allow traveling in unknown space +# max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable +# max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance +# max_planning_time: 2.0 # max time in s for planner to plan, smooth +# cost_travel_multiplier: 1.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. +# use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) +# smoother: +# max_iterations: 1000 +# w_smooth: 0.6 +# w_data: 0.2 +# tolerance: 1.0e-10 + planner_server: ros__parameters: - expected_planner_frequency: 20.0 planner_plugins: ["GridBased"] - costmap_update_timeout: 1.0 - introspection_mode: "disabled" + GridBased: - plugin: "nav2_navfn_planner::NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: true + plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::" + tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance + max_planning_time: 2.0 # max time in s for planner to plan, smooth + cost_travel_multiplier: 1.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. + use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) + smoother: + max_iterations: 1000 + w_smooth: 0.6 + w_data: 0.2 + tolerance: 1.0e-10 + # GridBased: + # plugin: "nav2_smac_planner::SmacPlannerLattice" # In Iron and older versions, "/" was used instead of "::" + # allow_unknown: true # Allow traveling in unknown space + # tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + # max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + # max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + # max_planning_time: 5.0 # Max time in s for planner to plan, smooth + # analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + # analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + # analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal + # analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) + # reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 + # change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + # non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 + # cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + # rotation_penalty: 0.0 # Penalty to apply to in-place rotations, if minimum control set contains them + # retrospective_penalty: 0.015 + # lattice_filepath: "/home/jakub/ros2_jazzy/src/aros_navigation/aros_navigation/config/lattice_planner.json" # The filepath to the state lattice graph + # lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + # cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + # allow_reverse_expansion: false # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). + # coarse_search_resolution: 1 # Number of bins to skip when doing a coarse search for the path. Only used for all_direction goal heading mode. + # goal_heading_mode: "DEFAULT" # DEFAULT, BIDIRECTIONAL, ALL_DIRECTION + # smooth_path: True # If true, does a simple and quick smoothing post-processing to the path + # smoother: + # max_iterations: 1000 + # w_smooth: 0.3 + # w_data: 0.2 + # tolerance: 1.0e-10 + # do_refinement: true + # refinement_num: 2 smoother_server: ros__parameters: smoother_plugins: ["simple_smoother", "route_smoother"] @@ -405,6 +1105,7 @@ behavior_server: global_costmap_topic: global_costmap/costmap_raw local_footprint_topic: local_costmap/published_footprint global_footprint_topic: global_costmap/published_footprint + #enable_stamped_cmd_vel: False cycle_frequency: 10.0 behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] @@ -480,17 +1181,19 @@ velocity_smoother: odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 + #enable_stamped_cmd_vel: False collision_monitor: ros__parameters: base_frame_id: "base_footprint" odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_in_topic: cmd_vel_smoothed #cmd_vel_smoothed #"cmd_vel_supervised" cmd_vel_out_topic: "cmd_vel" state_topic: "collision_monitor_state" transform_tolerance: 0.2 source_timeout: 1.0 base_shift_correction: True + #enable_stamped_cmd_vel: False stop_pub_timeout: 2.0 # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, # and robot footprint for "approach" action type. @@ -503,14 +1206,14 @@ collision_monitor: simulation_time_step: 0.1 min_points: 6 visualize: False - enabled: True + enabled: False observation_sources: ["scan"] scan: type: "scan" topic: "scan" min_height: 0.15 max_height: 2.0 - enabled: True + enabled: False docking_server: ros__parameters: @@ -530,12 +1233,13 @@ docking_server: dock_plugins: ["simple_charging_dock"] simple_charging_dock: plugin: "opennav_docking::SimpleChargingDock" - docking_threshold: 0.05 + docking_threshold: 0.01 staging_x_offset: -0.7 - use_external_detection_pose: true + use_external_detection_pose: false use_battery_status: false # true use_stall_detection: false # true dock_direction: "forward" + rotate_to_dock: False external_detection_timeout: 1.0 external_detection_translation_x: -0.18 @@ -545,6 +1249,19 @@ docking_server: external_detection_rotation_yaw: 0.0 filter_coef: 0.1 + docks: ['home_dock','flex_dock1', 'flex_dock2'] + home_dock: + type: 'simple_charging_dock' + frame: map + pose: [9.65, 7.65, 0.0] + flex_dock1: + type: 'simple_charging_dock' + frame: map + pose: [10.0, 10.0, 0.0] + flex_dock2: + type: 'simple_charging_dock' + frame: map + pose: [30.0, 30.0, 0.0] # Dock instances # The following example illustrates configuring dock instances. # docks: ['home_dock'] # Input your docks here @@ -554,10 +1271,10 @@ docking_server: # pose: [0.0, 0.0, 0.0] controller: - k_phi: 3.0 - k_delta: 2.0 - v_linear_min: 0.15 - v_linear_max: 0.15 + k_phi: 5.0 + k_delta: 5.0 + v_linear_min: 0.03 + v_linear_max: 0.1 use_collision_detection: true costmap_topic: "local_costmap/costmap_raw" footprint_topic: "local_costmap/published_footprint" @@ -565,6 +1282,8 @@ docking_server: projection_time: 5.0 simulation_step: 0.1 dock_collision_threshold: 0.3 + rotate_to_heading_angular_vel: 0.5 + rotate_to_heading_max_angular_accel: 1.0 loopback_simulator: ros__parameters: diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index 2fa895dd8b8..156ec4afff6 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/nav2_bt_navigator/CMakeLists.txt @@ -1,8 +1,9 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_bt_navigator) find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_behavior_tree REQUIRED) find_package(nav2_common REQUIRED) diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml deleted file mode 100644 index 6c32906f4f9..00000000000 --- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml +++ /dev/null @@ -1,54 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy.xml new file mode 100644 index 00000000000..01c9d8100d2 --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy.xml @@ -0,0 +1,157 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy_test_replanning.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy_test_replanning.xml new file mode 100644 index 00000000000..210d0f7ec88 --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy_test_replanning.xml @@ -0,0 +1,160 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid.xml new file mode 100644 index 00000000000..91192bf0480 --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid.xml @@ -0,0 +1,181 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy copy 2.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy copy 2.xml new file mode 100644 index 00000000000..290cb7607d1 --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy copy 2.xml @@ -0,0 +1,134 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy copy.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy copy.xml new file mode 100644 index 00000000000..627b183a8da --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy copy.xml @@ -0,0 +1,88 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy.xml new file mode 100644 index 00000000000..1e8079d8035 --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy.xml @@ -0,0 +1,157 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_path_if_not_in_goal_proximity.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_path_if_not_in_goal_proximity.xml new file mode 100644 index 00000000000..42030c00b0d --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_path_if_not_in_goal_proximity.xml @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 8a25e5a2c8d..adea1aba20f 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -23,9 +23,9 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_util/odometry_utils.hpp" #include "rclcpp_action/rclcpp_action.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_core/behavior_tree_navigator.hpp" #include "pluginlib/class_loader.hpp" diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 825e8f9eec3..62e5b7307ea 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -11,6 +11,7 @@ nav2_common ament_index_cpp + backward_ros geometry_msgs nav2_behavior_tree nav2_core diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 5f99c1452db..95d3c7dcaa7 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -54,7 +54,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & state) get_node_base_interface(), get_node_timers_interface()); tf_->setCreateTimerInterface(timer_interface); tf_->setUsingDedicatedThread(true); - tf_listener_ = std::make_shared(*tf_, this, false); + tf_listener_ = std::make_shared(*tf_, this, true); global_frame_ = this->declare_or_get_parameter("global_frame", std::string("map")); robot_frame_ = this->declare_or_get_parameter("robot_base_frame", std::string("base_link")); diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 1127c3ab2cd..9b8435ac4a3 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -31,11 +31,11 @@ NavigateThroughPosesNavigator::configure( auto node = parent_node.lock(); goals_blackboard_id_ = - node->declare_or_get_parameter("goals_blackboard_id", std::string("goals")); + node->declare_or_get_parameter(getName() + ".goals_blackboard_id", std::string("goals")); path_blackboard_id_ = - node->declare_or_get_parameter("path_blackboard_id", std::string("path")); + node->declare_or_get_parameter(getName() + ".path_blackboard_id", std::string("path")); waypoint_statuses_blackboard_id_ = - node->declare_or_get_parameter("waypoint_statuses_blackboard_id", + node->declare_or_get_parameter(getName() + ".waypoint_statuses_blackboard_id", std::string("waypoint_statuses")); // Odometry smoother object for getting current speed @@ -73,7 +73,6 @@ bool NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) { auto bt_xml_filename = goal->behavior_tree; - if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, "Error loading XML file: " + bt_xml_filename + ". Navigation canceled."); diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 03d77451e16..a9495645858 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -29,10 +29,10 @@ NavigateToPoseNavigator::configure( start_time_ = rclcpp::Time(0); auto node = parent_node.lock(); - goal_blackboard_id_ = - node->declare_or_get_parameter("goal_blackboard_id", std::string("goal")); - path_blackboard_id_ = - node->declare_or_get_parameter("path_blackboard_id", std::string("path")); + goal_blackboard_id_ = node->declare_or_get_parameter(getName() + ".goal_blackboard_id", + std::string("goal")); + path_blackboard_id_ = node->declare_or_get_parameter(getName() + ".path_blackboard_id", + std::string("path")); // Odometry smoother object for getting current speed odom_smoother_ = odom_smoother; @@ -83,7 +83,6 @@ bool NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) { auto bt_xml_filename = goal->behavior_tree; - if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, std::string("Error loading XML file: ") + bt_xml_filename + ". Navigation canceled."); diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index a7c22e59a09..eb91f295d47 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_collision_monitor) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_costmap_2d REQUIRED) @@ -16,6 +17,7 @@ find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(visualization_msgs REQUIRED) find_package(nav2_ros_common REQUIRED) +find_package(point_cloud_transport REQUIRED) nav2_package() @@ -53,6 +55,7 @@ target_link_libraries(${monitor_library_name} PUBLIC tf2::tf2 tf2_ros::tf2_ros ${visualization_msgs_TARGETS} + point_cloud_transport::point_cloud_transport ) target_link_libraries(${monitor_library_name} PRIVATE rclcpp_components::component @@ -87,6 +90,7 @@ target_link_libraries(${detector_library_name} PUBLIC tf2_ros::tf2_ros tf2::tf2 ${visualization_msgs_TARGETS} + point_cloud_transport::point_cloud_transport ) target_link_libraries(${detector_library_name} PRIVATE rclcpp_components::component @@ -168,6 +172,7 @@ ament_export_dependencies( tf2_ros nav2_ros_common visualization_msgs + point_cloud_transport ) ament_export_targets(export_${PROJECT_NAME}) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index 59b807f562e..450e97c8078 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -23,8 +23,8 @@ #include "rclcpp/rclcpp.hpp" #include "tf2/time.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_msgs/msg/collision_detector_state.hpp" diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index d73804cb827..c53540089d7 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -26,8 +26,8 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "tf2/time.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_util/twist_publisher.hpp" diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index 8a46d2b65c5..c94180f94cf 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -20,6 +20,7 @@ #include #include "sensor_msgs/msg/point_cloud2.hpp" +#include "point_cloud_transport/point_cloud_transport.hpp" #include "nav2_collision_monitor/source.hpp" @@ -98,7 +99,15 @@ class PointCloud : public Source // ----- Variables ----- /// @brief PointCloud data subscriber + #if RCLCPP_VERSION_GTE(30, 0, 0) + std::shared_ptr pct_; + point_cloud_transport::Subscriber data_sub_; + #else nav2::Subscription::SharedPtr data_sub_; + #endif + + // Transport type used for PointCloud messages (e.g., raw or compressed) + std::string transport_type_; // Minimum and maximum height of PointCloud projected to 2D space double min_height_, max_height_; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 12d64259993..02fc680afe5 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -24,7 +24,7 @@ #include "geometry_msgs/msg/polygon_stamped.hpp" #include "tf2/time.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_costmap_2d/footprint_subscriber.hpp" @@ -239,13 +239,6 @@ class Polygon rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( std::vector parameters); - /** - * @brief Checks if point is inside polygon - * @param point Given point to check - * @return True if given point is inside polygon, otherwise false - */ - bool isPointInside(const Point & point) const; - /** * @brief Extracts Polygon points from a string with of the form [[x1,y1],[x2,y2],[x3,y3]...] * @param poly_string Input String containing the verteceis of the polygon diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 312c13ea577..4e91dab13c7 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -22,7 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "tf2/time.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_ros_common/lifecycle_node.hpp" diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp index 55f894d87c5..8056973ddf9 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp @@ -24,7 +24,7 @@ #include "nav2_collision_monitor/types.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" namespace nav2_collision_monitor { diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 47ffe9a6d60..6b009f1634b 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -11,6 +11,7 @@ ament_cmake nav2_common + backward_ros geometry_msgs nav2_util nav2_costmap_2d @@ -24,6 +25,8 @@ tf2_ros visualization_msgs nav2_ros_common + point_cloud_transport + point_cloud_transport_plugins ament_cmake_gtest ament_lint_common diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml index eb3ee0a8739..bbeaadc5154 100644 --- a/nav2_collision_monitor/params/collision_detector_params.yaml +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -23,6 +23,7 @@ collision_detector: pointcloud: type: "pointcloud" topic: "/intel_realsense_r200_depth/points" + transport_type: "raw" # Options: raw, zlib, draco, zstd min_height: 0.1 max_height: 0.5 enabled: True diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index eac7486913c..28224362ea9 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -96,6 +96,7 @@ collision_monitor: pointcloud: type: "pointcloud" topic: "/intel_realsense_r200_depth/points" + transport_type: "raw" # Options: raw, zlib, draco, zstd min_height: 0.1 max_height: 0.5 enabled: True diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 739d0d12207..91023afae4f 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -19,7 +19,7 @@ #include #include -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_ros_common/node_utils.hpp" @@ -50,7 +50,7 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & state) this->get_node_base_interface(), this->get_node_timers_interface()); tf_buffer_->setCreateTimerInterface(timer_interface); - tf_listener_ = std::make_shared(*tf_buffer_); + tf_listener_ = std::make_shared(*tf_buffer_, this, true); state_pub_ = this->create_publisher( "collision_detector_state"); diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index f85bdf4a866..d7ec6131220 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -18,7 +18,7 @@ #include #include -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -52,7 +52,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state) this->get_node_base_interface(), this->get_node_timers_interface()); tf_buffer_->setCreateTimerInterface(timer_interface); - tf_listener_ = std::make_shared(*tf_buffer_); + tf_listener_ = std::make_shared(*tf_buffer_, this, true); std::string cmd_vel_in_topic; std::string cmd_vel_out_topic; diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 590eacb3cb8..72ef467b9a5 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -45,7 +45,11 @@ PointCloud::PointCloud( PointCloud::~PointCloud() { RCLCPP_INFO(logger_, "[%s]: Destroying PointCloud", source_name_.c_str()); + #if RCLCPP_VERSION_GTE(30, 0, 0) + data_sub_.shutdown(); + #else data_sub_.reset(); + #endif } void PointCloud::configure() @@ -60,10 +64,20 @@ void PointCloud::configure() getParameters(source_topic); + #if RCLCPP_VERSION_GTE(30, 0, 0) + const point_cloud_transport::TransportHints hint(transport_type_); + pct_ = std::make_shared(*node); + data_sub_ = pct_->subscribe( + source_topic, nav2::qos::SensorDataQoS(), + std::bind(&PointCloud::dataCallback, this, std::placeholders::_1), + {}, &hint + ); + #else data_sub_ = node->create_subscription( source_topic, std::bind(&PointCloud::dataCallback, this, std::placeholders::_1), nav2::qos::SensorDataQoS()); + #endif // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( @@ -131,6 +145,9 @@ void PointCloud::getParameters(std::string & source_topic) nav2::declare_parameter_if_not_declared( node, source_name_ + ".min_range", rclcpp::ParameterValue(0.0)); min_range_ = node->get_parameter(source_name_ + ".min_range").as_double(); + nav2::declare_parameter_if_not_declared( + node, source_name_ + ".transport_type", rclcpp::ParameterValue(std::string("raw"))); + transport_type_ = node->get_parameter(source_name_ + ".transport_type").as_string(); } void PointCloud::dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 5e13f5d5ef8..b37f677fb57 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -22,6 +22,7 @@ #include "tf2/transform_datatypes.hpp" #include "nav2_ros_common/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/array_parser.hpp" @@ -229,7 +230,7 @@ int Polygon::getPointsInside(const std::vector & points) const { int num = 0; for (const Point & point : points) { - if (isPointInside(point)) { + if (nav2_util::geometry_utils::isPointInsidePolygon(point.x, point.y, poly_)) { num++; } } @@ -602,38 +603,6 @@ void Polygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr updatePolygon(msg); } -inline bool Polygon::isPointInside(const Point & point) const -{ - // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon." - // Communications of the ACM 5.8 (1962): 434. - // Implementation of ray crossings algorithm for point in polygon task solving. - // Y coordinate is fixed. Moving the ray on X+ axis starting from given point. - // Odd number of intersections with polygon boundaries means the point is inside polygon. - const int poly_size = poly_.size(); - int i, j; // Polygon vertex iterators - bool res = false; // Final result, initialized with already inverted value - - // Starting from the edge where the last point of polygon is connected to the first - i = poly_size - 1; - for (j = 0; j < poly_size; j++) { - // Checking the edge only if given point is between edge boundaries by Y coordinates. - // One of the condition should contain equality in order to exclude the edges - // parallel to X+ ray. - if ((point.y <= poly_[i].y) == (point.y > poly_[j].y)) { - // Calculating the intersection coordinate of X+ ray - const double x_inter = poly_[i].x + - (point.y - poly_[i].y) * (poly_[j].x - poly_[i].x) / - (poly_[j].y - poly_[i].y); - // If intersection with checked edge is greater than point.x coordinate, inverting the result - if (x_inter > point.x) { - res = !res; - } - } - i = j; - } - return res; -} - bool Polygon::getPolygonFromString( std::string & poly_string, std::vector & polygon) diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 1bd2eb0ca0c..4bbd37bc4e9 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -34,7 +34,7 @@ #include "geometry_msgs/msg/polygon_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/collision_detector_node.hpp" diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index d64f0bd71e8..ea0213f468c 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -34,7 +34,7 @@ #include "geometry_msgs/msg/polygon_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/collision_monitor_node.hpp" diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index 52eff3c650b..86f393febfa 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -27,9 +27,9 @@ #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/polygon.hpp" diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index 04929bd339c..034cdd40d96 100644 --- a/nav2_collision_monitor/test/sources_test.cpp +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -30,9 +30,9 @@ #include "sensor_msgs/msg/range.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/scan.hpp" diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index d13fc873340..88024dd723b 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -27,9 +27,9 @@ #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/polygon.hpp" diff --git a/nav2_common/CMakeLists.txt b/nav2_common/CMakeLists.txt index 8504b8b23de..71a771b29c4 100644 --- a/nav2_common/CMakeLists.txt +++ b/nav2_common/CMakeLists.txt @@ -1,9 +1,10 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_common NONE) find_package(ament_cmake_core REQUIRED) find_package(ament_cmake_python REQUIRED) +find_package(backward_ros REQUIRED) ament_python_install_package(nav2_common) diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 7aa6a3925eb..12bbf129ccd 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -7,6 +7,7 @@ Carl Delsey Apache-2.0 + backward_ros launch launch_ros osrf_pycommon diff --git a/nav2_constrained_smoother/CMakeLists.txt b/nav2_constrained_smoother/CMakeLists.txt index 31e83e77fe5..6e179687f94 100644 --- a/nav2_constrained_smoother/CMakeLists.txt +++ b/nav2_constrained_smoother/CMakeLists.txt @@ -1,9 +1,10 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_constrained_smoother) set(CMAKE_BUILD_TYPE Release) # significant Ceres optimization speedup find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(Ceres REQUIRED COMPONENTS SuiteSparse) find_package(Eigen3 REQUIRED) find_package(geometry_msgs REQUIRED) diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index 11fb47b1b06..eb5b60ae9b4 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -12,6 +12,7 @@ eigen nav2_common + backward_ros geometry_msgs libceres-dev nav2_core diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index b9285fa9240..6e4e3d4bc90 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -23,7 +23,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" #include "nav2_costmap_2d/footprint_collision_checker.hpp" diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index a100f96a214..c8a655f4340 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -1,8 +1,9 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_controller) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(nav2_core REQUIRED) @@ -10,8 +11,6 @@ find_package(nav2_common REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) -find_package(nav_2d_msgs REQUIRED) -find_package(nav_2d_utils REQUIRED) find_package(pluginlib REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(rclcpp REQUIRED) @@ -41,9 +40,6 @@ target_link_libraries(${library_name} PUBLIC ${nav2_msgs_TARGETS} nav2_util::nav2_util_core nav2_ros_common::nav2_ros_common - ${nav_2d_msgs_TARGETS} - nav_2d_utils::conversions - nav_2d_utils::tf_help pluginlib::pluginlib rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle @@ -85,7 +81,6 @@ target_link_libraries(simple_progress_checker PUBLIC ) target_link_libraries(simple_progress_checker PRIVATE nav2_util::nav2_util_core - nav_2d_utils::conversions pluginlib::pluginlib ) @@ -106,7 +101,6 @@ target_link_libraries(pose_progress_checker PUBLIC target_link_libraries(pose_progress_checker PRIVATE angles::angles nav2_util::nav2_util_core - nav_2d_utils::conversions pluginlib::pluginlib ) @@ -217,8 +211,6 @@ ament_export_dependencies( nav2_costmap_2d nav2_msgs nav2_util - nav_2d_msgs - nav_2d_utils pluginlib rclcpp rclcpp_lifecycle diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index a112a3c9d66..b68c8e5bb94 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -26,13 +26,13 @@ #include "nav2_core/progress_checker.hpp" #include "nav2_core/goal_checker.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" #include "nav2_msgs/action/follow_path.hpp" #include "nav2_msgs/msg/speed_limit.hpp" -#include "nav_2d_utils/odom_subscriber.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_ros_common/simple_action_server.hpp" #include "nav2_util/robot_utils.hpp" +#include "nav2_util/odometry_utils.hpp" #include "nav2_util/twist_publisher.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" @@ -208,12 +208,12 @@ class ControllerServer : public nav2::LifecycleNode * @param Twist The current Twist from odometry * @return Twist Twist after thresholds applied */ - nav_2d_msgs::msg::Twist2D getThresholdedTwist(const nav_2d_msgs::msg::Twist2D & twist) + geometry_msgs::msg::Twist getThresholdedTwist(const geometry_msgs::msg::Twist & twist) { - nav_2d_msgs::msg::Twist2D twist_thresh; - twist_thresh.x = getThresholdedVelocity(twist.x, min_x_velocity_threshold_); - twist_thresh.y = getThresholdedVelocity(twist.y, min_y_velocity_threshold_); - twist_thresh.theta = getThresholdedVelocity(twist.theta, min_theta_velocity_threshold_); + geometry_msgs::msg::Twist twist_thresh; + twist_thresh.linear.x = getThresholdedVelocity(twist.linear.x, min_x_velocity_threshold_); + twist_thresh.linear.y = getThresholdedVelocity(twist.linear.y, min_y_velocity_threshold_); + twist_thresh.angular.z = getThresholdedVelocity(twist.angular.z, min_theta_velocity_threshold_); return twist_thresh; } @@ -233,7 +233,7 @@ class ControllerServer : public nav2::LifecycleNode std::unique_ptr costmap_thread_; // Publishers and subscribers - std::unique_ptr odom_sub_; + std::unique_ptr odom_sub_; std::unique_ptr vel_publisher_; nav2::Subscription::SharedPtr speed_limit_sub_; diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 16875164c0d..9360c10d05f 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -11,14 +11,13 @@ nav2_common angles + backward_ros geometry_msgs lifecycle_msgs nav2_core nav2_costmap_2d nav2_msgs nav2_util - nav_2d_msgs - nav_2d_utils pluginlib rcl_interfaces rclcpp diff --git a/nav2_controller/plugins/pose_progress_checker.cpp b/nav2_controller/plugins/pose_progress_checker.cpp index 453d20992e8..a891104cac0 100644 --- a/nav2_controller/plugins/pose_progress_checker.cpp +++ b/nav2_controller/plugins/pose_progress_checker.cpp @@ -18,7 +18,6 @@ #include #include #include "angles/angles.h" -#include "nav_2d_utils/conversions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" #include "nav2_ros_common/node_utils.hpp" diff --git a/nav2_controller/plugins/simple_progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp index 2e219cdeee8..df3cb77df69 100644 --- a/nav2_controller/plugins/simple_progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -17,7 +17,6 @@ #include #include #include -#include "nav_2d_utils/conversions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" #include "nav2_ros_common/node_utils.hpp" diff --git a/nav2_controller/plugins/test/CMakeLists.txt b/nav2_controller/plugins/test/CMakeLists.txt index cd8048513d4..ba0ce9eb2d5 100644 --- a/nav2_controller/plugins/test/CMakeLists.txt +++ b/nav2_controller/plugins/test/CMakeLists.txt @@ -1,5 +1,5 @@ ament_add_gtest(pctest progress_checker.cpp) -target_link_libraries(pctest simple_progress_checker pose_progress_checker nav_2d_utils::conversions) +target_link_libraries(pctest simple_progress_checker pose_progress_checker) ament_add_gtest(gctest goal_checker.cpp) -target_link_libraries(gctest simple_goal_checker stopped_goal_checker nav_2d_utils::conversions) +target_link_libraries(gctest simple_goal_checker stopped_goal_checker) diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index 4730aeb3a04..1aba1ba80bb 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -38,7 +38,6 @@ #include "gtest/gtest.h" #include "nav2_controller/plugins/simple_goal_checker.hpp" #include "nav2_controller/plugins/stopped_goal_checker.hpp" -#include "nav_2d_utils/conversions.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "eigen3/Eigen/Geometry" diff --git a/nav2_controller/plugins/test/progress_checker.cpp b/nav2_controller/plugins/test/progress_checker.cpp index 0791b9e9c27..293daf13a25 100644 --- a/nav2_controller/plugins/test/progress_checker.cpp +++ b/nav2_controller/plugins/test/progress_checker.cpp @@ -38,7 +38,6 @@ #include "gtest/gtest.h" #include "nav2_controller/plugins/simple_progress_checker.hpp" #include "nav2_controller/plugins/pose_progress_checker.hpp" -#include "nav_2d_utils/conversions.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_util/geometry_utils.hpp" diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 26187087ec1..98a6eda8629 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -21,8 +21,6 @@ #include "lifecycle_msgs/msg/state.hpp" #include "nav2_core/controller_exceptions.hpp" -#include "nav_2d_utils/conversions.hpp" -#include "nav_2d_utils/tf_help.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_controller/controller_server.hpp" @@ -65,6 +63,9 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) declare_parameter("publish_zero_velocity", rclcpp::ParameterValue(true)); declare_parameter("costmap_update_timeout", 0.30); // 300ms + declare_parameter("odom_topic", rclcpp::ParameterValue("odom")); + declare_parameter("odom_duration", rclcpp::ParameterValue(0.3)); + // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( "local_costmap", std::string{get_namespace()}, @@ -125,8 +126,11 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_); RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_); - std::string speed_limit_topic; + std::string speed_limit_topic, odom_topic; get_parameter("speed_limit_topic", speed_limit_topic); + get_parameter("odom_topic", odom_topic); + double odom_duration; + get_parameter("odom_duration", odom_duration); get_parameter("failure_tolerance", failure_tolerance_); get_parameter("use_realtime_priority", use_realtime_priority_); get_parameter("publish_zero_velocity", publish_zero_velocity_); @@ -219,7 +223,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) get_logger(), "Controller Server has %s controllers available.", controller_ids_concat_.c_str()); - odom_sub_ = std::make_unique(node); + odom_sub_ = std::make_unique(node, odom_duration, odom_topic); vel_publisher_ = std::make_unique(node, "cmd_vel"); double costmap_update_timeout_dbl; @@ -624,7 +628,7 @@ void ControllerServer::computeAndPublishVelocity() throw nav2_core::FailedToMakeProgress("Failed to make progress"); } - nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); + geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist()); geometry_msgs::msg::TwistStamped cmd_vel_2d; @@ -632,7 +636,7 @@ void ControllerServer::computeAndPublishVelocity() cmd_vel_2d = controllers_[current_controller_]->computeVelocityCommands( pose, - nav_2d_utils::twist2Dto3D(twist), + twist, goal_checkers_[current_goal_checker_].get()); last_valid_cmd_time_ = now(); cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID(); @@ -665,10 +669,9 @@ void ControllerServer::computeAndPublishVelocity() // Find the closest pose to current pose on global path geometry_msgs::msg::PoseStamped robot_pose_in_path_frame; - rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance())); - if (!nav_2d_utils::transformPose( - costmap_ros_->getTfBuffer(), current_path_.header.frame_id, pose, - robot_pose_in_path_frame, tolerance)) + if (!nav2_util::transformPoseInTargetFrame( + pose, robot_pose_in_path_frame, *costmap_ros_->getTfBuffer(), + current_path_.header.frame_id, costmap_ros_->getTransformTolerance())) { throw nav2_core::ControllerTFError("Failed to transform robot pose to path frame"); } @@ -793,14 +796,13 @@ bool ControllerServer::isGoalReached() return false; } - nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); - geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(twist); + geometry_msgs::msg::Twist velocity = getThresholdedTwist(odom_sub_->getRawTwist()); geometry_msgs::msg::PoseStamped transformed_end_pose; - rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance())); - nav_2d_utils::transformPose( - costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(), - end_pose_, transformed_end_pose, tolerance); + end_pose_.header.stamp = pose.header.stamp; + nav2_util::transformPoseInTargetFrame( + end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(), + costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance()); return goal_checkers_[current_goal_checker_]->isGoalReached( pose.pose, transformed_end_pose.pose, diff --git a/nav2_core/CMakeLists.txt b/nav2_core/CMakeLists.txt index 1d0e6b724f5..1f7ae52aa05 100644 --- a/nav2_core/CMakeLists.txt +++ b/nav2_core/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_core) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_behavior_tree REQUIRED) find_package(nav2_common REQUIRED) diff --git a/nav2_core/include/nav2_core/behavior.hpp b/nav2_core/include/nav2_core/behavior.hpp index b2e00e8cf53..7a58d3aa2a0 100644 --- a/nav2_core/include/nav2_core/behavior.hpp +++ b/nav2_core/include/nav2_core/behavior.hpp @@ -20,7 +20,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_costmap_2d/costmap_topic_collision_checker.hpp" namespace nav2_core diff --git a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp index 87824c7fd31..fdabbf28133 100644 --- a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp +++ b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp @@ -21,7 +21,7 @@ #include #include "nav2_util/odometry_utils.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "rclcpp/rclcpp.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "pluginlib/class_loader.hpp" @@ -201,6 +201,12 @@ class BehaviorTreeNavigator : public NavigatorBase // get the default behavior tree for this navigator std::string default_bt_xml_filename = getDefaultBTFilepath(parent_node); + auto search_directories = node->declare_or_get_parameter( + "bt_search_directories", + std::vector{ament_index_cpp::get_package_share_directory( + "nav2_bt_navigator") + "/behavior_trees"} + ); + // Create the Behavior Tree Action Server for this navigator bt_action_server_ = std::make_unique>( @@ -213,7 +219,8 @@ class BehaviorTreeNavigator : public NavigatorBase std::bind(&BehaviorTreeNavigator::onPreempt, this, std::placeholders::_1), std::bind( &BehaviorTreeNavigator::onCompletion, this, - std::placeholders::_1, std::placeholders::_2)); + std::placeholders::_1, std::placeholders::_2), + search_directories); bool ok = true; if (!bt_action_server_->on_configure()) { diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index 261eec22436..d24e84d6680 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -41,7 +41,7 @@ #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" #include "pluginlib/class_loader.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" diff --git a/nav2_core/include/nav2_core/global_planner.hpp b/nav2_core/include/nav2_core/global_planner.hpp index bcf65da6d4f..fa63d2e519e 100644 --- a/nav2_core/include/nav2_core/global_planner.hpp +++ b/nav2_core/include/nav2_core/global_planner.hpp @@ -18,7 +18,7 @@ #include #include #include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_ros_common/lifecycle_node.hpp" diff --git a/nav2_core/include/nav2_core/smoother.hpp b/nav2_core/include/nav2_core/smoother.hpp index 7705106177d..deb4a6107ec 100644 --- a/nav2_core/include/nav2_core/smoother.hpp +++ b/nav2_core/include/nav2_core/smoother.hpp @@ -20,8 +20,8 @@ #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_costmap_2d/footprint_subscriber.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "pluginlib/class_loader.hpp" #include "nav_msgs/msg/path.hpp" diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 044eb215676..aa95ad053fd 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -11,6 +11,7 @@ ament_cmake nav2_common + backward_ros geometry_msgs nav2_behavior_tree nav2_costmap_2d diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 221c5595515..00cb720aa15 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -1,8 +1,9 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_costmap_2d) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(laser_geometry REQUIRED) find_package(map_msgs REQUIRED) @@ -25,6 +26,7 @@ find_package(tf2_ros REQUIRED) find_package(nav2_ros_common REQUIRED) find_package(tf2_sensor_msgs REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(point_cloud_transport REQUIRED) nav2_package() @@ -87,6 +89,7 @@ target_link_libraries(layers PUBLIC nav2_ros_common::nav2_ros_common tf2::tf2 nav2_ros_common::nav2_ros_common + point_cloud_transport::point_cloud_transport ) target_link_libraries(layers PRIVATE pluginlib::pluginlib @@ -221,6 +224,7 @@ ament_export_dependencies( tf2_ros tf2_sensor_msgs nav2_ros_common + point_cloud_transport ) pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) ament_package() diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index fbacf15aa24..fe3764c443a 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -461,105 +461,7 @@ class Costmap2D */ virtual void initMaps(unsigned int size_x, unsigned int size_y); - /** - * @brief Raytrace a line and apply some action at each step - * @param at The action to take... a functor - * @param x0 The starting x coordinate - * @param y0 The starting y coordinate - * @param x1 The ending x coordinate - * @param y1 The ending y coordinate - * @param max_length The maximum desired length of the segment... - * allows you to not go all the way to the endpoint - * @param min_length The minimum desired length of the segment - */ - template - inline void raytraceLine( - ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, - unsigned int y1, - unsigned int max_length = UINT_MAX, unsigned int min_length = 0) - { - int dx_full = x1 - x0; - int dy_full = y1 - y0; - - // we need to chose how much to scale our dominant dimension, - // based on the maximum length of the line - double dist = std::hypot(dx_full, dy_full); - if (dist < min_length) { - return; - } - - unsigned int min_x0, min_y0; - if (dist > 0.0) { - // Adjust starting point and offset to start from min_length distance - min_x0 = (unsigned int)(x0 + dx_full / dist * min_length); - min_y0 = (unsigned int)(y0 + dy_full / dist * min_length); - } else { - // dist can be 0 if [x0, y0]==[x1, y1]. - // In this case only this cell should be processed. - min_x0 = x0; - min_y0 = y0; - } - unsigned int offset = min_y0 * size_x_ + min_x0; - - int dx = x1 - min_x0; - int dy = y1 - min_y0; - - unsigned int abs_dx = abs(dx); - unsigned int abs_dy = abs(dy); - - int offset_dx = sign(dx); - int offset_dy = sign(dy) * size_x_; - - double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist); - // if x is dominant - if (abs_dx >= abs_dy) { - int error_y = abs_dx / 2; - - bresenham2D( - at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx)); - return; - } - - // otherwise y is dominant - int error_x = abs_dy / 2; - - bresenham2D( - at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy)); - } - private: - /** - * @brief A 2D implementation of Bresenham's raytracing algorithm... - * applies an action at each step - */ - template - inline void bresenham2D( - ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, - int offset_a, - int offset_b, unsigned int offset, - unsigned int max_length) - { - unsigned int end = std::min(max_length, abs_da); - for (unsigned int i = 0; i < end; ++i) { - at(offset); - offset += offset_a; - error_b += abs_db; - if ((unsigned int)error_b >= abs_da) { - offset += offset_b; - error_b -= abs_da; - } - } - at(offset); - } - - /** - * @brief get the sign of an int - */ - inline int sign(int x) - { - return x > 0 ? 1.0 : -1.0; - } - mutex_t * access_; protected: 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 b18938352a3..5b1fa01591b 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -56,8 +56,8 @@ #include "pluginlib/class_loader.hpp" #include "tf2/convert.hpp" #include "tf2/LinearMath/Transform.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "tf2/time.hpp" #include "tf2/transform_datatypes.hpp" #include "nav2_ros_common/service_server.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index e51809df696..da4720ff798 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -99,7 +99,7 @@ class CostmapFilter : public Layer */ void updateBounds( double robot_x, double robot_y, double robot_yaw, - double * min_x, double * min_y, double * max_x, double * max_y) final; + double * min_x, double * min_y, double * max_x, double * max_y) override; /** * @brief Update the costs in the master costmap in the window @@ -185,20 +185,6 @@ class CostmapFilter : public Layer const std::string mask_frame, geometry_msgs::msg::Pose & mask_pose) const; - /** - * @brief: Convert from world coordinates to mask coordinates. - Similar to Costmap2D::worldToMap() method but works directly with OccupancyGrid-s. - * @param filter_mask Filter mask on which to convert - * @param wx The x world coordinate - * @param wy The y world coordinate - * @param mx Will be set to the associated mask x coordinate - * @param my Will be set to the associated mask y coordinate - * @return True if the conversion was successful (legal bounds) false otherwise - */ - bool worldToMask( - nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, - double wx, double wy, unsigned int & mx, unsigned int & my) const; - /** * @brief Get the data of a cell in the filter mask * @param filter_mask Filter mask to get the data from diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp index e615ba465a0..118fc3ef295 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp @@ -68,6 +68,20 @@ class KeepoutFilter : public CostmapFilter void initializeFilter( const std::string & filter_info_topic); + /** + * @brief Update the bounds of the master costmap by this layer's update dimensions + * @param robot_x X pose of robot + * @param robot_y Y pose of robot + * @param robot_yaw Robot orientation + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ + void updateBounds( + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, double * max_x, double * max_y) override; + /** * @brief Process the keepout layer at the current pose / bounds / grid */ @@ -106,8 +120,15 @@ class KeepoutFilter : public CostmapFilter bool override_lethal_cost_{false}; // If true, lethal cost will be overridden unsigned char lethal_override_cost_{252}; // Value to override lethal cost with bool last_pose_lethal_{false}; // If true, last pose was lethal - unsigned int lethal_state_update_min_x_{999999u}, lethal_state_update_min_y_{999999u}; - unsigned int lethal_state_update_max_x_{0u}, lethal_state_update_max_y_{0u}; + bool is_pose_lethal_{false}; + double lethal_state_update_min_x_, lethal_state_update_min_y_; + double lethal_state_update_max_x_, lethal_state_update_max_y_; + + unsigned int x_{0}; + unsigned int y_{0}; + unsigned int width_{0}; + unsigned int height_{0}; + bool has_updated_data_{false}; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker copy.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker copy.hpp new file mode 100644 index 00000000000..36b16755846 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker copy.hpp @@ -0,0 +1,94 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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. +// +// Modified by: Shivang Patel (shivaang14@gmail.com) + +#ifndef NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_ +#define NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/robot_utils.hpp" + +namespace nav2_costmap_2d +{ +typedef std::vector Footprint; + +/** + * @class FootprintCollisionChecker + * @brief Checker for collision with a footprint on a costmap + */ +template +class FootprintCollisionChecker +{ +public: + /** + * @brief A constructor. + */ + FootprintCollisionChecker(); + /** + * @brief A constructor. + */ + explicit FootprintCollisionChecker(CostmapT costmap); + /** + * @brief Find the footprint cost in oriented footprint + * @param footprint The footprint to check + * @param check_full_area If true, checks the full area after perimeter check (default: false) + * @return The maximum cost found (perimeter only or full area depending on parameter) + */ + double footprintCost(const Footprint & footprint, bool check_full_area = false); + /** + * @brief Find the footprint cost a a post with an unoriented footprint + */ + double footprintCostAtPose( + double x, double y, double theta, const Footprint & footprint, + bool check_full_area = false); + /** + * @brief Get the cost for a line segment + */ + double lineCost(int x0, int x1, int y0, int y1) const; + /** + * @brief Get the map coordinates from a world point + */ + bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my); + /** + * @brief Get the cost of a point + */ + double pointCost(int x, int y) const; + /** + * @brief Set the current costmap object to use for collision detection + */ + void setCostmap(CostmapT costmap); + /** + * @brief Get the current costmap object + */ + CostmapT getCostmap() + { + return costmap_; + } + +protected: + CostmapT costmap_; +}; + +} // namespace nav2_costmap_2d + +#endif // NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index 2136db7c856..571df68c9d3 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp @@ -41,7 +41,7 @@ #include #include -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp index c54d586b729..09f0e79c114 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp @@ -43,7 +43,7 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "rclcpp/time.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "tf2_sensor_msgs/tf2_sensor_msgs.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "nav2_costmap_2d/observation.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp index 16113d61b16..9e035eb6aeb 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -47,10 +47,11 @@ #include "laser_geometry/laser_geometry.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wreorder" -#include "tf2_ros/message_filter.h" +#include "tf2_ros/message_filter.hpp" #pragma GCC diagnostic pop #include "message_filters/subscriber.hpp" +#include "point_cloud_transport/subscriber_filter.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "sensor_msgs/msg/laser_scan.hpp" @@ -219,6 +220,7 @@ class ObstacleLayer : public CostmapLayer std::vector transformed_footprint_; bool footprint_clearing_enabled_; + double footprint_clearing_padding_; /** * @brief Clear costmap layer info below the robot's footprint */ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp index 2e00a412637..8a9aeb29623 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp @@ -27,7 +27,7 @@ #include "nav2_costmap_2d/costmap_layer.hpp" #include "nav2_costmap_2d/observation_buffer.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" -#include "tf2_ros/message_filter.h" +#include "tf2_ros/message_filter.hpp" #include "pluginlib/class_loader.hpp" using nav2_costmap_2d::LETHAL_OBSTACLE; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index 7da443ebd25..6ffdbca4980 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -153,6 +153,15 @@ class StaticLayer : public CostmapLayer */ unsigned char interpretValue(unsigned char value); + /** + * @brief Check if two double values are equal within a given epsilon + * @param a First double value + * @param b Second double value + * @param epsilon The tolerance for equality check + * @return True if the values are equal within the tolerance, false otherwise + */ + bool isEqual(double a, double b, double epsilon); + /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message @@ -163,6 +172,7 @@ class StaticLayer : public CostmapLayer std::vector transformed_footprint_; bool footprint_clearing_enabled_; bool restore_cleared_footprint_; + double footprint_clearing_padding_; /** * @brief Clear costmap layer info below the robot's footprint */ diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index c01e7bb2dd6..4e4940a59af 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -20,6 +20,7 @@ nav2_common angles + backward_ros geometry_msgs laser_geometry map_msgs @@ -41,6 +42,8 @@ tf2_sensor_msgs visualization_msgs nav2_ros_common + point_cloud_transport + point_cloud_transport_plugins ament_lint_common ament_lint_auto diff --git a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp index 097537d40c6..c805a537396 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp @@ -44,6 +44,7 @@ #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" #include "nav2_util/occ_grid_values.hpp" +#include "nav2_util/occ_grid_utils.hpp" namespace nav2_costmap_2d { @@ -190,8 +191,8 @@ void BinaryFilter::process( // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j) unsigned int mask_robot_i, mask_robot_j; - if (!worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i, - mask_robot_j)) + if (!nav2_util::worldToMap(filter_mask_, mask_pose.position.x, mask_pose.position.y, + mask_robot_i, mask_robot_j)) { // Robot went out of mask range. Set "false" state by-default RCLCPP_WARN( diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index bc4e89f878e..58b89740b8b 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -193,36 +193,13 @@ bool CostmapFilter::transformPose( return true; } -bool CostmapFilter::worldToMask( - nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, - double wx, double wy, unsigned int & mx, unsigned int & my) const -{ - const double origin_x = filter_mask->info.origin.position.x; - const double origin_y = filter_mask->info.origin.position.y; - const double resolution = filter_mask->info.resolution; - const unsigned int size_x = filter_mask->info.width; - const unsigned int size_y = filter_mask->info.height; - - if (wx < origin_x || wy < origin_y) { - return false; - } - - mx = static_cast((wx - origin_x) / resolution); - my = static_cast((wy - origin_y) / resolution); - if (mx >= size_x || my >= size_y) { - return false; - } - - return true; -} - unsigned char CostmapFilter::getMaskCost( nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, const unsigned int mx, const unsigned int & my) const { const unsigned int index = my * filter_mask->info.width + mx; - const char data = filter_mask->data[index]; + const signed char data = filter_mask->data[index]; if (data == nav2_util::OCC_GRID_UNKNOWN) { return NO_INFORMATION; } else { diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index e727d00634e..c432a917e59 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -35,14 +35,17 @@ * Author: Alexey Merzlyakov *********************************************************************/ +#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp" + #include #include #include #include "tf2/convert.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/occ_grid_utils.hpp" namespace nav2_costmap_2d { @@ -84,6 +87,8 @@ void KeepoutFilter::initializeFilter( // clamp lethal_override_cost_ in case if higher than MAX_NON_OBSTACLE is given lethal_override_cost_ = \ std::clamp(lethal_override_cost_, FREE_SPACE, MAX_NON_OBSTACLE); + lethal_state_update_max_x_ = lethal_state_update_max_y_ = std::numeric_limits::lowest(); + lethal_state_update_min_x_ = lethal_state_update_min_y_ = std::numeric_limits::max(); } void KeepoutFilter::filterInfoCallback( @@ -155,12 +160,86 @@ void KeepoutFilter::maskCallback( // Store filter_mask_ filter_mask_ = msg; + has_updated_data_ = true; + x_ = y_ = 0; + width_ = msg->info.width; + height_ = msg->info.height; +} + +void KeepoutFilter::updateBounds( + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, double * max_x, double * max_y) +{ + if (!enabled_) { + return; + } + + CostmapFilter::updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); + + // If new keepout zone received + if(has_updated_data_) { + double wx, wy; + layered_costmap_->getCostmap()->mapToWorld(x_, y_, wx, wy); + *min_x = std::min(wx, *min_x); + *min_y = std::min(wy, *min_y); + + layered_costmap_->getCostmap()->mapToWorld(x_ + width_, y_ + height_, wx, wy); + *max_x = std::max(wx, *max_x); + *max_y = std::max(wy, *max_y); + + has_updated_data_ = false; + return; + } + + // Let's find the pose's cost if we are allowed to override the lethal cost + is_pose_lethal_ = false; + if (override_lethal_cost_) { + geometry_msgs::msg::Pose pose; + pose.position.x = robot_x; + pose.position.y = robot_y; + pose.position.z = 0.0; + pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(robot_yaw); + geometry_msgs::msg::Pose mask_pose; + if (transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) { + unsigned int mask_robot_i, mask_robot_j; + if (nav2_util::worldToMap(filter_mask_, mask_pose.position.x, mask_pose.position.y, + mask_robot_i, mask_robot_j)) + { + auto data = getMaskCost(filter_mask_, mask_robot_i, mask_robot_j); + is_pose_lethal_ = (data == INSCRIBED_INFLATED_OBSTACLE || data == LETHAL_OBSTACLE); + if (is_pose_lethal_) { + RCLCPP_WARN_THROTTLE( + logger_, *(clock_), 2000, + "KeepoutFilter: Pose is in keepout zone, reducing cost override to navigate out."); + } + } + } + + // If in lethal space or just exited lethal space, + // we need to update all possible spaces touched during this state + if (is_pose_lethal_ || (last_pose_lethal_ && !is_pose_lethal_)) { + lethal_state_update_min_x_ = std::min(*min_x, lethal_state_update_min_x_); + *min_x = lethal_state_update_min_x_; + lethal_state_update_min_y_ = std::min(*min_y, lethal_state_update_min_y_); + *min_y = lethal_state_update_min_y_; + lethal_state_update_max_x_ = std::max(*max_x, lethal_state_update_max_x_); + *max_x = lethal_state_update_max_x_; + lethal_state_update_max_y_ = std::max(*max_y, lethal_state_update_max_y_); + *max_y = lethal_state_update_max_y_; + } else { + // If out of lethal space, reset managed lethal state sizes + lethal_state_update_min_x_ = std::numeric_limits::max(); + lethal_state_update_min_y_ = std::numeric_limits::max(); + lethal_state_update_max_x_ = std::numeric_limits::lowest(); + lethal_state_update_max_y_ = std::numeric_limits::lowest(); + } + } } void KeepoutFilter::process( nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j, - const geometry_msgs::msg::Pose & pose) + const geometry_msgs::msg::Pose & /*pose*/) { std::lock_guard guard(*getMutex()); @@ -255,49 +334,10 @@ void KeepoutFilter::process( } // unsigned<-signed conversions. - unsigned int mg_min_x_u = static_cast(mg_min_x); - unsigned int mg_min_y_u = static_cast(mg_min_y); - unsigned int mg_max_x_u = static_cast(mg_max_x); - unsigned int mg_max_y_u = static_cast(mg_max_y); - - // Let's find the pose's cost if we are allowed to override the lethal cost - bool is_pose_lethal = false; - if (override_lethal_cost_) { - geometry_msgs::msg::Pose mask_pose; - if (transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) { - unsigned int mask_robot_i, mask_robot_j; - if (worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i, - mask_robot_j)) - { - auto data = getMaskCost(filter_mask_, mask_robot_i, mask_robot_j); - is_pose_lethal = (data == INSCRIBED_INFLATED_OBSTACLE || data == LETHAL_OBSTACLE); - if (is_pose_lethal) { - RCLCPP_WARN_THROTTLE( - logger_, *(clock_), 2000, - "KeepoutFilter: Pose is in keepout zone, reducing cost override to navigate out."); - } - } - } - - // If in lethal space or just exited lethal space, - // we need to update all possible spaces touched during this state - if (is_pose_lethal || (last_pose_lethal_ && !is_pose_lethal)) { - lethal_state_update_min_x_ = std::min(mg_min_x_u, lethal_state_update_min_x_); - mg_min_x_u = lethal_state_update_min_x_; - lethal_state_update_min_y_ = std::min(mg_min_y_u, lethal_state_update_min_y_); - mg_min_y_u = lethal_state_update_min_y_; - lethal_state_update_max_x_ = std::max(mg_max_x_u, lethal_state_update_max_x_); - mg_max_x_u = lethal_state_update_max_x_; - lethal_state_update_max_y_ = std::max(mg_max_y_u, lethal_state_update_max_y_); - mg_max_y_u = lethal_state_update_max_y_; - } else { - // If out of lethal space, reset managed lethal state sizes - lethal_state_update_min_x_ = master_grid.getSizeInCellsX(); - lethal_state_update_min_y_ = master_grid.getSizeInCellsY(); - lethal_state_update_max_x_ = 0u; - lethal_state_update_max_y_ = 0u; - } - } + const unsigned int mg_min_x_u = static_cast(mg_min_x); + const unsigned int mg_min_y_u = static_cast(mg_min_y); + const unsigned int mg_max_x_u = static_cast(mg_max_x); + const unsigned int mg_max_y_u = static_cast(mg_max_y); unsigned int i, j; // master_grid iterators unsigned int index; // corresponding index of master_grid @@ -328,7 +368,7 @@ void KeepoutFilter::process( msk_wy = gl_wy; } // Get mask coordinates corresponding to (i, j) point at filter_mask_ - if (worldToMask(filter_mask_, msk_wx, msk_wy, mx, my)) { + if (nav2_util::worldToMap(filter_mask_, msk_wx, msk_wy, mx, my)) { data = getMaskCost(filter_mask_, mx, my); // Update if mask_ data is valid and greater than existing master_grid's one if (data == NO_INFORMATION) { @@ -336,7 +376,7 @@ void KeepoutFilter::process( } if (data > old_data || old_data == NO_INFORMATION) { - if (override_lethal_cost_ && is_pose_lethal) { + if (override_lethal_cost_ && is_pose_lethal_) { master_array[index] = lethal_override_cost_; } else { master_array[index] = data; @@ -346,7 +386,7 @@ void KeepoutFilter::process( } } - last_pose_lethal_ = is_pose_lethal; + last_pose_lethal_ = is_pose_lethal_; } void KeepoutFilter::resetFilter() diff --git a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp index a66886bc6d0..c1e822cb03a 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp @@ -43,6 +43,7 @@ #include #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" +#include "nav2_util/occ_grid_utils.hpp" namespace nav2_costmap_2d { @@ -198,8 +199,8 @@ void SpeedFilter::process( // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j) unsigned int mask_robot_i, mask_robot_j; - if (!worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i, - mask_robot_j)) + if (!nav2_util::worldToMap(filter_mask_, mask_pose.position.x, mask_pose.position.y, + mask_robot_i, mask_robot_j)) { return; } diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index bac51b92f92..352a58f563d 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -45,7 +45,9 @@ #include "pluginlib/class_list_macros.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" +#include "nav2_util/raytrace_line_2d.hpp" #include "nav2_costmap_2d/costmap_math.hpp" +#include "nav2_ros_common/node_utils.hpp" #include "rclcpp/version.h" PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::ObstacleLayer, nav2_costmap_2d::Layer) @@ -87,6 +89,7 @@ void ObstacleLayer::onInitialize() declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0)); declareParameter("combination_method", rclcpp::ParameterValue(1)); declareParameter("observation_sources", rclcpp::ParameterValue(std::string(""))); + declareParameter("footprint_clearing_padding", rclcpp::ParameterValue(0.0)); auto node = node_.lock(); if (!node) { @@ -95,6 +98,7 @@ void ObstacleLayer::onInitialize() node->get_parameter(name_ + "." + "enabled", enabled_); node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); + node->get_parameter(name_ + "." + "footprint_clearing_padding", footprint_clearing_padding_); node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_); node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_); node->get_parameter("track_unknown_space", track_unknown_space); @@ -141,7 +145,7 @@ void ObstacleLayer::onInitialize() while (ss >> source) { // get the parameters for the specific topic double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height; - std::string topic, sensor_frame, data_type; + std::string topic, sensor_frame, data_type, transport_type; bool inf_is_valid, clearing, marking; declareParameter(source + "." + "topic", rclcpp::ParameterValue(source)); @@ -158,6 +162,7 @@ void ObstacleLayer::onInitialize() declareParameter(source + "." + "obstacle_min_range", rclcpp::ParameterValue(0.0)); declareParameter(source + "." + "raytrace_max_range", rclcpp::ParameterValue(3.0)); declareParameter(source + "." + "raytrace_min_range", rclcpp::ParameterValue(0.0)); + declareParameter(source + "." + "transport_type", rclcpp::ParameterValue(std::string("raw"))); node->get_parameter(name_ + "." + source + "." + "topic", topic); node->get_parameter(name_ + "." + source + "." + "sensor_frame", sensor_frame); @@ -173,6 +178,7 @@ void ObstacleLayer::onInitialize() node->get_parameter(name_ + "." + source + "." + "inf_is_valid", inf_is_valid); node->get_parameter(name_ + "." + source + "." + "marking", marking); node->get_parameter(name_ + "." + source + "." + "clearing", clearing); + node->get_parameter(name_ + "." + source + "." + "transport_type", transport_type); if (!(data_type == "PointCloud2" || data_type == "LaserScan")) { RCLCPP_FATAL( @@ -287,16 +293,23 @@ void ObstacleLayer::onInitialize() tf_filter_tolerance)); } else { + // For Rolling and Newer Support from PointCloudTransport API change + #if RCLCPP_VERSION_GTE(30, 0, 0) + std::shared_ptr sub; // For Kilted and Older Support from Message Filters API change - #if RCLCPP_VERSION_GTE(29, 6, 0) + #elif RCLCPP_VERSION_GTE(29, 6, 0) std::shared_ptr> sub; #else std::shared_ptr> sub; #endif + // For Rolling compatibility in PointCloudTransport API change + #if RCLCPP_VERSION_GTE(30, 0, 0) + sub = std::make_shared( + *node, topic, transport_type, custom_qos_profile, sub_opt); // For Kilted compatibility in Message Filters API change - #if RCLCPP_VERSION_GTE(29, 6, 0) + #elif RCLCPP_VERSION_GTE(29, 6, 0) sub = std::make_shared>( node, topic, custom_qos_profile, sub_opt); // For Jazzy compatibility in Message Filters API change @@ -576,7 +589,14 @@ ObstacleLayer::updateFootprint( double * max_y) { if (!footprint_clearing_enabled_) {return;} - transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); + + // Get the base footprint and apply padding before transformation + std::vector padded_footprint = getFootprint(); + if (footprint_clearing_padding_ > 0.0) { + padFootprint(padded_footprint, footprint_clearing_padding_); + } + // Transform the padded footprint to the robot's current pose + transformFootprint(robot_x, robot_y, robot_yaw, padded_footprint, transformed_footprint_); for (unsigned int i = 0; i < transformed_footprint_.size(); i++) { touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y); @@ -759,7 +779,8 @@ ObstacleLayer::raytraceFreespace( unsigned int cell_raytrace_min_range = cellDistance(clearing_observation.raytrace_min_range_); MarkCell marker(costmap_, FREE_SPACE); // and finally... we can execute our trace to clear obstacles along that line - raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_max_range, cell_raytrace_min_range); + nav2_util::raytraceLine( + marker, x0, y0, x1, y1, size_x_, cell_raytrace_max_range, cell_raytrace_min_range); updateRaytraceBounds( ox, oy, wx, wy, clearing_observation.raytrace_max_range_, diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index e4dbe277723..18a0eadb153 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -47,6 +47,8 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_ros_common/validate_messages.hpp" +#define EPSILON 1e-5 + PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer) using nav2_costmap_2d::NO_INFORMATION; @@ -137,6 +139,7 @@ StaticLayer::getParameters() declareParameter("map_topic", rclcpp::ParameterValue("map")); declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(false)); declareParameter("restore_cleared_footprint", rclcpp::ParameterValue(true)); + declareParameter("footprint_clearing_padding", rclcpp::ParameterValue(0.0)); auto node = node_.lock(); if (!node) { @@ -147,6 +150,7 @@ StaticLayer::getParameters() node->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); node->get_parameter(name_ + "." + "restore_cleared_footprint", restore_cleared_footprint_); + node->get_parameter(name_ + "." + "footprint_clearing_padding", footprint_clearing_padding_); node->get_parameter(name_ + "." + "map_topic", map_topic_); map_topic_ = joinWithParentNamespace(map_topic_); node->get_parameter( @@ -190,9 +194,9 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) Costmap2D * master = layered_costmap_->getCostmap(); if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x || master->getSizeInCellsY() != size_y || - master->getResolution() != new_map.info.resolution || - master->getOriginX() != new_map.info.origin.position.x || - master->getOriginY() != new_map.info.origin.position.y || + !isEqual(master->getResolution(), new_map.info.resolution, EPSILON) || + !isEqual(master->getOriginX(), new_map.info.origin.position.x, EPSILON) || + !isEqual(master->getOriginY(), new_map.info.origin.position.y, EPSILON) || !layered_costmap_->isSizeLocked())) { // Update the size of the layered costmap (and all layers, including this one) @@ -206,9 +210,9 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) new_map.info.origin.position.y, true); } else if (size_x_ != size_x || size_y_ != size_y || // NOLINT - resolution_ != new_map.info.resolution || - origin_x_ != new_map.info.origin.position.x || - origin_y_ != new_map.info.origin.position.y) + !isEqual(resolution_, new_map.info.resolution, EPSILON) || + !isEqual(origin_x_, new_map.info.origin.position.x, EPSILON) || + !isEqual(origin_y_, new_map.info.origin.position.y, EPSILON)) { // only update the size of the costmap stored locally in this layer RCLCPP_INFO( @@ -384,8 +388,13 @@ StaticLayer::updateFootprint( double * max_y) { if (!footprint_clearing_enabled_) {return;} - - transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); + // Get the base footprint and apply padding before transformation + std::vector padded_footprint = getFootprint(); + if (footprint_clearing_padding_ > 0.0) { + padFootprint(padded_footprint, footprint_clearing_padding_); + } + // Transform the padded footprint to the robot's current pose + transformFootprint(robot_x, robot_y, robot_yaw, padded_footprint, transformed_footprint_); for (unsigned int i = 0; i < transformed_footprint_.size(); i++) { touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y); @@ -469,6 +478,18 @@ StaticLayer::updateCosts( current_ = true; } +/** + * @brief Check if two floating point numbers are equal within a given epsilon + * @param a First number + * @param b Second number + * @param epsilon Tolerance for equality check + * @return True if numbers are equal within the tolerance, false otherwise + */ +bool StaticLayer::isEqual(double a, double b, double epsilon) +{ + return std::abs(a - b) < epsilon; +} + /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message @@ -497,6 +518,8 @@ StaticLayer::dynamicParametersCallback( } else if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == name_ + "." + "transform_tolerance") { transform_tolerance_ = tf2::durationFromSec(parameter.as_double()); + } else if (param_name == name_ + "." + "footprint_clearing_padding") { + footprint_clearing_padding_ = parameter.as_double(); } } else if (param_type == ParameterType::PARAMETER_BOOL) { if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) { diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index f3903308cff..28798b0f8a3 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -43,6 +43,7 @@ #include #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/occ_grid_values.hpp" +#include "nav2_util/raytrace_line_2d.hpp" namespace nav2_costmap_2d { @@ -462,14 +463,15 @@ void Costmap2D::polygonOutlineCells( { PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells); for (unsigned int i = 0; i < polygon.size() - 1; ++i) { - raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y); + nav2_util::raytraceLine( + cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y, size_x_); } if (!polygon.empty()) { unsigned int last_index = polygon.size() - 1; // we also need to close the polygon by going from the last point to the first - raytraceLine( + nav2_util::raytraceLine( cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, - polygon[0].y); + polygon[0].y, size_x_); } } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 49dd1885fea..f43373d286b 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -48,7 +48,7 @@ #include "nav2_util/execution_timer.hpp" #include "nav2_ros_common/node_utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_util/robot_utils.hpp" #include "rcl_interfaces/msg/set_parameters_result.hpp" diff --git a/nav2_costmap_2d/src/footprint_collision_checker copy.cpp b/nav2_costmap_2d/src/footprint_collision_checker copy.cpp new file mode 100644 index 00000000000..da234601a05 --- /dev/null +++ b/nav2_costmap_2d/src/footprint_collision_checker copy.cpp @@ -0,0 +1,268 @@ +// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2025 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. +// +// Modified by: Shivang Patel (shivaang14@gmail.com) + +#include +#include +#include +#include +#include + +#include "nav2_costmap_2d/footprint_collision_checker.hpp" + +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_costmap_2d/exceptions.hpp" +#include "nav2_costmap_2d/footprint.hpp" +#include "nav2_util/line_iterator.hpp" + +using namespace std::chrono_literals; + +namespace nav2_costmap_2d +{ + +// Simple 2D point structure +struct Point2D +{ + int x, y; + Point2D(int x_val, int y_val) + : x(x_val), y(y_val) {} +}; + +// Simple rectangle structure +struct Rectangle +{ + int x, y, width, height; + Rectangle(int x_val, int y_val, int w, int h) + : x(x_val), y(y_val), width(w), height(h) {} +}; + +// Calculate bounding rectangle for a set of points +Rectangle calculateBoundingRect(const std::vector & points) +{ + if (points.empty()) { + return Rectangle(0, 0, 0, 0); + } + + int min_x = points[0].x; + int max_x = points[0].x; + int min_y = points[0].y; + int max_y = points[0].y; + + for (const auto & pt : points) { + min_x = std::min(min_x, pt.x); + max_x = std::max(max_x, pt.x); + min_y = std::min(min_y, pt.y); + max_y = std::max(max_y, pt.y); + } + + return Rectangle(min_x, min_y, max_x - min_x + 1, max_y - min_y + 1); +} + +// Point-in-polygon test using ray casting algorithm +bool isPointInPolygon(int x, int y, const std::vector & polygon) +{ + if (polygon.size() < 3) { + return false; + } + + bool inside = false; + size_t j = polygon.size() - 1; + + for (size_t i = 0; i < polygon.size(); i++) { + if (((polygon[i].y > y) != (polygon[j].y > y)) && + (x < + (polygon[j].x - polygon[i].x) * (y - polygon[i].y) / (polygon[j].y - polygon[i].y) + + polygon[i].x)) + { + inside = !inside; + } + j = i; + } + + return inside; +} + +template +FootprintCollisionChecker::FootprintCollisionChecker() +: costmap_(nullptr) +{ +} + +template +FootprintCollisionChecker::FootprintCollisionChecker( + CostmapT costmap) +: costmap_(costmap) +{ +} + +template +double FootprintCollisionChecker::footprintCost( + const Footprint & footprint, + bool check_full_area) +{ + if (footprint.empty()) { + return static_cast(NO_INFORMATION); + } + + // Pre-convert all footprint points to map coordinates once + std::vector polygon_points; + polygon_points.reserve(footprint.size()); + + for (const auto & point : footprint) { + unsigned int mx, my; + if (!worldToMap(point.x, point.y, mx, my)) { + return static_cast(NO_INFORMATION); + } + polygon_points.emplace_back(static_cast(mx), static_cast(my)); + } + + // Check perimeter using pre-converted coordinates + double perimeter_cost = 0.0; + for (size_t i = 0; i < polygon_points.size(); ++i) { + size_t next_i = (i + 1) % polygon_points.size(); + double line_cost = lineCost( + polygon_points[i].x, polygon_points[next_i].x, + polygon_points[i].y, polygon_points[next_i].y); + + perimeter_cost = std::max(perimeter_cost, line_cost); + + // Early termination if lethal obstacle found + if (perimeter_cost == static_cast(LETHAL_OBSTACLE)) { + return perimeter_cost; + } + } + + // If perimeter check found collision or full area check not requested, return perimeter result + if (perimeter_cost == static_cast(LETHAL_OBSTACLE) || !check_full_area) { + return perimeter_cost; + } + + // If no collision on perimeter and full area check requested, rasterize the full area + Rectangle bbox = calculateBoundingRect(polygon_points); + + // Clamp bounding box to costmap dimensions for safety + unsigned int costmap_width = costmap_->getSizeInCellsX(); + unsigned int costmap_height = costmap_->getSizeInCellsY(); + + int min_x = std::max(0, bbox.x); + int min_y = std::max(0, bbox.y); + int max_x = std::min(static_cast(costmap_width - 1), bbox.x + bbox.width - 1); + int max_y = std::min(static_cast(costmap_height - 1), bbox.y + bbox.height - 1); + + // Translate polygon points to bounding box coordinates + std::vector bbox_polygon; + bbox_polygon.reserve(polygon_points.size()); + for (const auto & pt : polygon_points) { + bbox_polygon.emplace_back(pt.x - bbox.x, pt.y - bbox.y); + } + + double max_cost = perimeter_cost; + + // Iterate through the clamped bounding box and check costs only for cells inside the polygon + for (int y = min_y; y <= max_y; ++y) { + for (int x = min_x; x <= max_x; ++x) { + // Convert to bounding box coordinates for polygon test + int bbox_x = x - bbox.x; + int bbox_y = y - bbox.y; + + if (isPointInPolygon(bbox_x, bbox_y, bbox_polygon)) { + double cell_cost = pointCost(x, y); + + // Early termination if lethal obstacle found + if (cell_cost == static_cast(LETHAL_OBSTACLE)) { + return cell_cost; + } + + max_cost = std::max(max_cost, cell_cost); + } + } + } + + return max_cost; +} + +template +double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int y1) const +{ + double line_cost = 0.0; + double point_cost = -1.0; + + for (nav2_util::LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) { + point_cost = pointCost(line.getX(), line.getY()); // Score the current point + + // if in collision, no need to continue + if (point_cost == static_cast(LETHAL_OBSTACLE)) { + return point_cost; + } + + if (line_cost < point_cost) { + line_cost = point_cost; + } + } + + return line_cost; +} + +template +bool FootprintCollisionChecker::worldToMap( + double wx, double wy, unsigned int & mx, unsigned int & my) +{ + return costmap_->worldToMap(wx, wy, mx, my); +} + +template +double FootprintCollisionChecker::pointCost(int x, int y) const +{ + // Bounds checking to prevent segmentation faults + if (x < 0 || y < 0 || + x >= static_cast(costmap_->getSizeInCellsX()) || + y >= static_cast(costmap_->getSizeInCellsY())) + { + return static_cast(LETHAL_OBSTACLE); + } + + return static_cast(costmap_->getCost(x, y)); +} + +template +void FootprintCollisionChecker::setCostmap(CostmapT costmap) +{ + costmap_ = costmap; +} + +template +double FootprintCollisionChecker::footprintCostAtPose( + double x, double y, double theta, const Footprint & footprint, bool check_full_area) +{ + double cos_th = cos(theta); + double sin_th = sin(theta); + Footprint oriented_footprint; + oriented_footprint.reserve(footprint.size()); + geometry_msgs::msg::Point new_pt; + for (unsigned int i = 0; i < footprint.size(); ++i) { + new_pt.x = x + (footprint[i].x * cos_th - footprint[i].y * sin_th); + new_pt.y = y + (footprint[i].x * sin_th + footprint[i].y * cos_th); + oriented_footprint.push_back(new_pt); + } + + return footprintCost(oriented_footprint, check_full_area); +} + +// declare our valid template parameters +template class FootprintCollisionChecker>; +template class FootprintCollisionChecker; + +} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/test/integration/costmap_tester.cpp b/nav2_costmap_2d/test/integration/costmap_tester.cpp index c7d44e6e70c..8569cae54e3 100644 --- a/nav2_costmap_2d/test/integration/costmap_tester.cpp +++ b/nav2_costmap_2d/test/integration/costmap_tester.cpp @@ -38,7 +38,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/cost_values.hpp" diff --git a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp index a543dda02ae..94e0f72ea77 100644 --- a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp +++ b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp @@ -19,7 +19,7 @@ #include "rclcpp/rclcpp.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" class DynParamTestNode { diff --git a/nav2_costmap_2d/test/integration/footprint_tests.cpp b/nav2_costmap_2d/test/integration/footprint_tests.cpp index 2f527b5faac..0bde175ce0a 100644 --- a/nav2_costmap_2d/test/integration/footprint_tests.cpp +++ b/nav2_costmap_2d/test/integration/footprint_tests.cpp @@ -41,7 +41,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" #include "nav2_costmap_2d/footprint.hpp" class FootprintTestNode diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index c0616176647..a365cdc9a7d 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -19,7 +19,7 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_costmap_2d/cost_values.hpp" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" class CostmapRosLifecycleNode : public nav2::LifecycleNode diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index 1543060ccca..e5b73e4b5cc 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -29,10 +29,10 @@ #include "nav2_util/robot_utils.hpp" #include "nav2_ros_common/node_utils.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/transform_broadcaster.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" #include "tf2/utils.hpp" diff --git a/nav2_costmap_2d/test/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt index d468dd37987..435d9072d03 100644 --- a/nav2_costmap_2d/test/regression/CMakeLists.txt +++ b/nav2_costmap_2d/test/regression/CMakeLists.txt @@ -1,9 +1,3 @@ -# Bresenham2D corner cases test -ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp) -target_link_libraries(costmap_bresenham_2d - nav2_costmap_2d_core -) - # OrderLayer for checking Costmap2D plugins API calling order add_library(order_layer SHARED order_layer.cpp) target_link_libraries(order_layer PUBLIC diff --git a/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp b/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp deleted file mode 100644 index c4afee34e00..00000000000 --- a/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp +++ /dev/null @@ -1,159 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2022 Samsung Research Russia -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* Author: Alexey Merzlyakov -*********************************************************************/ -#include -#include - -class CostmapAction -{ -public: - explicit CostmapAction( - unsigned char * costmap, unsigned int size, unsigned char mark_val = 128) - : costmap_(costmap), size_(size), mark_val_(mark_val) - { - } - - inline void operator()(unsigned int off) - { - ASSERT_TRUE(off < size_); - costmap_[off] = mark_val_; - } - - inline unsigned int get(unsigned int off) - { - return costmap_[off]; - } - -private: - unsigned char * costmap_; - unsigned int size_; - unsigned char mark_val_; -}; - -class CostmapTest : public nav2_costmap_2d::Costmap2D -{ -public: - CostmapTest( - unsigned int size_x, unsigned int size_y, double resolution, - double origin_x, double origin_y, unsigned char default_val = 0) - : nav2_costmap_2d::Costmap2D(size_x, size_y, resolution, origin_x, origin_y, default_val) - { - } - - unsigned char * getCostmap() - { - return costmap_; - } - - unsigned int getSize() - { - return size_x_ * size_y_; - } - - void raytraceLine( - CostmapAction ca, unsigned int x0, unsigned int y0, unsigned int x1, - unsigned int y1, - unsigned int max_length = UINT_MAX, unsigned int min_length = 0) - { - nav2_costmap_2d::Costmap2D::raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } -}; - -TEST(costmap_2d, bresenham2DBoundariesCheck) -{ - const unsigned int sz_x = 60; - const unsigned int sz_y = 60; - const unsigned int max_length = 60; - const unsigned int min_length = 6; - CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0); - CostmapAction ca(ct.getCostmap(), ct.getSize()); - - // Initial point - some asymmetrically standing point in order to cover most corner cases - const unsigned int x0 = 2; - const unsigned int y0 = 4; - // (x1, y1) point will move - unsigned int x1, y1; - - // Running on (x, 0) edge - y1 = 0; - for (x1 = 0; x1 < sz_x; x1++) { - ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } - - // Running on (x, sz_y) edge - y1 = sz_y - 1; - for (x1 = 0; x1 < sz_x; x1++) { - ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } - - // Running on (0, y) edge - x1 = 0; - for (y1 = 0; y1 < sz_y; y1++) { - ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } - - // Running on (sz_x, y) edge - x1 = sz_x - 1; - for (y1 = 0; y1 < sz_y; y1++) { - ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } -} - -TEST(costmap_2d, bresenham2DSamePoint) -{ - const unsigned int sz_x = 60; - const unsigned int sz_y = 60; - const unsigned int max_length = 60; - const unsigned int min_length = 0; - CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0); - CostmapAction ca(ct.getCostmap(), ct.getSize()); - - // Initial point - const double x0 = 2; - const double y0 = 4; - - unsigned int offset = y0 * sz_x + x0; - unsigned char val_before = ca.get(offset); - // Same point to check - ct.raytraceLine(ca, x0, y0, x0, y0, max_length, min_length); - unsigned char val_after = ca.get(offset); - ASSERT_FALSE(val_before == val_after); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/nav2_costmap_2d/test/unit/binary_filter_test.cpp b/nav2_costmap_2d/test/unit/binary_filter_test.cpp index befb8d00e1e..e0061e91e88 100644 --- a/nav2_costmap_2d/test/unit/binary_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/binary_filter_test.cpp @@ -24,9 +24,9 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/occ_grid_values.hpp" #include "nav2_costmap_2d/cost_values.hpp" diff --git a/nav2_costmap_2d/test/unit/costmap_filter_test.cpp b/nav2_costmap_2d/test/unit/costmap_filter_test.cpp index 320eeaa1653..8cf4583d15d 100644 --- a/nav2_costmap_2d/test/unit/costmap_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/costmap_filter_test.cpp @@ -19,6 +19,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_util/occ_grid_values.hpp" +#include "nav2_util/occ_grid_utils.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "geometry_msgs/msg/pose.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" @@ -30,13 +31,6 @@ class CostmapFilterWrapper : public nav2_costmap_2d::CostmapFilter public: CostmapFilterWrapper() {} - bool worldToMask( - nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, - double wx, double wy, unsigned int & mx, unsigned int & my) const - { - return nav2_costmap_2d::CostmapFilter::worldToMask(filter_mask, wx, wy, mx, my); - } - unsigned char getMaskCost( nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, const unsigned int mx, const unsigned int & my) const @@ -52,7 +46,7 @@ class CostmapFilterWrapper : public nav2_costmap_2d::CostmapFilter void resetFilter() {} }; -TEST(CostmapFilter, testWorldToMask) +TEST(CostmapFilter, testWorldToMap) { // Create occupancy grid for test as follows: // @@ -82,19 +76,19 @@ TEST(CostmapFilter, testWorldToMask) CostmapFilterWrapper cf; unsigned int mx, my; // Point inside mask - ASSERT_TRUE(cf.worldToMask(mask, 4.0, 5.0, mx, my)); + ASSERT_TRUE(nav2_util::worldToMap(mask, 4.0, 5.0, mx, my)); ASSERT_EQ(mx, 1u); ASSERT_EQ(my, 2u); // Corner cases - ASSERT_TRUE(cf.worldToMask(mask, 3.0, 3.0, mx, my)); + ASSERT_TRUE(nav2_util::worldToMap(mask, 3.0, 3.0, mx, my)); ASSERT_EQ(mx, 0u); ASSERT_EQ(my, 0u); - ASSERT_TRUE(cf.worldToMask(mask, 5.9, 5.9, mx, my)); + ASSERT_TRUE(nav2_util::worldToMap(mask, 5.9, 5.9, mx, my)); ASSERT_EQ(mx, 2u); ASSERT_EQ(my, 2u); // Point outside mask - ASSERT_FALSE(cf.worldToMask(mask, 2.9, 2.9, mx, my)); - ASSERT_FALSE(cf.worldToMask(mask, 6.0, 6.0, mx, my)); + ASSERT_FALSE(nav2_util::worldToMap(mask, 2.9, 2.9, mx, my)); + ASSERT_FALSE(nav2_util::worldToMap(mask, 6.0, 6.0, mx, my)); } TEST(CostmapFilter, testGetMaskCost) diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test copy.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test copy.cpp new file mode 100644 index 00000000000..c11fbb14d52 --- /dev/null +++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test copy.cpp @@ -0,0 +1,793 @@ +// Copyright (c) 2020 Shivang Patel +// +// 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 +#include + +#include "gtest/gtest.h" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_costmap_2d/footprint.hpp" +#include "nav2_costmap_2d/cost_values.hpp" + +// Test basic footprint collision checking with no obstacles +// Verifies that a diamond-shaped footprint placed on an empty costmap returns zero cost +TEST(collision_footprint, test_basic) +{ + // Create a 100x100 costmap with 0.1m resolution, all cells initialized to cost 0 + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + // Create a diamond-shaped footprint + geometry_msgs::msg::Point p1; + p1.x = -0.5; // Left vertex + p1.y = 0.0; + geometry_msgs::msg::Point p2; + p2.x = 0.0; // Top vertex + p2.y = 0.5; + geometry_msgs::msg::Point p3; + p3.x = 0.5; // Right vertex + p3.y = 0.0; + geometry_msgs::msg::Point p4; + p4.x = 0.0; // Bottom vertex + p4.y = -0.5; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Check footprint cost at position (5.0, 5.0) with no rotation + // Should return 0 since no obstacles are present + auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); + + EXPECT_NEAR(value, 0.0, 0.001); +} + +// Test point cost functionality +// Verifies that the pointCost method correctly returns the cost value at a specific grid cell +TEST(collision_footprint, test_point_cost) +{ + // Create empty costmap with all cells at cost 0 + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Test point cost at grid cell (50, 50) - should be 0 for empty costmap + auto value = collision_checker.pointCost(50, 50); + + EXPECT_NEAR(value, 0.0, 0.001); +} + +// Test world-to-map coordinate conversion and point cost retrieval +// Verifies that world coordinates are correctly converted to map coordinates +// and that costs can be retrieved and set at those locations +TEST(collision_footprint, test_world_to_map) +{ + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + unsigned int x, y; + + // Convert world coordinates (1.0, 1.0) to map coordinates + // With 0.1m resolution and origin at (0,0), this should map to grid cell (10, 10) + collision_checker.worldToMap(1.0, 1.0, x, y); + + // Check that the cost at the converted coordinates is 0 (empty space) + auto value = collision_checker.pointCost(x, y); + EXPECT_NEAR(value, 0.0, 0.001); + + // Set a cost of 200 at grid cell (50, 50) and verify it can be retrieved + costmap_->setCost(50, 50, 200); + collision_checker.worldToMap(5.0, 5.0, x, y); // World (5.0, 5.0) -> Grid (50, 50) + + EXPECT_NEAR(collision_checker.pointCost(x, y), 200.0, 0.001); +} + +// Test footprint collision detection when robot moves into obstacle areas +// Creates a costmap with obstacles everywhere except a safe zone, then tests +// footprint placement at different positions to verify collision detection +TEST(collision_footprint, test_footprint_at_pose_with_movement) +{ + // Create costmap with all cells set to cost 254 (high obstacle cost) + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 254); + + // Create a safe zone (cost 0) in the center of the map from grid (40,40) to (60,60) + // This corresponds to world coordinates roughly (4.0,4.0) to (6.0,6.0) + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap_->setCost(i, j, 0); + } + } + + // Create a 2x2 meter square footprint + geometry_msgs::msg::Point p1; + p1.x = -1.0; // Bottom-left + p1.y = 1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; // Bottom-right + p2.y = 1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; // Top-right + p3.y = -1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; // Top-left + p4.y = -1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Test footprint at center of safe zone - should have zero cost + auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); + EXPECT_NEAR(value, 0.0, 0.001); + + // Test footprint moved slightly up - should hit obstacle area (cost 254) + auto up_value = collision_checker.footprintCostAtPose(5.0, 4.9, 0.0, footprint); + EXPECT_NEAR(up_value, 254.0, 0.001); + + // Test footprint moved slightly down - should hit obstacle area (cost 254) + auto down_value = collision_checker.footprintCostAtPose(5.0, 5.2, 0.0, footprint); + EXPECT_NEAR(down_value, 254.0, 0.001); +} + +// Test point and line cost detection with strategically placed obstacles +// Places obstacles at specific locations and verifies they are detected when the +// footprint perimeter intersects them during different robot movements +TEST(collision_footprint, test_point_and_line_cost) +{ + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.10000, 0, 0.0, 0.0); + + // Place obstacles at strategic locations: + costmap_->setCost(62, 50, 254); // Obstacle to the right of center + costmap_->setCost(39, 60, 254); // Obstacle to the left and up from center + + // Create a 2x2 meter square footprint + geometry_msgs::msg::Point p1; + p1.x = -1.0; + p1.y = 1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = 1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; + p3.y = -1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = -1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Test footprint at center position - should not hit obstacles + auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); + EXPECT_NEAR(value, 0.0, 0.001); + + // Move footprint left - should detect obstacle at (39, 60) + auto left_value = collision_checker.footprintCostAtPose(4.9, 5.0, 0.0, footprint); + EXPECT_NEAR(left_value, 254.0, 0.001); + + // Move footprint right - should detect obstacle at (62, 50) + auto right_value = collision_checker.footprintCostAtPose(5.2, 5.0, 0.0, footprint); + EXPECT_NEAR(right_value, 254.0, 0.001); +} + +// Test utility function for footprints with insufficient points +// Verifies that distance calculation functions handle edge cases correctly +// when footprint has fewer than 3 points (minimum for a valid polygon) +TEST(collision_footprint, not_enough_points) +{ + geometry_msgs::msg::Point p1; + p1.x = 2.0; + p1.y = 2.0; + + geometry_msgs::msg::Point p2; + p2.x = -2.0; + p2.y = -2.0; + + // Create footprint with only 2 points (insufficient for polygon) + std::vector footprint = {p1, p2}; + double min_dist = 0.0; + double max_dist = 0.0; + + // calculateMinAndMaxDistances should handle this gracefully + std::tie(min_dist, max_dist) = nav2_costmap_2d::calculateMinAndMaxDistances(footprint); + EXPECT_EQ(min_dist, std::numeric_limits::max()); + EXPECT_EQ(max_dist, 0.0f); +} + +// Test conversion from Point to Point32 message types +// Verifies that geometry message type conversions preserve coordinate values +TEST(collision_footprint, to_point_32) { + geometry_msgs::msg::Point p; + p.x = 123.0; + p.y = 456.0; + p.z = 789.0; + + // Convert to Point32 format + geometry_msgs::msg::Point32 p32; + p32 = nav2_costmap_2d::toPoint32(p); + + // Verify all coordinates are preserved + EXPECT_NEAR(p.x, p32.x, 1e-5); + EXPECT_NEAR(p.y, p32.y, 1e-5); + EXPECT_NEAR(p.z, p32.z, 1e-5); +} + +// Test conversion from vector of Points to Polygon message type +// Verifies that footprint data can be converted to ROS message format +TEST(collision_footprint, to_polygon) { + geometry_msgs::msg::Point p1; + p1.x = 1.2; + p1.y = 3.4; + p1.z = 5.1; + + geometry_msgs::msg::Point p2; + p2.x = -5.6; + p2.y = -7.8; + p2.z = -9.1; + std::vector pts = {p1, p2}; + + // Convert to Polygon message format + geometry_msgs::msg::Polygon poly; + poly = nav2_costmap_2d::toPolygon(pts); + + // Verify structure and coordinate preservation + EXPECT_EQ(2u, sizeof(poly.points) / sizeof(poly.points[0])); + EXPECT_NEAR(poly.points[0].x, p1.x, 1e-5); + EXPECT_NEAR(poly.points[0].y, p1.y, 1e-5); + EXPECT_NEAR(poly.points[0].z, p1.z, 1e-5); + EXPECT_NEAR(poly.points[1].x, p2.x, 1e-5); + EXPECT_NEAR(poly.points[1].y, p2.y, 1e-5); + EXPECT_NEAR(poly.points[1].z, p2.z, 1e-5); +} + +// Test successful parsing of footprint from string representation +// Verifies that string-based footprint configuration works correctly +TEST(collision_footprint, make_footprint_from_string) { + std::vector footprint; + + // Parse a valid footprint string with scientific notation + bool result = nav2_costmap_2d::makeFootprintFromString( + "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2]]", footprint); + + EXPECT_EQ(result, true); + EXPECT_EQ(4u, footprint.size()); + + // Verify all coordinate values are parsed correctly + EXPECT_NEAR(footprint[0].x, 1.0, 1e-5); + EXPECT_NEAR(footprint[0].y, 2.2, 1e-5); + EXPECT_NEAR(footprint[1].x, 0.3, 1e-5); + EXPECT_NEAR(footprint[1].y, -4e4, 1e-5); + EXPECT_NEAR(footprint[2].x, -0.3, 1e-5); + EXPECT_NEAR(footprint[2].y, -4e4, 1e-5); + EXPECT_NEAR(footprint[3].x, -1.0, 1e-5); + EXPECT_NEAR(footprint[3].y, 2.2, 1e-5); +} + +// Test error handling for malformed footprint strings +// Verifies that parsing fails gracefully with invalid input +TEST(collision_footprint, make_footprint_from_string_parse_error) { + std::vector footprint; + + // Try to parse a malformed string (missing closing bracket) + bool result = nav2_costmap_2d::makeFootprintFromString( + "[[bad_string", footprint); + EXPECT_EQ(result, false); +} + +// Test error handling for footprints with too few points +// Verifies that polygons with insufficient vertices are rejected +TEST(collision_footprint, make_footprint_from_string_two_points_error) { + std::vector footprint; + + // Try to parse footprint with only 2 points (need at least 3 for polygon) + bool result = nav2_costmap_2d::makeFootprintFromString( + "[[1, 2.2], [.3, -4e4]", footprint); + EXPECT_EQ(result, false); +} + +// Test error handling for points with wrong number of coordinates +// Verifies that points with extra coordinates are rejected +TEST(collision_footprint, make_footprint_from_string_not_pairs) { + std::vector footprint; + + // Try to parse footprint with a point having 3 coordinates (should be 2) + bool result = nav2_costmap_2d::makeFootprintFromString( + "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2, 5.6]]", footprint); + EXPECT_EQ(result, false); +} + +// Test full area checking functionality when no obstacles are present +// Verifies that both perimeter-only and full-area checking return the same result +// when the footprint area is completely free of obstacles +TEST(collision_footprint, test_full_area_check_no_collision) { + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + // Create a 2x2 meter rectangular footprint + geometry_msgs::msg::Point p1; + p1.x = -1.0; // Bottom-left + p1.y = -1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; // Bottom-right + p2.y = -1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; // Top-right + p3.y = 1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; // Top-left + p4.y = 1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Test perimeter only check at a valid position (5.0, 5.0) within the 10x10m costmap + auto perimeter_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, false); + EXPECT_NEAR(perimeter_cost, 0.0, 0.001); + + // Test full area check at the same position + auto full_area_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, true); + EXPECT_NEAR(full_area_cost, 0.0, 0.001); + + // Both should be the same when no obstacles present + EXPECT_NEAR(perimeter_cost, full_area_cost, 0.001); +} + +// Test that full area checking detects obstacles inside the footprint +// Obstacles that are inside the footprint but not on the perimeter +// should only be detected by full area checking +TEST(collision_footprint, test_full_area_check_interior_obstacle) { + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + // Create a 2x2 meter rectangular footprint that will be centered at (5.0, 5.0) + // This maps to grid coordinates around (50, 50) with the 0.1m resolution + geometry_msgs::msg::Point p1; + p1.x = -1.0; + p1.y = -1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = -1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; + p3.y = 1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = 1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + // Place an obstacle in the interior of the footprint (not on perimeter) + // Grid cell (50, 50) corresponds to world coordinates (5.0, 5.0) - center of footprint + costmap_->setCost(50, 50, 200); + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Test perimeter only check - should NOT detect interior obstacle + auto perimeter_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, false); + EXPECT_NEAR(perimeter_cost, 0.0, 0.001); + + // Test full area check - SHOULD detect interior obstacle + auto full_area_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, true); + EXPECT_NEAR(full_area_cost, 200.0, 0.001); +} + +// Test full area checking with lethal obstacles for early termination +// Verifies that when a lethal obstacle is found during full area checking, +// the algorithm returns immediately with lethal cost (performance optimization) +TEST(collision_footprint, test_full_area_check_lethal_obstacle) { + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + // Create a triangular footprint (simpler shape for testing) + geometry_msgs::msg::Point p1; + p1.x = 0.0; // Top vertex + p1.y = 1.0; + geometry_msgs::msg::Point p2; + p2.x = -1.0; // Bottom-left vertex + p2.y = -1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; // Bottom-right vertex + p3.y = -1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3}; + + // Place a lethal obstacle in the interior of the triangle + // Grid cell (50, 49) should be inside the triangle when centered at (5.0, 5.0) + costmap_->setCost(50, 49, nav2_costmap_2d::LETHAL_OBSTACLE); + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Test full area check with lethal obstacle - should return lethal cost immediately + auto full_area_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, true); + EXPECT_NEAR(full_area_cost, static_cast(nav2_costmap_2d::LETHAL_OBSTACLE), 0.001); +} + +// Test full area checking with rotated footprints +// Verifies that the full area checking works correctly when the footprint +// is rotated, ensuring the OpenCV polygon rasterization handles rotation properly +TEST(collision_footprint, test_full_area_check_rotated_footprint) { + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + // Create a rectangular footprint that's taller than it is wide + // This will test rotation effects more clearly + geometry_msgs::msg::Point p1; + p1.x = -0.5; // Left edge + p1.y = -1.5; // Bottom edge (tall rectangle) + geometry_msgs::msg::Point p2; + p2.x = 0.5; // Right edge + p2.y = -1.5; + geometry_msgs::msg::Point p3; + p3.x = 0.5; + p3.y = 1.5; // Top edge + geometry_msgs::msg::Point p4; + p4.x = -0.5; + p4.y = 1.5; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + // Place obstacle where it would be inside the footprint when rotated 90 degrees + // Grid cell (52, 50) should be inside the rotated footprint + costmap_->setCost(52, 50, 150); + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Test with 90-degree rotation (M_PI/2 radians) + // The tall rectangle becomes a wide rectangle, covering the obstacle + auto rotated_cost = collision_checker.footprintCostAtPose(5.0, 5.0, M_PI / 2, footprint, true); + EXPECT_GT(rotated_cost, 0.0); // Should detect the obstacle +} + +// Test the lineCost function directly +// Verifies that line cost calculation works correctly for line segments, +// including early termination when lethal obstacles are encountered +TEST(collision_footprint, test_line_cost_function) { + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Test line cost with no obstacles - should return 0 + auto line_cost = collision_checker.lineCost(10, 20, 10, 20); + EXPECT_NEAR(line_cost, 0.0, 0.001); + + // Add obstacle in the line path from (10,10) to (20,20) + costmap_->setCost(15, 15, 100); + line_cost = collision_checker.lineCost(10, 20, 10, 20); + EXPECT_NEAR(line_cost, 100.0, 0.001); + + // Test with lethal obstacle - should return immediately without checking rest of line + costmap_->setCost(12, 12, nav2_costmap_2d::LETHAL_OBSTACLE); + line_cost = collision_checker.lineCost(10, 20, 10, 20); + EXPECT_NEAR(line_cost, static_cast(nav2_costmap_2d::LETHAL_OBSTACLE), 0.001); +} + +// Test handling of footprints that extend outside the costmap boundaries +// Verifies that out-of-bounds footprint coordinates are handled correctly +// and return lethal cost (indicating collision/invalid placement) +TEST(collision_footprint, test_out_of_bounds_footprint) { + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + // Create a large footprint that will extend beyond map boundaries + geometry_msgs::msg::Point p1; + p1.x = -2.0; // Will extend to negative world coordinates + p1.y = -2.0; + geometry_msgs::msg::Point p2; + p2.x = 2.0; + p2.y = -2.0; + geometry_msgs::msg::Point p3; + p3.x = 2.0; + p3.y = 2.0; + geometry_msgs::msg::Point p4; + p4.x = -2.0; + p4.y = 2.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Place footprint so part of it extends outside the map (negative world coordinates) + // With costmap starting at origin (0,0), placing at (-1.0, -1.0) will put parts out of bounds + auto out_of_bounds_cost = collision_checker.footprintCostAtPose(-1.0, -1.0, 0.0, footprint, true); + EXPECT_EQ(out_of_bounds_cost, static_cast(nav2_costmap_2d::LETHAL_OBSTACLE)); +} + +// Test the setCostmap and getCostmap functionality +// Verifies that the collision checker can switch between different costmaps +// and that cost queries are performed on the correct costmap +TEST(collision_footprint, test_set_costmap_function) { + // Create two different costmaps with different sizes + std::shared_ptr costmap1 = + std::make_shared(50, 50, 0.1, 0, 0, 0); + std::shared_ptr costmap2 = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + // Set different costs at the same grid location in both costmaps + costmap1->setCost(25, 25, 100); + costmap2->setCost(25, 25, 200); + + // Initialize collision checker with first costmap + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap1); + + // Test with first costmap - should return cost 100 + auto cost1 = collision_checker.pointCost(25, 25); + EXPECT_NEAR(cost1, 100.0, 0.001); + + // Switch to second costmap using setCostmap + collision_checker.setCostmap(costmap2); + auto cost2 = collision_checker.pointCost(25, 25); + EXPECT_NEAR(cost2, 200.0, 0.001); + + // Verify getCostmap function returns the correct costmap reference + auto retrieved_costmap = collision_checker.getCostmap(); + EXPECT_EQ(retrieved_costmap, costmap2); +} + +// Test full area checking with complex polygons (non-rectangular shapes) +// Verifies that the OpenCV polygon rasterization works correctly for +// more complex shapes like pentagons, ensuring the implementation is robust +TEST(collision_footprint, test_complex_polygon_full_area) { + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + // Create a complex polygon (pentagon) to test non-trivial polygon rasterization + geometry_msgs::msg::Point p1; + p1.x = 0.0; // Top vertex + p1.y = 1.5; + geometry_msgs::msg::Point p2; + p2.x = 1.4; // Top-right vertex + p2.y = 0.5; + geometry_msgs::msg::Point p3; + p3.x = 0.9; // Bottom-right vertex + p3.y = -1.2; + geometry_msgs::msg::Point p4; + p4.x = -0.9; // Bottom-left vertex + p4.y = -1.2; + geometry_msgs::msg::Point p5; + p5.x = -1.4; // Top-left vertex + p5.y = 0.5; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4, p5}; + + // Place obstacle in what should be the interior of the pentagon + costmap_->setCost(50, 50, 180); + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Test that full area check detects interior obstacle in complex polygon + auto perimeter_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, false); + auto full_area_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, true); + + // Perimeter should not detect interior obstacle + EXPECT_LT(perimeter_cost, 180.0); + // Full area should detect it (demonstrating the value of full area checking) + EXPECT_NEAR(full_area_cost, 180.0, 0.001); +} + +// Performance test for footprint collision checking +// Measures execution time for both perimeter and full-area checking methods +// Tests deterministic scenarios: no obstacles, vertex obstacle, edge obstacle, interior obstacle +TEST(collision_footprint, test_performance_benchmark) { + // Create a reasonably large costmap for performance testing + std::shared_ptr costmap_ = + std::make_shared(500, 500, 0.05, 0, 0, 0); + + // Create a rectangular footprint (2m x 1m) + geometry_msgs::msg::Point p1; + p1.x = -1.0; // Left edge + p1.y = -0.5; // Bottom edge + geometry_msgs::msg::Point p2; + p2.x = 1.0; // Right edge + p2.y = -0.5; + geometry_msgs::msg::Point p3; + p3.x = 1.0; + p3.y = 0.5; // Top edge + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = 0.5; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Fixed test position (center of costmap) + const double test_x = 12.5; // Center of 25m x 25m map + const double test_y = 12.5; + const double test_theta = 0.0; + + // Convert footprint vertices to grid coordinates for obstacle placement + unsigned int grid_x, grid_y; + + const int num_iterations = 1000; + + // Define test scenarios + struct TestScenario + { + std::string name; + std::vector> obstacle_positions; + }; + + std::vector scenarios; + + // Scenario 0: Obstacle at a vertex of the polygon (fastest) + // Place obstacle at the first vertex (p1: left edge, bottom edge) + collision_checker.worldToMap(test_x - 1.0, test_y - 0.5, grid_x, grid_y); + scenarios.push_back({"Obstacle at vertex", {{grid_x, grid_y}}}); + + // Scenario 1: Obstacle on a line segment (edge) + // Place obstacle on the last line segment (p4 to p1: top edge, from right to left) + collision_checker.worldToMap(test_x - 0.5, test_y + 0.5, grid_x, grid_y); + scenarios.push_back({"Obstacle on edge", {{grid_x, grid_y}}}); + + // Scenario 2: Obstacle inside the polygon + // Place obstacle at the center of the rectangle + collision_checker.worldToMap(test_x, test_y, grid_x, grid_y); + scenarios.push_back({"Obstacle inside", {{grid_x, grid_y}}}); + + // Scenario 3: No obstacles (slowest) + scenarios.push_back({"No obstacles", {}}); + + std::cout << "\n=== FOOTPRINT COLLISION CHECKER PERFORMANCE BENCHMARK ===" << std::endl; + std::cout << "Test configuration:" << std::endl; + std::cout << " - Costmap size: 500x500 cells (25m x 25m @ 0.05m resolution)" << std::endl; + std::cout << " - Footprint: Rectangle 2m x 1m" << std::endl; + std::cout << " - Test position: (" << test_x << ", " << test_y << ", " << test_theta << ")" << + std::endl; + std::cout << " - Iterations per scenario: " << num_iterations << std::endl; + std::cout << " - Obstacle cost: 254" << std::endl; + + // Run tests for each scenario + std::vector full_area_times; // Store timing for early exit verification + + for (const auto & scenario : scenarios) { + // Clear costmap + for (unsigned int i = 0; i < costmap_->getSizeInCellsX(); ++i) { + for (unsigned int j = 0; j < costmap_->getSizeInCellsY(); ++j) { + costmap_->setCost(i, j, 0); + } + } + + // Place obstacles for this scenario + for (const auto & pos : scenario.obstacle_positions) { + costmap_->setCost(pos.first, pos.second, 254); + } + + // Test perimeter-only checking + auto start_perimeter = std::chrono::high_resolution_clock::now(); + double perimeter_result = 0.0; + for (int iter = 0; iter < num_iterations; ++iter) { + perimeter_result = collision_checker.footprintCostAtPose( + test_x, test_y, test_theta, footprint, false); + } + auto end_perimeter = std::chrono::high_resolution_clock::now(); + auto perimeter_duration = std::chrono::duration_cast( + end_perimeter - start_perimeter); + + // Test full-area checking + auto start_full_area = std::chrono::high_resolution_clock::now(); + double full_area_result = 0.0; + for (int iter = 0; iter < num_iterations; ++iter) { + full_area_result = collision_checker.footprintCostAtPose( + test_x, test_y, test_theta, footprint, true); + } + auto end_full_area = std::chrono::high_resolution_clock::now(); + auto full_area_duration = std::chrono::duration_cast( + end_full_area - start_full_area); + + // Calculate averages + double avg_perimeter_us = static_cast(perimeter_duration.count()) / num_iterations; + double avg_full_area_us = static_cast(full_area_duration.count()) / num_iterations; + + // Store full-area timing for early exit verification + full_area_times.push_back(avg_full_area_us); + + // Print results for this scenario + std::cout << "\nScenario: " << scenario.name << std::endl; + std::cout << " Perimeter checking:" << std::endl; + std::cout << " - Result: " << perimeter_result << std::endl; + std::cout << " - Average time: " << std::fixed << std::setprecision(2) + << avg_perimeter_us << " microseconds" << std::endl; + std::cout << " Full-area checking:" << std::endl; + std::cout << " - Result: " << full_area_result << std::endl; + std::cout << " - Average time: " << std::fixed << std::setprecision(2) + << avg_full_area_us << " microseconds" << std::endl; + std::cout << " Performance ratio (full-area / perimeter): " + << std::fixed << std::setprecision(2) + << (avg_perimeter_us > + 0 ? avg_full_area_us / avg_perimeter_us : 0.0) << "x" << std::endl; + + // Verify expected behavior for each scenario + if (scenario.name == "No obstacles") { + EXPECT_NEAR(perimeter_result, 0.0, 0.001); + EXPECT_NEAR(full_area_result, 0.0, 0.001); + } else if (scenario.name == "Obstacle at vertex" || scenario.name == "Obstacle on edge") { + // Both methods should detect obstacles on perimeter + EXPECT_NEAR(perimeter_result, 254.0, 0.001); + EXPECT_NEAR(full_area_result, 254.0, 0.001); + } else if (scenario.name == "Obstacle inside") { + // Only full-area should detect interior obstacles + EXPECT_LT(perimeter_result, 254.0); // Should not detect interior obstacle + EXPECT_NEAR(full_area_result, 254.0, 0.001); // Should detect interior obstacle + } + + // Performance sanity checks + EXPECT_LT(avg_perimeter_us, 1000.0); // Should be less than 1ms per call + EXPECT_LT(avg_full_area_us, 5000.0); // Should be less than 5ms per call + } + + // Verify early exit performance ordering for full-area checking + // Expected order (fastest to slowest due to early exit): + // 0: "Obstacle at vertex" - exits earliest when scanning perimeter (fastest) + // 1: "Obstacle on edge" - exits when scanning perimeter + // 2: "Obstacle inside" - exits when scanning interior + // 3: "No obstacles" - must scan entire area (slowest) + std::cout << "\nEarly exit performance verification (full-area checking):" << std::endl; + std::cout << " Obstacle at vertex: " << std::fixed << std::setprecision(2) + << full_area_times[0] << " μs (fastest)" << std::endl; + std::cout << " Obstacle on edge: " << std::fixed << std::setprecision(2) + << full_area_times[1] << " μs" << std::endl; + std::cout << " Obstacle inside: " << std::fixed << std::setprecision(2) + << full_area_times[2] << " μs" << std::endl; + std::cout << " No obstacles: " << std::fixed << std::setprecision(2) + << full_area_times[3] << " μs (slowest)" << std::endl; + + // Verify early exit behavior: obstacles found earlier should result in faster execution + // Expected order: vertex < edge < inside < no obstacles + // Allow some tolerance for measurement noise, but expect clear ordering + double tolerance_factor = 1.2; // Allow 20% tolerance for timing variations + + // Obstacle at vertex should be fastest (first vertex checked) + EXPECT_LT(full_area_times[0] * tolerance_factor, full_area_times[1]) // vertex < edge + << "Obstacle at vertex (" << full_area_times[0] << + " μs) should be faster than obstacle on edge (" + << full_area_times[1] << " μs) due to early exit"; + + EXPECT_LT(full_area_times[1] * tolerance_factor, full_area_times[2]) // edge < inside + << "Obstacle on edge (" << full_area_times[1] << " μs) should be faster than obstacle inside (" + << full_area_times[2] << " μs) due to early exit"; + + EXPECT_LT(full_area_times[2] * tolerance_factor, full_area_times[3]) // inside < no obstacles + << "Obstacle inside (" << full_area_times[2] << " μs) should be faster than no obstacles (" + << full_area_times[3] << " μs) due to early exit"; + + std::cout << " ✓ Early exit performance ordering verified" << std::endl; + std::cout << "============================================================\n" << std::endl; +} diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp index c75acf2b7dc..37a5a683299 100644 --- a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp @@ -22,9 +22,9 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_util/occ_grid_values.hpp" #include "nav2_costmap_2d/cost_values.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp index 76e96187924..5d3eebb6db5 100644 --- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp @@ -24,9 +24,9 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_util/occ_grid_values.hpp" #include "nav2_costmap_2d/cost_values.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt index 83680e3eeae..8801b18d5a8 100644 --- a/nav2_docking/opennav_docking/CMakeLists.txt +++ b/nav2_docking/opennav_docking/CMakeLists.txt @@ -1,8 +1,9 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(opennav_docking) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_graceful_controller REQUIRED) diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp index 06c6a7252e0..4538b2e447d 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp @@ -27,14 +27,14 @@ #include "nav2_ros_common/node_utils.hpp" #include "nav2_ros_common/simple_action_server.hpp" #include "nav2_util/twist_publisher.hpp" -#include "nav_2d_utils/odom_subscriber.hpp" +#include "nav2_util/odometry_utils.hpp" #include "opennav_docking/controller.hpp" #include "opennav_docking/utils.hpp" #include "opennav_docking/types.hpp" #include "opennav_docking/dock_database.hpp" #include "opennav_docking/navigator.hpp" #include "opennav_docking_core/charging_dock.hpp" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" namespace opennav_docking { @@ -264,7 +264,7 @@ class DockingServer : public nav2::LifecycleNode rclcpp::Time action_start_time_; std::unique_ptr vel_publisher_; - std::unique_ptr odom_sub_; + std::unique_ptr odom_sub_; typename DockingActionServer::SharedPtr docking_action_server_; typename UndockingActionServer::SharedPtr undocking_action_server_; diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml index ecfa8082379..b65cb91ee56 100644 --- a/nav2_docking/opennav_docking/package.xml +++ b/nav2_docking/opennav_docking/package.xml @@ -11,6 +11,7 @@ nav2_common angles + backward_ros geometry_msgs nav2_graceful_controller nav2_msgs diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index 9048c70a29a..7838158f8af 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/nav2_docking/opennav_docking/src/controller.cpp @@ -19,7 +19,6 @@ #include "opennav_docking/controller.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_ros_common/node_utils.hpp" -#include "nav_2d_utils/conversions.hpp" #include "tf2/utils.hpp" namespace opennav_docking diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 3584af4e9d3..de552804640 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -42,6 +42,7 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options) declare_parameter("dock_backwards", rclcpp::PARAMETER_BOOL); declare_parameter("dock_prestaging_tolerance", 0.5); declare_parameter("odom_topic", "odom"); + declare_parameter("odom_duration", 0.3); declare_parameter("rotation_angular_tolerance", 0.05); } @@ -83,7 +84,9 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state) // Create odom subscriber for backward blind docking std::string odom_topic; get_parameter("odom_topic", odom_topic); - odom_sub_ = std::make_unique(node, odom_topic); + double odom_duration; + get_parameter("odom_duration", odom_duration); + odom_sub_ = std::make_unique(node, odom_duration, odom_topic); // Create the action servers for dock / undock docking_action_server_ = node->create_action_server( @@ -118,7 +121,7 @@ DockingServer::on_activate(const rclcpp_lifecycle::State & /*state*/) auto node = shared_from_this(); - tf2_listener_ = std::make_unique(*tf2_buffer_); + tf2_listener_ = std::make_unique(*tf2_buffer_, this, true); dock_db_->activate(); navigator_->activate(); vel_publisher_->on_activate(); @@ -316,6 +319,7 @@ void DockingServer::dockRobot() // Approach the dock using control law if (approachDock(dock, dock_pose, dock_backward)) { // We are docked, wait for charging to begin + publishZeroVelocity(); RCLCPP_INFO( get_logger(), "Made contact with dock, waiting for charge to start (if applicable)."); if (waitForCharge(dock)) { @@ -464,7 +468,7 @@ void DockingServer::rotateToDock(const geometry_msgs::msg::PoseStamped & dock_po } auto current_vel = std::make_unique(); - current_vel->twist.angular.z = odom_sub_->getTwist().theta; + current_vel->twist.angular.z = odom_sub_->getRawTwist().angular.z; auto command = std::make_unique(); command->header = robot_pose.header; diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp index f0e982a919b..8708b31428d 100644 --- a/nav2_docking/opennav_docking/test/test_controller.cpp +++ b/nav2_docking/opennav_docking/test/test_controller.cpp @@ -21,7 +21,7 @@ #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_ros_common/node_utils.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" // Testing the controller at high level; the nav2_graceful_controller diff --git a/nav2_docking/opennav_docking_bt/CMakeLists.txt b/nav2_docking/opennav_docking_bt/CMakeLists.txt index 015265a3753..2d0306397aa 100644 --- a/nav2_docking/opennav_docking_bt/CMakeLists.txt +++ b/nav2_docking/opennav_docking_bt/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(opennav_docking_bt) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_behavior_tree REQUIRED) diff --git a/nav2_docking/opennav_docking_bt/package.xml b/nav2_docking/opennav_docking_bt/package.xml index 3b946c8c296..9fc0d3c0692 100644 --- a/nav2_docking/opennav_docking_bt/package.xml +++ b/nav2_docking/opennav_docking_bt/package.xml @@ -10,6 +10,7 @@ ament_cmake nav2_common + backward_ros behaviortree_cpp geometry_msgs nav2_behavior_tree diff --git a/nav2_docking/opennav_docking_core/CMakeLists.txt b/nav2_docking/opennav_docking_core/CMakeLists.txt index 9575861645a..e8a4f821884 100644 --- a/nav2_docking/opennav_docking_core/CMakeLists.txt +++ b/nav2_docking/opennav_docking_core/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(opennav_docking_core) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(rclcpp_lifecycle REQUIRED) diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp index a923cc98d26..67e599405d7 100644 --- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp @@ -20,7 +20,7 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" namespace opennav_docking_core diff --git a/nav2_docking/opennav_docking_core/package.xml b/nav2_docking/opennav_docking_core/package.xml index 735f89cf24d..285bf274cce 100644 --- a/nav2_docking/opennav_docking_core/package.xml +++ b/nav2_docking/opennav_docking_core/package.xml @@ -10,6 +10,7 @@ ament_cmake nav2_common + backward_ros geometry_msgs rclcpp_lifecycle tf2_ros diff --git a/nav2_dwb_controller/costmap_queue/CMakeLists.txt b/nav2_dwb_controller/costmap_queue/CMakeLists.txt index 28d47ed3dd5..c330e96a9f0 100644 --- a/nav2_dwb_controller/costmap_queue/CMakeLists.txt +++ b/nav2_dwb_controller/costmap_queue/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(costmap_queue) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_costmap_2d REQUIRED) diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 6d96d276a12..a74b57e69a6 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -9,6 +9,7 @@ ament_cmake nav2_common + backward_ros nav2_costmap_2d ament_cmake_gtest diff --git a/nav2_dwb_controller/dwb_core/CMakeLists.txt b/nav2_dwb_controller/dwb_core/CMakeLists.txt index ba787fb831c..f69d546f551 100644 --- a/nav2_dwb_controller/dwb_core/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_core/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(dwb_core) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(dwb_msgs REQUIRED) find_package(geometry_msgs REQUIRED) @@ -44,7 +45,6 @@ target_link_libraries(dwb_core PUBLIC nav2_util::nav2_util_core ${nav_2d_msgs_TARGETS} nav_2d_utils::conversions - nav_2d_utils::tf_help ${nav_msgs_TARGETS} pluginlib::pluginlib rclcpp::rclcpp diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index 41120a7c48a..63013bd091e 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -199,7 +199,7 @@ class DWBLocalPlanner : public nav2_core::Controller bool prune_plan_; double prune_distance_; bool debug_trajectory_details_; - rclcpp::Duration transform_tolerance_{0, 0}; + double transform_tolerance_{0}; bool shorten_transformed_plan_; double forward_prune_distance_; diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index ce879804bbc..a76dd8c474c 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -10,6 +10,7 @@ ament_cmake nav2_common + backward_ros builtin_interfaces dwb_msgs geometry_msgs diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index e539c98819a..b85727b95a0 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -44,7 +44,6 @@ #include "dwb_msgs/msg/critic_score.hpp" #include "nav_2d_msgs/msg/twist2_d.hpp" #include "nav_2d_utils/conversions.hpp" -#include "nav_2d_utils/tf_help.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_ros_common/node_utils.hpp" @@ -112,8 +111,7 @@ void DWBLocalPlanner::configure( std::string traj_generator_name; double transform_tolerance; - node->get_parameter(dwb_plugin_name_ + ".transform_tolerance", transform_tolerance); - transform_tolerance_ = rclcpp::Duration::from_seconds(transform_tolerance); + node->get_parameter(dwb_plugin_name_ + ".transform_tolerance", transform_tolerance_); RCLCPP_INFO(logger_, "Setting transform_tolerance to %f", transform_tolerance); node->get_parameter(dwb_plugin_name_ + ".prune_plan", prune_plan_); @@ -292,10 +290,11 @@ DWBLocalPlanner::prepareGlobalPlan( } goal_pose.header.frame_id = global_plan_.header.frame_id; + goal_pose.header.stamp = pose.header.stamp; goal_pose.pose = global_plan_.poses.back().pose; - nav_2d_utils::transformPose( - tf_, costmap_ros_->getGlobalFrameID(), goal_pose, - goal_pose, transform_tolerance_); + nav2_util::transformPoseInTargetFrame( + goal_pose, goal_pose, *tf_, + costmap_ros_->getGlobalFrameID(), transform_tolerance_); } nav_2d_msgs::msg::Twist2DStamped @@ -470,9 +469,9 @@ DWBLocalPlanner::transformGlobalPlan( // let's get the pose of the robot in the frame of the plan geometry_msgs::msg::PoseStamped robot_pose; - if (!nav_2d_utils::transformPose( - tf_, global_plan_.header.frame_id, pose, - robot_pose, transform_tolerance_)) + if (!nav2_util::transformPoseInTargetFrame( + pose, robot_pose, *tf_, + global_plan_.header.frame_id, transform_tolerance_)) { throw nav2_core:: ControllerTFError("Unable to transform robot pose into global plan's frame"); @@ -537,10 +536,17 @@ DWBLocalPlanner::transformGlobalPlan( // Helper function for the transform below. Converts a pose2D from global // frame to local auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped transformed_pose; - nav_2d_utils::transformPose( - tf_, transformed_plan.header.frame_id, - global_plan_pose, transformed_pose, transform_tolerance_); + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = global_plan_.header.frame_id; + stamped_pose.header.stamp = robot_pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + if (!nav2_util::transformPoseInTargetFrame( + stamped_pose, transformed_pose, *tf_, + transformed_plan.header.frame_id, transform_tolerance_)) + { + throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); + } + transformed_pose.pose.position.z = 0.0; return transformed_pose; }; diff --git a/nav2_dwb_controller/dwb_critics/CMakeLists.txt b/nav2_dwb_controller/dwb_critics/CMakeLists.txt index b1cb464cd5c..be02bcc98f8 100644 --- a/nav2_dwb_controller/dwb_critics/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_critics/CMakeLists.txt @@ -1,8 +1,9 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(dwb_critics) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(costmap_queue REQUIRED) find_package(dwb_core REQUIRED) find_package(dwb_msgs REQUIRED) diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 79daf191b20..eb923ca051e 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -9,6 +9,7 @@ ament_cmake nav2_common + backward_ros angles costmap_queue dwb_core diff --git a/nav2_dwb_controller/dwb_critics/src/goal_align.cpp b/nav2_dwb_controller/dwb_critics/src/goal_align.cpp index 1ae85c2e1dd..f8b713c032f 100644 --- a/nav2_dwb_controller/dwb_critics/src/goal_align.cpp +++ b/nav2_dwb_controller/dwb_critics/src/goal_align.cpp @@ -37,7 +37,6 @@ #include #include "dwb_critics/alignment_util.hpp" #include "pluginlib/class_list_macros.hpp" -#include "nav_2d_utils/parameters.hpp" namespace dwb_critics { @@ -52,9 +51,8 @@ void GoalAlignCritic::onInit() throw std::runtime_error{"Failed to lock node"}; } - forward_point_distance_ = nav_2d_utils::searchAndGetParam( - node, - dwb_plugin_name_ + "." + name_ + ".forward_point_distance", 0.325); + forward_point_distance_ = node->declare_or_get_parameter(dwb_plugin_name_ + "." + name_ + + ".forward_point_distance", 0.325); } bool GoalAlignCritic::prepare( diff --git a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp index bad8e40e3ff..170a9764c1f 100644 --- a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp +++ b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp @@ -37,7 +37,6 @@ #include #include #include -#include "nav_2d_utils/parameters.hpp" #include "nav2_ros_common/node_utils.hpp" #include "dwb_core/exceptions.hpp" #include "pluginlib/class_list_macros.hpp" @@ -99,16 +98,13 @@ void OscillationCritic::onInit() clock_ = node->get_clock(); - oscillation_reset_dist_ = nav_2d_utils::searchAndGetParam( - node, - dwb_plugin_name_ + "." + name_ + ".oscillation_reset_dist", 0.05); + oscillation_reset_dist_ = node->declare_or_get_parameter(dwb_plugin_name_ + "." + name_ + + ".oscillation_reset_dist", 0.05); oscillation_reset_dist_sq_ = oscillation_reset_dist_ * oscillation_reset_dist_; - oscillation_reset_angle_ = nav_2d_utils::searchAndGetParam( - node, + oscillation_reset_angle_ = node->declare_or_get_parameter( dwb_plugin_name_ + "." + name_ + ".oscillation_reset_angle", 0.2); oscillation_reset_time_ = rclcpp::Duration::from_seconds( - nav_2d_utils::searchAndGetParam( - node, + node->declare_or_get_parameter( dwb_plugin_name_ + "." + name_ + ".oscillation_reset_time", -1.0)); nav2::declare_parameter_if_not_declared( diff --git a/nav2_dwb_controller/dwb_critics/src/path_align.cpp b/nav2_dwb_controller/dwb_critics/src/path_align.cpp index 6c4b28ea6f5..d91414586dd 100644 --- a/nav2_dwb_controller/dwb_critics/src/path_align.cpp +++ b/nav2_dwb_controller/dwb_critics/src/path_align.cpp @@ -37,7 +37,6 @@ #include #include "dwb_critics/alignment_util.hpp" #include "pluginlib/class_list_macros.hpp" -#include "nav_2d_utils/parameters.hpp" namespace dwb_critics { @@ -52,8 +51,7 @@ void PathAlignCritic::onInit() throw std::runtime_error{"Failed to lock node"}; } - forward_point_distance_ = nav_2d_utils::searchAndGetParam( - node, + forward_point_distance_ = node->declare_or_get_parameter( dwb_plugin_name_ + "." + name_ + ".forward_point_distance", 0.325); } diff --git a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp index f4323188b18..34f2ae95335 100644 --- a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp +++ b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp @@ -35,7 +35,6 @@ #include "dwb_critics/rotate_to_goal.hpp" #include #include -#include "nav_2d_utils/parameters.hpp" #include "dwb_core/exceptions.hpp" #include "pluginlib/class_list_macros.hpp" #include "dwb_core/trajectory_utils.hpp" @@ -58,19 +57,15 @@ void RotateToGoalCritic::onInit() throw std::runtime_error{"Failed to lock node"}; } - xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam( - node, + xy_goal_tolerance_ = node->declare_or_get_parameter( dwb_plugin_name_ + ".xy_goal_tolerance", 0.25); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; - double stopped_xy_velocity = nav_2d_utils::searchAndGetParam( - node, + double stopped_xy_velocity = node->declare_or_get_parameter( dwb_plugin_name_ + ".trans_stopped_velocity", 0.25); stopped_xy_velocity_sq_ = stopped_xy_velocity * stopped_xy_velocity; - slowing_factor_ = nav_2d_utils::searchAndGetParam( - node, + slowing_factor_ = node->declare_or_get_parameter( dwb_plugin_name_ + "." + name_ + ".slowing_factor", 5.0); - lookahead_time_ = nav_2d_utils::searchAndGetParam( - node, + lookahead_time_ = node->declare_or_get_parameter( dwb_plugin_name_ + "." + name_ + ".lookahead_time", -1.0); reset(); } diff --git a/nav2_dwb_controller/dwb_msgs/CMakeLists.txt b/nav2_dwb_controller/dwb_msgs/CMakeLists.txt index 2f34fcdf5ea..9ff83591d2e 100644 --- a/nav2_dwb_controller/dwb_msgs/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_msgs/CMakeLists.txt @@ -1,6 +1,7 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(dwb_msgs) +find_package(backward_ros REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_2d_msgs REQUIRED) diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 7c17869c2f3..9d1b3f47e6b 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -9,6 +9,7 @@ ament_cmake + backward_ros builtin_interfaces geometry_msgs nav_2d_msgs diff --git a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt index f725aa2cf94..b462e7b3aab 100644 --- a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(dwb_plugins) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(dwb_core REQUIRED) find_package(dwb_msgs REQUIRED) find_package(geometry_msgs REQUIRED) diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index d07a7b79f08..f25f724d95a 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -12,6 +12,7 @@ ament_cmake nav2_common + backward_ros dwb_core dwb_msgs geometry_msgs diff --git a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp index dde49133011..c1152bc3f5b 100644 --- a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp @@ -38,7 +38,6 @@ #include #include -#include "nav_2d_utils/parameters.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" diff --git a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp index 24d1c255973..4c096f7e1f1 100644 --- a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp @@ -36,7 +36,6 @@ #include #include #include -#include "nav_2d_utils/parameters.hpp" #include "pluginlib/class_list_macros.hpp" #include "dwb_core/exceptions.hpp" #include "nav2_ros_common/node_utils.hpp" @@ -65,8 +64,7 @@ void LimitedAccelGenerator::initialize( RCLCPP_WARN( rclcpp::get_logger("LimitedAccelGenerator"), "'sim_period' parameter is not set for %s", plugin_name.c_str()); - double controller_frequency = nav_2d_utils::searchAndGetParam( - nh, "controller_frequency", 20.0); + double controller_frequency = nh->declare_or_get_parameter("controller_frequency", 20.0); if (controller_frequency > 0) { acceleration_time_ = 1.0 / controller_frequency; } else { diff --git a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp index 8e68a399f08..a11dd8ece0b 100644 --- a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp @@ -38,7 +38,6 @@ #include #include #include "dwb_plugins/xy_theta_iterator.hpp" -#include "nav_2d_utils/parameters.hpp" #include "pluginlib/class_list_macros.hpp" #include "dwb_core/exceptions.hpp" #include "nav2_ros_common/node_utils.hpp" diff --git a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp index c319bef4a72..01dab35cb7b 100644 --- a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp @@ -38,7 +38,6 @@ #include #include -#include "nav_2d_utils/parameters.hpp" #include "nav2_ros_common/node_utils.hpp" namespace dwb_plugins diff --git a/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt b/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt index e900fd292ca..3fd5af5cc2a 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt +++ b/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_dwb_controller) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(nav2_common REQUIRED) nav2_package() diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 7d8363bcfc2..8eea7edacb8 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -11,8 +11,10 @@ Apache-2.0 ament_cmake + backward_ros nav2_common + backward_ros costmap_queue dwb_core dwb_critics diff --git a/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt b/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt index c2838963c9b..f597712deda 100644 --- a/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt +++ b/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt @@ -1,6 +1,7 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav_2d_msgs) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 129c4d3367a..bd343580140 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -11,6 +11,7 @@ ament_cmake + backward_ros geometry_msgs std_msgs rosidl_default_generators diff --git a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt index 8a729af46a9..5378f2e4b83 100644 --- a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt +++ b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav_2d_utils) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_2d_msgs REQUIRED) find_package(nav_msgs REQUIRED) @@ -55,28 +56,7 @@ target_link_libraries(path_ops PRIVATE ${geometry_msgs_TARGETS} ) -add_library(tf_help SHARED - src/tf_help.cpp -) -target_include_directories(tf_help - PUBLIC - "$" - "$" -) -target_link_libraries(tf_help PUBLIC - conversions - ${geometry_msgs_TARGETS} - ${nav_2d_msgs_TARGETS} - rclcpp::rclcpp - nav2_ros_common::nav2_ros_common - tf2::tf2 - tf2_geometry_msgs::tf2_geometry_msgs - tf2_ros::tf2_ros - rclcpp_lifecycle::rclcpp_lifecycle - rcl_lifecycle::rcl_lifecycle -) - -install(TARGETS conversions path_ops tf_help +install(TARGETS conversions path_ops EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -100,7 +80,7 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include/${PROJECT_NAME}) -ament_export_libraries(conversions path_ops tf_help) +ament_export_libraries(conversions path_ops) ament_export_dependencies( geometry_msgs nav_2d_msgs diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp index 35f3ec2018f..3a30fd03050 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp +++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp @@ -49,10 +49,6 @@ namespace nav_2d_utils { geometry_msgs::msg::Twist twist2Dto3D(const nav_2d_msgs::msg::Twist2D & cmd_vel_2d); nav_2d_msgs::msg::Twist2D twist3Dto2D(const geometry_msgs::msg::Twist & cmd_vel); -geometry_msgs::msg::PoseStamped poseToPoseStamped( - const geometry_msgs::msg::Pose & pose, - const std::string & frame, const rclcpp::Time & stamp); -nav_msgs::msg::Path posesToPath(const std::vector & poses); nav_msgs::msg::Path posesToPath( const std::vector & poses, const std::string & frame, const rclcpp::Time & stamp); diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp deleted file mode 100644 index 2f989e71f2f..00000000000 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp +++ /dev/null @@ -1,100 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_ -#define NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_ - -#include -#include -#include -#include -#include "nav_2d_msgs/msg/twist2_d_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "nav2_ros_common/lifecycle_node.hpp" -#include "rclcpp/rclcpp.hpp" -#include "nav2_ros_common/node_utils.hpp" - -namespace nav_2d_utils -{ - -/** - * @class OdomSubscriber - * Wrapper for some common odometry operations. Subscribes to the topic with a mutex. - */ -class OdomSubscriber -{ -public: - /** - * @brief Constructor that subscribes to an Odometry topic - * - * @param nh NodeHandle for creating subscriber - * @param default_topic Name of the topic that will be loaded of the odom_topic param is not set. - */ - explicit OdomSubscriber( - nav2::LifecycleNode::SharedPtr nh, - std::string default_topic = "odom") - { - nav2::declare_parameter_if_not_declared( - nh, "odom_topic", rclcpp::ParameterValue(default_topic)); - - std::string odom_topic; - nh->get_parameter_or("odom_topic", odom_topic, default_topic); - odom_sub_ = - nh->create_subscription( - odom_topic, - std::bind(&OdomSubscriber::odomCallback, this, std::placeholders::_1)); - } - - inline nav_2d_msgs::msg::Twist2D getTwist() {return odom_vel_.velocity;} - inline nav_2d_msgs::msg::Twist2DStamped getTwistStamped() {return odom_vel_;} - -protected: - void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) - { - // ROS_INFO_ONCE("odom received!"); - std::lock_guard lock(odom_mutex_); - odom_vel_.header = msg->header; - odom_vel_.velocity.x = msg->twist.twist.linear.x; - odom_vel_.velocity.y = msg->twist.twist.linear.y; - odom_vel_.velocity.theta = msg->twist.twist.angular.z; - } - - nav2::Subscription::SharedPtr odom_sub_; - nav_2d_msgs::msg::Twist2DStamped odom_vel_; - std::mutex odom_mutex_; -}; - -} // namespace nav_2d_utils - -#endif // NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_ diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp deleted file mode 100644 index 09b039487e0..00000000000 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp +++ /dev/null @@ -1,76 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef NAV_2D_UTILS__PARAMETERS_HPP_ -#define NAV_2D_UTILS__PARAMETERS_HPP_ - -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "nav2_ros_common/lifecycle_node.hpp" -#include "nav2_ros_common/node_utils.hpp" - -// TODO(crdelsey): Remove when code is re-enabled -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-parameter" -namespace nav_2d_utils -{ - -/** - * @brief Search for a parameter and load it, or use the default value - * - * This templated function shortens a commonly used ROS pattern in which you - * search for a parameter and get its value if it exists, otherwise returning a default value. - * - * @param nh NodeHandle to start the parameter search from - * @param param_name Name of the parameter to search for - * @param default_value Value to return if not found - * @return Value of parameter if found, otherwise the default_value - */ -template -param_t searchAndGetParam( - const nav2::LifecycleNode::SharedPtr & nh, const std::string & param_name, - const param_t & default_value) -{ - nav2::declare_parameter_if_not_declared( - nh, param_name, - rclcpp::ParameterValue(default_value)); - return nh->get_parameter(param_name).get_value(); -} - -} // namespace nav_2d_utils -#pragma GCC diagnostic pop - -#endif // NAV_2D_UTILS__PARAMETERS_HPP_ diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp deleted file mode 100644 index 9bed6cd6810..00000000000 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp +++ /dev/null @@ -1,85 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef NAV_2D_UTILS__TF_HELP_HPP_ -#define NAV_2D_UTILS__TF_HELP_HPP_ - -#include -#include -#include "tf2_ros/buffer.h" -#include "nav_2d_utils/conversions.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - -namespace nav_2d_utils -{ -/** - * @brief Transform a PoseStamped from one frame to another while catching exceptions - * - * Also returns immediately if the frames are equal. - * @param tf Smart pointer to TFListener - * @param frame Frame to transform the pose into - * @param in_pose Pose to transform - * @param out_pose Place to store the resulting transformed pose - * @return True if successful transform - */ -bool transformPose( - const std::shared_ptr tf, - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose, - rclcpp::Duration & transform_tolerance -); - -/** - * @brief Transform a PoseStamped from one frame to another while catching exceptions - * - * Also returns immediately if the frames are equal. - * @param tf Smart pointer to TFListener - * @param frame Frame to transform the pose into - * @param in_pose Pose to transform - * @param out_pose Place to store the resulting transformed pose - * @return True if successful transform - */ -bool transformPose( - const std::shared_ptr tf, - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose, - rclcpp::Duration & transform_tolerance -); - -} // namespace nav_2d_utils - -#endif // NAV_2D_UTILS__TF_HELP_HPP_ diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index ea8e213c77d..9968469eab4 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -10,6 +10,7 @@ ament_cmake nav2_common + backward_ros geometry_msgs nav_2d_msgs nav_msgs diff --git a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp index 4ec9a73fb99..3629e8bb989 100644 --- a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp +++ b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp @@ -72,34 +72,6 @@ nav_2d_msgs::msg::Twist2D twist3Dto2D(const geometry_msgs::msg::Twist & cmd_vel) return cmd_vel_2d; } - -geometry_msgs::msg::PoseStamped poseToPoseStamped( - const geometry_msgs::msg::Pose & pose_in, - const std::string & frame, const rclcpp::Time & stamp) -{ - geometry_msgs::msg::PoseStamped pose; - pose.header.frame_id = frame; - pose.header.stamp = stamp; - pose.pose = pose_in; - return pose; -} - -nav_msgs::msg::Path posesToPath(const std::vector & poses) -{ - nav_msgs::msg::Path path; - if (poses.empty()) { - return path; - } - path.poses.resize(poses.size()); - path.header.frame_id = poses[0].header.frame_id; - path.header.stamp = poses[0].header.stamp; - for (unsigned int i = 0; i < poses.size(); i++) { - path.poses[i] = poses[i]; - } - return path; -} - - nav_msgs::msg::Path posesToPath( const std::vector & poses, const std::string & frame, const rclcpp::Time & stamp) diff --git a/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp b/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp deleted file mode 100644 index 0916cc6a402..00000000000 --- a/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp +++ /dev/null @@ -1,98 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include "nav_2d_utils/tf_help.hpp" - -namespace nav_2d_utils -{ - -bool transformPose( - const std::shared_ptr tf, - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose, - rclcpp::Duration & transform_tolerance -) -{ - if (in_pose.header.frame_id == frame) { - out_pose = in_pose; - return true; - } - - try { - tf->transform(in_pose, out_pose, frame); - return true; - } catch (tf2::ExtrapolationException & ex) { - auto transform = tf->lookupTransform( - frame, - in_pose.header.frame_id, - tf2::TimePointZero - ); - if ( - (rclcpp::Time(in_pose.header.stamp) - rclcpp::Time(transform.header.stamp)) > - transform_tolerance) - { - RCLCPP_ERROR( - rclcpp::get_logger("tf_help"), - "Transform data too old when converting from %s to %s", - in_pose.header.frame_id.c_str(), - frame.c_str() - ); - RCLCPP_ERROR( - rclcpp::get_logger("tf_help"), - "Data time: %ds %uns, Transform time: %ds %uns", - in_pose.header.stamp.sec, - in_pose.header.stamp.nanosec, - transform.header.stamp.sec, - transform.header.stamp.nanosec - ); - return false; - } else { - tf2::doTransform(in_pose, out_pose, transform); - return true; - } - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - rclcpp::get_logger("tf_help"), - "Exception in transformPose: %s", - ex.what() - ); - return false; - } - return false; -} - -} // namespace nav_2d_utils diff --git a/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp b/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp index c7f5c502f76..fd7d209d3d7 100644 --- a/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp +++ b/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp @@ -48,59 +48,55 @@ using nav_2d_utils::posesToPath; TEST(nav_2d_utils, PosesToPathEmpty) { - std::vector poses; - nav_msgs::msg::Path path = posesToPath(poses); + std::vector poses; + auto node = std::make_shared("twod_utils_test_node"); + rclcpp::Time time = node->now(); + std::string frame = "test_frame"; + + nav_msgs::msg::Path path = posesToPath(poses, frame, time); EXPECT_EQ(path.poses.size(), 0ul); + EXPECT_EQ(path.header.frame_id, frame); + EXPECT_EQ(path.header.stamp, time); } TEST(nav_2d_utils, PosesToPathNonEmpty) { - std::vector poses; - geometry_msgs::msg::PoseStamped pose1; - rclcpp::Time time1, time2; auto node = std::make_shared("twod_utils_test_node"); - time1 = node->now(); + rclcpp::Time time = node->now(); + std::string frame = "map"; tf2::Quaternion quat1, quat2; quat1.setRPY(0, 0, 0.123); - pose1.pose.position.x = 1.0; - pose1.pose.position.y = 2.0; - pose1.pose.orientation.w = quat1.w(); - pose1.pose.orientation.x = quat1.x(); - pose1.pose.orientation.y = quat1.y(); - pose1.pose.orientation.z = quat1.z(); - pose1.header.stamp = time1; - pose1.header.frame_id = "frame1_id"; - - geometry_msgs::msg::PoseStamped pose2; - pose2.pose.position.x = 4.0; - pose2.pose.position.y = 5.0; quat2.setRPY(0, 0, 0.987); - pose2.pose.orientation.w = quat2.w(); - pose2.pose.orientation.x = quat2.x(); - pose2.pose.orientation.y = quat2.y(); - pose2.pose.orientation.z = quat2.z(); - time2 = node->now(); - pose2.header.stamp = time2; - pose2.header.frame_id = "frame2_id"; + geometry_msgs::msg::Pose pose1; + pose1.position.x = 1.0; + pose1.position.y = 2.0; + pose1.orientation = tf2::toMsg(quat1); - poses.push_back(pose1); - poses.push_back(pose2); + geometry_msgs::msg::Pose pose2; + pose2.position.x = 4.0; + pose2.position.y = 5.0; + pose2.orientation = tf2::toMsg(quat2); - nav_msgs::msg::Path path = posesToPath(poses); + std::vector poses = {pose1, pose2}; + + nav_msgs::msg::Path path = posesToPath(poses, frame, time); EXPECT_EQ(path.poses.size(), 2ul); EXPECT_EQ(path.poses[0].pose.position.x, 1.0); EXPECT_EQ(path.poses[0].pose.position.y, 2.0); - EXPECT_EQ(path.poses[0].header.stamp, time1); - EXPECT_EQ(path.poses[0].header.frame_id, "frame1_id"); EXPECT_EQ(path.poses[1].pose.position.x, 4.0); EXPECT_EQ(path.poses[1].pose.position.y, 5.0); - EXPECT_EQ(path.poses[1].header.frame_id, "frame2_id"); - EXPECT_EQ(path.header.stamp, time1); + for (const auto & stamped_pose : path.poses) { + EXPECT_EQ(stamped_pose.header.frame_id, frame); + EXPECT_EQ(stamped_pose.header.stamp, time); + } + + EXPECT_EQ(path.header.frame_id, frame); + EXPECT_EQ(path.header.stamp, time); } diff --git a/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt index f0cf339a322..fd861291fca 100644 --- a/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt +++ b/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt @@ -11,6 +11,3 @@ target_include_directories(2d_utils_tests ament_add_gtest(path_ops_tests path_ops_test.cpp) target_link_libraries(path_ops_tests path_ops) - -ament_add_gtest(tf_help_tests tf_help_test.cpp) -target_link_libraries(tf_help_tests tf_help conversions) diff --git a/nav2_dwb_controller/nav_2d_utils/test/tf_help_test.cpp b/nav2_dwb_controller/nav_2d_utils/test/tf_help_test.cpp deleted file mode 100644 index 4095567473a..00000000000 --- a/nav2_dwb_controller/nav_2d_utils/test/tf_help_test.cpp +++ /dev/null @@ -1,98 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, Wilco Bonestroo - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include - -#include "gtest/gtest.h" -#include "nav_2d_utils/tf_help.hpp" - -TEST(TF_Help, TransformToSelf) { - bool result; - - std::shared_ptr tf; - std::string frame = "frame_id"; - geometry_msgs::msg::PoseStamped in_pose; - in_pose.header.frame_id = "frame_id"; - in_pose.pose.position.x = 1.0; - in_pose.pose.position.y = 2.0; - in_pose.pose.position.z = 3.0; - tf2::Quaternion qt; - qt.setRPY(0.5, 1.0, 1.5); - in_pose.pose.orientation.w = qt.w(); - in_pose.pose.orientation.x = qt.x(); - in_pose.pose.orientation.y = qt.y(); - in_pose.pose.orientation.z = qt.z(); - - geometry_msgs::msg::PoseStamped out_pose; - rclcpp::Duration transform_tolerance(0, 500); - - result = nav_2d_utils::transformPose(tf, frame, in_pose, out_pose, transform_tolerance); - - EXPECT_TRUE(result); - EXPECT_EQ(out_pose.header.frame_id, "frame_id"); - EXPECT_EQ(out_pose.pose.position.x, 1.0); - EXPECT_EQ(out_pose.pose.position.y, 2.0); - EXPECT_EQ(out_pose.pose.position.z, 3.0); - EXPECT_EQ(out_pose.pose.orientation.w, qt.w()); - EXPECT_EQ(out_pose.pose.orientation.x, qt.x()); - EXPECT_EQ(out_pose.pose.orientation.y, qt.y()); - EXPECT_EQ(out_pose.pose.orientation.z, qt.z()); -} - -TEST(TF_Help, EmptyBuffer) { - auto clock = std::make_shared(RCL_ROS_TIME); - auto buffer = std::make_shared(clock); - - std::string frame = "frame_id"; - geometry_msgs::msg::PoseStamped in_pose; - in_pose.header.frame_id = "other_frame_id"; - in_pose.pose.position.x = 1.0; - in_pose.pose.position.y = 2.0; - in_pose.pose.position.z = 3.0; - tf2::Quaternion qt; - qt.setRPY(0.5, 1.0, 1.5); - in_pose.pose.orientation.w = qt.w(); - in_pose.pose.orientation.x = qt.x(); - in_pose.pose.orientation.y = qt.y(); - in_pose.pose.orientation.z = qt.z(); - - geometry_msgs::msg::PoseStamped out_pose; - rclcpp::Duration transform_tolerance(0, 500); - - bool result; - result = nav_2d_utils::transformPose(buffer, frame, in_pose, out_pose, transform_tolerance); - - EXPECT_FALSE(result); -} diff --git a/nav2_graceful_controller/CMakeLists.txt b/nav2_graceful_controller/CMakeLists.txt index 1a72a698085..95d682a8cad 100644 --- a/nav2_graceful_controller/CMakeLists.txt +++ b/nav2_graceful_controller/CMakeLists.txt @@ -1,14 +1,14 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_graceful_controller) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(nav2_util REQUIRED) -find_package(nav_2d_utils REQUIRED) find_package(nav_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) @@ -57,7 +57,6 @@ target_link_libraries(${library_name} PUBLIC nav2_ros_common::nav2_ros_common nav2_util::nav2_util_core nav2_costmap_2d::nav2_costmap_2d_core - nav_2d_utils::tf_help ${nav_msgs_TARGETS} pluginlib::pluginlib rclcpp::rclcpp @@ -103,7 +102,6 @@ ament_export_dependencies( nav2_core nav2_util nav2_costmap_2d - nav_2d_utils nav_msgs pluginlib rclcpp diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp index 96358ae9607..28bec7eddd8 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp @@ -107,6 +107,24 @@ class GracefulController : public nav2_core::Controller void setSpeedLimit(const double & speed_limit, const bool & percentage) override; protected: + /** + * @brief Validate a given target pose for calculating command velocity + * @param target_pose Target pose to validate + * @param dist_to_target Distance to target pose + * @param dist_to_goal Distance to navigation goal + * @param trajectory Trajectory to validate in simulation + * @param costmap_transform Transform between global and local costmap + * @param cmd_vel Initial command velocity to validate in simulation + * @return true if target pose is valid, false otherwise + */ + bool validateTargetPose( + geometry_msgs::msg::PoseStamped & target_pose, + double dist_to_target, + double dist_to_goal, + nav_msgs::msg::Path & trajectory, + geometry_msgs::msg::TransformStamped & costmap_transform, + geometry_msgs::msg::TwistStamped & cmd_vel); + /** * @brief Simulate trajectory calculating in every step the new velocity command based on * a new curvature value and checking for collisions. diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp index aaa64f1bb62..a24a84c2228 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp @@ -35,7 +35,7 @@ class PathHandler * @brief Constructor for nav2_graceful_controller::PathHandler */ PathHandler( - tf2::Duration transform_tolerance, + double transform_tolerance, std::shared_ptr tf, std::shared_ptr costmap_ros); @@ -71,7 +71,7 @@ class PathHandler nav_msgs::msg::Path getPlan() {return global_plan_;} protected: - rclcpp::Duration transform_tolerance_{0, 0}; + double transform_tolerance_{0}; std::shared_ptr tf_buffer_; std::shared_ptr costmap_ros_; nav_msgs::msg::Path global_plan_; diff --git a/nav2_graceful_controller/package.xml b/nav2_graceful_controller/package.xml index a195e466bec..98b6e1e257a 100644 --- a/nav2_graceful_controller/package.xml +++ b/nav2_graceful_controller/package.xml @@ -11,11 +11,11 @@ nav2_common angles + backward_ros geometry_msgs nav2_core nav2_costmap_2d nav2_util - nav_2d_utils nav_msgs pluginlib rclcpp diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index fb0010698f4..b8cce0b0aed 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -18,6 +18,7 @@ #include "angles/angles.h" #include "nav2_core/controller_exceptions.hpp" #include "nav2_util/geometry_utils.hpp" +#include "nav2_util/controller_utils.hpp" #include "nav2_graceful_controller/graceful_controller.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" @@ -48,7 +49,7 @@ void GracefulController::configure( // Handles global path transformations path_handler_ = std::make_unique( - tf2::durationFromSec(params_->transform_tolerance), tf_buffer_, costmap_ros_); + params_->transform_tolerance, tf_buffer_, costmap_ros_); // Handles the control law to generate the velocity commands control_law_ = std::make_unique( @@ -195,46 +196,38 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( // Else, fall through and see if we should follow control law longer } - // Precompute distance to candidate poses + // Find a valid target pose and its trajectory + nav_msgs::msg::Path local_plan; + geometry_msgs::msg::PoseStamped target_pose; + + double dist_to_target; std::vector target_distances; computeDistanceAlongPath(transformed_plan.poses, target_distances); - // Work back from the end of plan to find valid target pose + bool is_first_iteration = true; for (int i = transformed_plan.poses.size() - 1; i >= 0; --i) { - // Underlying control law needs a single target pose, which should: - // * Be as far away as possible from the robot (for smoothness) - // * But no further than the max_lookahed_ distance - // * Be feasible to reach in a collision free manner - geometry_msgs::msg::PoseStamped target_pose = transformed_plan.poses[i]; - double dist_to_target = target_distances[i]; - - // Continue if target_pose is too far away from robot - if (dist_to_target > params_->max_lookahead) {continue;} - - if (dist_to_goal < params_->max_lookahead) { - if (params_->prefer_final_rotation) { - // Avoid instability and big sweeping turns at the end of paths by - // ignoring final heading - double yaw = std::atan2(target_pose.pose.position.y, target_pose.pose.position.x); - target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); - } - } else if (dist_to_target < params_->min_lookahead) { - // Make sure target is far enough away to avoid instability - break; - } - - // Flip the orientation of the motion target if the robot is moving backwards - bool reversing = false; - if (params_->allow_backward && target_pose.pose.position.x < 0.0) { - reversing = true; - target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( - tf2::getYaw(target_pose.pose.orientation) + M_PI); + if (is_first_iteration) { + // Calculate target pose through lookahead interpolation to get most accurate + // lookahead point, if possible + dist_to_target = params_->max_lookahead; + // Interpolate after goal false for graceful controller + // Requires interpolating the orientation which is not yet implemented + // Updates dist_to_target for target_pose returned if using the point on the path + target_pose = nav2_util::getLookAheadPoint(dist_to_target, transformed_plan, false); + is_first_iteration = false; + } else { + // Underlying control law needs a single target pose, which should: + // * Be as far away as possible from the robot (for smoothness) + // * But no further than the max_lookahed_ distance + // * Be feasible to reach in a collision free manner + dist_to_target = target_distances[i]; + target_pose = transformed_plan.poses[i]; } - // Actually simulate our path - nav_msgs::msg::Path local_plan; - if (simulateTrajectory(target_pose, costmap_transform, local_plan, cmd_vel, reversing)) { - // Successfully simulated to target_pose - compute velocity at this moment + // Compute velocity at this moment if valid target pose is found + if (validateTargetPose( + target_pose, dist_to_target, dist_to_goal, local_plan, costmap_transform, cmd_vel)) + { // Publish the selected target_pose motion_target_pub_->publish(target_pose); // Publish marker for slowdown radius around motion target for debugging / visualization @@ -283,6 +276,49 @@ void GracefulController::setSpeedLimit( } } +bool GracefulController::validateTargetPose( + geometry_msgs::msg::PoseStamped & target_pose, + double dist_to_target, + double dist_to_goal, + nav_msgs::msg::Path & trajectory, + geometry_msgs::msg::TransformStamped & costmap_transform, + geometry_msgs::msg::TwistStamped & cmd_vel) +{ + // Continue if target_pose is too far away from robot + if (dist_to_target > params_->max_lookahead) { + return false; + } + + if (dist_to_goal < params_->max_lookahead) { + if (params_->prefer_final_rotation) { + // Avoid instability and big sweeping turns at the end of paths by + // ignoring final heading + double yaw = std::atan2(target_pose.pose.position.y, target_pose.pose.position.x); + target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); + } + } else if (dist_to_target < params_->min_lookahead) { + // Make sure target is far enough away to avoid instability + return false; + } + + // Flip the orientation of the motion target if the robot is moving backwards + bool reversing = false; + if (params_->allow_backward && target_pose.pose.position.x < 0.0) { + reversing = true; + target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( + tf2::getYaw(target_pose.pose.orientation) + M_PI); + } + + // Actually simulate the path + if (simulateTrajectory(target_pose, costmap_transform, trajectory, cmd_vel, reversing)) { + // Successfully simulated to target_pose + return true; + } + + // Validation not successful + return false; +} + bool GracefulController::simulateTrajectory( const geometry_msgs::msg::PoseStamped & motion_target, const geometry_msgs::msg::TransformStamped & costmap_transform, diff --git a/nav2_graceful_controller/src/path_handler.cpp b/nav2_graceful_controller/src/path_handler.cpp index 9dbfc291f8d..b60b4a874f0 100644 --- a/nav2_graceful_controller/src/path_handler.cpp +++ b/nav2_graceful_controller/src/path_handler.cpp @@ -22,7 +22,7 @@ #include "nav2_core/controller_exceptions.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" -#include "nav_2d_utils/tf_help.hpp" +#include "nav2_util/robot_utils.hpp" #include "nav2_graceful_controller/path_handler.hpp" namespace nav2_graceful_controller @@ -31,7 +31,7 @@ namespace nav2_graceful_controller using nav2_util::geometry_utils::euclidean_distance; PathHandler::PathHandler( - tf2::Duration transform_tolerance, + double transform_tolerance, std::shared_ptr tf, std::shared_ptr costmap_ros) : transform_tolerance_(transform_tolerance), tf_buffer_(tf), costmap_ros_(costmap_ros) @@ -49,8 +49,8 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( // Let's get the pose of the robot in the frame of the plan geometry_msgs::msg::PoseStamped robot_pose; - if (!nav_2d_utils::transformPose( - tf_buffer_, global_plan_.header.frame_id, pose, robot_pose, + if (!nav2_util::transformPoseInTargetFrame( + pose, robot_pose, *tf_buffer_, global_plan_.header.frame_id, transform_tolerance_)) { throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); @@ -88,9 +88,9 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( stamped_pose.header.frame_id = global_plan_.header.frame_id; stamped_pose.header.stamp = robot_pose.header.stamp; stamped_pose.pose = global_plan_pose.pose; - if (!nav_2d_utils::transformPose( - tf_buffer_, costmap_ros_->getBaseFrameID(), stamped_pose, - transformed_pose, transform_tolerance_)) + if (!nav2_util::transformPoseInTargetFrame( + stamped_pose, transformed_pose, *tf_buffer_, + costmap_ros_->getBaseFrameID(), transform_tolerance_)) { throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); } diff --git a/nav2_lifecycle_manager/CMakeLists.txt b/nav2_lifecycle_manager/CMakeLists.txt index 654b7153d06..50874482167 100644 --- a/nav2_lifecycle_manager/CMakeLists.txt +++ b/nav2_lifecycle_manager/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_lifecycle_manager) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(bondcpp REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(geometry_msgs REQUIRED) diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 99b0b2f9250..0b6790935fc 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -10,6 +10,7 @@ ament_cmake nav2_common + backward_ros bondcpp diagnostic_updater geometry_msgs diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml index 9d55a6d4837..1df96f1a27e 100644 --- a/nav2_loopback_sim/package.xml +++ b/nav2_loopback_sim/package.xml @@ -7,12 +7,13 @@ steve macenski Apache-2.0 - rclpy geometry_msgs + nav2_simple_commander nav_msgs - tf_transformations - tf2_ros python3-transforms3d + rclpy + tf2_ros + tf_transformations ament_copyright ament_flake8 diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index 438e1aa12fb..6459b42ed2c 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -1,24 +1,30 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_map_server) list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(GRAPHICSMAGICKCPP REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) +find_package(PkgConfig REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) find_package(nav2_ros_common REQUIRED) find_package(yaml_cpp_vendor REQUIRED) find_package(yaml-cpp REQUIRED) +pkg_search_module(UUID REQUIRED uuid) + nav2_package() set(map_server_executable map_server) @@ -27,12 +33,16 @@ set(library_name ${map_server_executable}_core) set(map_io_library_name map_io) +set(vo_library_name vector_object_core) + set(map_saver_cli_executable map_saver_cli) set(map_saver_server_executable map_saver_server) set(costmap_filter_info_server_executable costmap_filter_info_server) +set(vo_server_executable vector_object_server) + add_library(${map_io_library_name} SHARED src/map_mode.cpp src/map_io.cpp) @@ -77,6 +87,31 @@ target_link_libraries(${library_name} PRIVATE yaml-cpp::yaml-cpp ) +add_library(${vo_library_name} SHARED + src/vo_server/vector_object_shapes.cpp + src/vo_server/vector_object_server.cpp) +target_include_directories(${vo_library_name} + PUBLIC + "$" + "$" + "$") +target_link_libraries(${vo_library_name} PUBLIC + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + rclcpp_components::component + tf2_ros::tf2_ros + ${UUID_LIBRARIES} +) +target_link_libraries(${library_name} PRIVATE + ${lifecycle_msgs_TARGETS} + rclcpp_components::component + yaml-cpp::yaml-cpp +) + add_executable(${map_server_executable} src/map_server/main.cpp) target_include_directories(${map_server_executable} @@ -141,12 +176,28 @@ target_link_libraries(${costmap_filter_info_server_executable} PRIVATE rclcpp_lifecycle::rclcpp_lifecycle ) +add_executable(${vo_server_executable} + src/vo_server/main.cpp) +target_include_directories(${vo_server_executable} + PRIVATE + "$" + "$") +target_link_libraries(${vo_server_executable} PRIVATE + ${vo_library_name} + ${nav_msgs_TARGETS} + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) + rclcpp_components_register_nodes(${library_name} "nav2_map_server::CostmapFilterInfoServer") rclcpp_components_register_nodes(${library_name} "nav2_map_server::MapSaver") rclcpp_components_register_nodes(${library_name} "nav2_map_server::MapServer") +rclcpp_components_register_nodes(${vo_library_name} "nav2_map_server::VectorObjectServer") install(TARGETS - ${library_name} ${map_io_library_name} + ${library_name} ${map_io_library_name} ${vo_library_name} EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -154,13 +205,14 @@ install(TARGETS install(TARGETS ${map_server_executable} ${map_saver_cli_executable} ${map_saver_server_executable} - ${costmap_filter_info_server_executable} + ${costmap_filter_info_server_executable} ${vo_server_executable} RUNTIME DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -177,6 +229,7 @@ ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries( ${library_name} ${map_io_library_name} + ${vo_library_name} ) ament_export_dependencies(nav_msgs nav2_msgs nav2_util rclcpp rclcpp_lifecycle nav2_ros_common) ament_export_targets(${library_name}) diff --git a/nav2_map_server/include/nav2_map_server/map_io.hpp b/nav2_map_server/include/nav2_map_server/map_io.hpp index 97dc81821c9..5fc34937ee1 100644 --- a/nav2_map_server/include/nav2_map_server/map_io.hpp +++ b/nav2_map_server/include/nav2_map_server/map_io.hpp @@ -98,6 +98,14 @@ bool saveMapToFile( const nav_msgs::msg::OccupancyGrid & map, const SaveParameters & save_parameters); +/** + * @brief to_string_with_precision + * @param value + * @param precision + * @return + */ +std::string to_string_with_precision(double value, int precision); + /** * @brief Expand ~/ to home user dir. * @param yaml_filename Name of input YAML file. diff --git a/nav2_map_server/include/nav2_map_server/vector_object_server.hpp b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp new file mode 100644 index 00000000000..ed4186722bc --- /dev/null +++ b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp @@ -0,0 +1,228 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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_MAP_SERVER__VECTOR_OBJECT_SERVER_HPP_ +#define NAV2_MAP_SERVER__VECTOR_OBJECT_SERVER_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" + +#include "nav2_msgs/srv/add_shapes.hpp" +#include "nav2_msgs/srv/remove_shapes.hpp" +#include "nav2_msgs/srv/get_shapes.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" + +#include "nav2_map_server/vector_object_utils.hpp" +#include "nav2_map_server/vector_object_shapes.hpp" + +namespace nav2_map_server +{ + +/// @brief Vector Object server node +class VectorObjectServer : public nav2::LifecycleNode +{ +public: + /** + * @brief Constructor for the VectorObjectServer + * @param options Additional options to control creation of the node. + */ + explicit VectorObjectServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +protected: + /** + * @brief: Initializes TF buffer/listener, obtains ROS-parameters, creates incoming services, + * and output map publisher + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Activates output map publisher and creates bond connection + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Deactivates map publisher and timer (if any), destroys bond connection + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Resets all services, publishers, map and TF-s + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + /** + * @brief Called in shutdown state + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Supporting routine obtaining all ROS-parameters + * @return True if all parameters were obtained or false in the failure case + */ + bool obtainParams(); + + /** + * @brief Finds the shape with given UUID + * @param uuid Given UUID to search with + * @return Iterator to the shape, if found. Otherwise past-the-end iterator. + */ + std::vector>::iterator findShape(const unsigned char * uuid); + + /** + * @brief Transform all vector shapes from their local frame to output map frame + * @return Whether all vector objects were transformed successfully + */ + bool transformVectorObjects(); + + /** + * @brief Obtains map boundaries to place all vector objects inside + * @param min_x output min X-boundary of required map + * @param min_y output min Y-boundary of required map + * @param max_x output max X-boundary of required map + * @param max_y output max Y-boundary of required map + * @throw std::exception if can not obtain one of the map boundaries + */ + void getMapBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) const; + + /** + * @brief Creates or updates existing map with required sizes and fills it with default value + * @param min_x min X-boundary of new map + * @param min_y min Y-boundary of new map + * @param max_x max X-boundary of new map + * @param max_y max Y-boundary of new map + * @throw std::exception if map has negative X or Y size + */ + void updateMap( + const double & min_x, const double & min_y, const double & max_x, const double & max_y); + + /** + * @brief Processes all vector objects on raster output map + */ + void putVectorObjectsOnMap(); + + /** + * @brief Publishes output map + */ + void publishMap(); + + /** + * @brief Calculates new map sizes, updates map, processes all vector objects on it + * and publishes output map one time + */ + void processMap(); + + /** + * @brief If map to be update dynamically, creates map processing timer, + * otherwise process map once + */ + void switchMapUpdate(); + + /** + * @brief Callback for AddShapes service call. + * Reads all input vector objects from service request, + * creates them or updates their shape in case of existing objects + * and switches map processing/publishing routine + * @param request_header Service request header + * @param request Service request + * @param response Service response + */ + void addShapesCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + /** + * @brief Callback for GetShapes service call. + * Gets all shapes and returns them to the service response + * @param request_header Service request header + * @param request Service request + * @param response Service response + */ + void getShapesCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + /** + * @brief Callback for RemoveShapes service call. + * Try to remove requested vector objects and switches map processing/publishing routine + * @param request_header Service request header + * @param request Service request + * @param response Service response + */ + void removeShapesCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + +protected: + /// @brief TF buffer + std::shared_ptr tf_buffer_; + /// @brief TF listener + std::shared_ptr tf_listener_; + + /// @brief All shapes vector + std::vector> shapes_; + + /// @brief Output map resolution + double resolution_; + /// @brief Default value the output map to be filled with + int8_t default_value_; + /// @brief @Overlay Type of overlay of vector objects on the map + OverlayType overlay_type_; + + /// @brief Output map with vector objects on it + nav_msgs::msg::OccupancyGrid::SharedPtr map_; + /// @brief Whether to process and publish map + double process_map_; + + /// @brief Frame of output map + std::string global_frame_id_; + /// @brief Transform tolerance + double transform_tolerance_; + + /// @brief Frequency to dynamically update/publish the map (if necessary) + double update_frequency_; + /// @brief Map update timer + rclcpp::TimerBase::SharedPtr map_timer_; + + /// @brief AddShapes service + nav2::ServiceServer::SharedPtr add_shapes_service_; + /// @brief GetShapes service + nav2::ServiceServer::SharedPtr get_shapes_service_; + /// @brief RemoveShapes service + nav2::ServiceServer::SharedPtr remove_shapes_service_; + + /// @beirf Topic name where the output map to be published to + std::string map_topic_; + /// @brief Output map publisher + nav2::Publisher::SharedPtr map_pub_; +}; + +} // namespace nav2_map_server + +#endif // NAV2_MAP_SERVER__VECTOR_OBJECT_SERVER_HPP_ diff --git a/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp new file mode 100644 index 00000000000..dd5cae95b62 --- /dev/null +++ b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp @@ -0,0 +1,413 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_ +#define NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/polygon.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "tf2_ros/buffer.hpp" + +#include "nav2_msgs/msg/polygon_object.hpp" +#include "nav2_msgs/msg/circle_object.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" + +#include "nav2_map_server/vector_object_utils.hpp" + +namespace nav2_map_server +{ + +/// @brief Possible vector object shape types +enum ShapeType +{ + UNKNOWN = 0, + POLYGON = 1, + CIRCLE = 2 +}; + +/// @brief Basic class, other vector objects to be inherited from +class Shape +{ +public: + /** + * @brief Shape basic class constructor + * @param node Vector Object server node pointer + */ + explicit Shape(const nav2::LifecycleNode::WeakPtr & node); + + /** + * @brief Shape destructor + */ + virtual ~Shape(); + + /** + * @brief Returns type of the shape + * @return Type of the shape + */ + ShapeType getType(); + + /** + * @brief Supporting routine obtaining shape UUID from ROS-parameters + * for the given shape object + * @return True if UUID was obtained or false in failure case + */ + bool obtainShapeUUID(const std::string & shape_name, unsigned char * out_uuid); + + /** + * @brief Gets the value of the shape. + * Empty virtual method intended to be used in child implementations + * @return OccupancyGrid value of the shape + */ + virtual int8_t getValue() const = 0; + + /** + * @brief Gets frame ID of the shape. + * Empty virtual method intended to be used in child implementations + * @return Frame ID where the shape is + */ + virtual std::string getFrameID() const = 0; + + /** + * @brief Gets UUID of the shape. + * Empty virtual method intended to be used in child implementations + * @return Shape UUID string + */ + virtual std::string getUUID() const = 0; + + /** + * @brief Checks whether the shape is equal to a given UUID. + * Empty virtual method intended to be used in child implementations + * @param uuid Given UUID to check with + * @return True if the shape has the same as given UUID, otherwise false + */ + virtual bool isUUID(const unsigned char * uuid) const = 0; + + /** + * @brief Whether the shape to be filled or only its borders to be put on map. + * Empty virtual method intended to be used in child implementations + * @return True if shape to be filled + */ + virtual bool isFill() const = 0; + + /** + * @brief Supporting routine obtaining ROS-parameters for the given vector object. + * Empty virtual method intended to be used in child implementations + * @param shape_name Name of the shape + * @return True if all parameters were obtained or false in failure case + */ + virtual bool obtainParams(const std::string & shape_name) = 0; + + /** + * @brief Transforms shape coordinates to a new frame. + * Empty virtual method intended to be used in child implementations + * @param to_frame Frame ID to transform to + * @param tf_buffer TF buffer to use for the transformation + * @param transform_tolerance Transform tolerance + * @return Whether it was transformed successfully + */ + virtual bool toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance) = 0; + + /** + * @brief Gets shape box-boundaries. + * Empty virtual method intended to be used in child implementations + * @param min_x output min X-boundary of shape + * @param min_y output min Y-boundary of shape + * @param max_x output max X-boundary of shape + * @param max_y output max Y-boundary of shape + */ + virtual void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) = 0; + + /** + * @brief Is the point inside the shape. + * Empty virtual method intended to be used in child implementations + * @param px X-coordinate of the given point to check + * @param py Y-coordinate of the given point to check + * @return True if given point inside the shape + */ + virtual bool isPointInside(const double px, const double py) const = 0; + + /** + * @brief Puts shape borders on map. + * Empty virtual method intended to be used in child implementations + * @param map Output map pointer + * @param overlay_type Overlay type + */ + virtual void putBorders( + nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) = 0; + +protected: + /// @brief Type of shape + ShapeType type_; + + /// @brief VectorObjectServer node + nav2::LifecycleNode::WeakPtr node_; +}; + +/// @brief Polygon shape class +class Polygon : public Shape +{ +public: + /* + * @brief Polygon class constructor + * @param node Vector Object server node pointer + * @note setParams()/obtainParams() should be called after to configure the shape + */ + explicit Polygon(const nav2::LifecycleNode::WeakPtr & node); + + /** + * @brief Gets the value of the shape. + * @return OccupancyGrid value of the shape + */ + int8_t getValue() const; + + /** + * @brief Gets frame ID of the shape. + * @return Frame ID where the shape is + */ + std::string getFrameID() const; + + /** + * @brief Gets UUID of the shape. + * @return Shape UUID string + */ + std::string getUUID() const; + + /** + * @brief Checks whether the shape is equal to a given UUID. + * @param uuid Given UUID to check with + * @return True if the shape has the same as given UUID, otherwise false + */ + bool isUUID(const unsigned char * uuid) const; + + /** + * @brief Whether the shape to be filled or only its borders to be put on map. + * @return True if shape to be filled + */ + bool isFill() const; + + /** + * @brief Supporting routine obtaining ROS-parameters for the given vector object. + * @param shape_name Name of the shape + * @return True if all parameters were obtained or false in failure case + */ + bool obtainParams(const std::string & shape_name); + + /** + * @brief Gets Polygon parameters + * @return Polygon parameters + */ + nav2_msgs::msg::PolygonObject::SharedPtr getParams() const; + + /** + * @brief Tries to update Polygon parameters + * @return False in case of inconsistent shape + */ + bool setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params); + + /** + * @brief Transforms shape coordinates to a new frame. + * @param to_frame Frame ID to transform to + * @param tf_buffer TF buffer to use for the transformation + * @param transform_tolerance Transform tolerance + * @return Whether it was transformed successfully + */ + bool toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance); + + /** + * @brief Gets shape box-boundaries + * @param min_x output min X-boundary of shape + * @param min_y output min Y-boundary of shape + * @param max_x output max X-boundary of shape + * @param max_y output max Y-boundary of shape + */ + void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y); + + /** + * @brief Is the point inside the shape. + * @param px X-coordinate of the given point to check + * @param py Y-coordinate of the given point to check + * @return True if given point inside the shape + */ + bool isPointInside(const double px, const double py) const; + + /** + * @brief Puts shape borders on map. + * @param map Output map pointer + * @param overlay_type Overlay type + */ + void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type); + +protected: + /** + * @brief Checks that shape is consistent for further operation + * @return False in case of inconsistent shape + */ + bool checkConsistency(); + + /// @brief Input polygon parameters (could be in any frame) + nav2_msgs::msg::PolygonObject::SharedPtr params_; + /// @brief Polygon in the map's frame + geometry_msgs::msg::Polygon::SharedPtr polygon_; +}; + +/// @brief Circle shape class +class Circle : public Shape +{ +public: + /* + * @brief Circle class constructor + * @param node Vector Object server node pointer + * @note setParams()/obtainParams() should be called after to configure the shape + */ + explicit Circle(const nav2::LifecycleNode::WeakPtr & node); + + /** + * @brief Gets the value of the shape. + * @return OccupancyGrid value of the shape + */ + int8_t getValue() const; + + /** + * @brief Gets frame ID of the shape. + * @return Frame ID where the shape is + */ + std::string getFrameID() const; + + /** + * @brief Gets UUID of the shape. + * @return Shape UUID string + */ + std::string getUUID() const; + + /** + * @brief Checks whether the shape is equal to a given UUID. + * @param uuid Given UUID to check with + * @return True if the shape has the same as given UUID, otherwise false + */ + bool isUUID(const unsigned char * uuid) const; + + /** + * @brief Whether the shape to be filled or only its borders to be put on map. + * @return True if shape to be filled + */ + bool isFill() const; + + /** + * @brief Supporting routine obtaining ROS-parameters for the given vector object. + * @param shape_name Name of the shape + * @return True if all parameters were obtained or false in failure case + */ + bool obtainParams(const std::string & shape_name); + + /** + * @brief Gets Circle parameters + * @return Circle parameters + */ + nav2_msgs::msg::CircleObject::SharedPtr getParams() const; + + /** + * @brief Tries to update Circle parameters + * @return False in case of inconsistent shape + */ + bool setParams(const nav2_msgs::msg::CircleObject::SharedPtr params); + + /** + * @brief Transforms shape coordinates to a new frame. + * @param to_frame Frame ID to transform to + * @param tf_buffer TF buffer to use for the transformation + * @param transform_tolerance Transform tolerance + * @return Whether it was transformed successfully + */ + bool toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance); + + /** + * @brief Gets shape box-boundaries + * @param min_x output min X-boundary of shape + * @param min_y output min Y-boundary of shape + * @param max_x output max X-boundary of shape + * @param max_y output max Y-boundary of shape + */ + void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y); + + /** + * @brief Is the point inside the shape. + * @param px X-coordinate of the given point to check + * @param py Y-coordinate of the given point to check + * @return True if given point inside the shape + */ + bool isPointInside(const double px, const double py) const; + + /** + * @brief Puts shape borders on map. + * @param map Output map pointer + * @param overlay_type Overlay type + */ + void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type); + +protected: + /** + * @brief Checks that shape is consistent for further operation + * @return False in case of inconsistent shape + */ + bool checkConsistency(); + + /** + * @brief Converts circle center to map coordinates + * considering FP-accuracy losing on small values when using conventional + * nav2_util::worldToMap() approach + * @param map Map pointer + * @param mcx Output X-coordinate of associated circle center on map + * @param mcy Output Y-coordinate of associated circle center on map + * @return True if the conversion was successful + */ + bool centerToMap( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + unsigned int & mcx, unsigned int & mcy); + + /** + * @brief Put Circle's point on map + * @param mx X-coordinate of given point + * @param my Y-coordinate of given point + * @param map Output map pointer + * @param overlay_type Overlay type + */ + void putPoint( + unsigned int mx, unsigned int my, + nav_msgs::msg::OccupancyGrid::SharedPtr map, + const OverlayType overlay_type); + + /// @brief Input circle parameters (could be in any frame) + nav2_msgs::msg::CircleObject::SharedPtr params_; + /// @brief Circle center in the map's frame + geometry_msgs::msg::Point32::SharedPtr center_; +}; + +} // namespace nav2_map_server + +#endif // NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_ diff --git a/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp new file mode 100644 index 00000000000..f478f71b3ea --- /dev/null +++ b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp @@ -0,0 +1,142 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_ +#define NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/node_utils.hpp" +#include "nav2_util/occ_grid_values.hpp" + +namespace nav2_map_server +{ + +// ---------- Working with UUID-s ---------- + +/** + * @brief Converts UUID from input array to unparsed string + * @param uuid Input UUID in array format + * @return Unparsed UUID string + */ +inline std::string unparseUUID(const unsigned char * uuid) +{ + char uuid_str[37]; + uuid_unparse(uuid, uuid_str); + return std::string(uuid_str); +} + +// ---------- Working with shapes' overlays ---------- + +/// @brief Type of overlay between different vector objects and map +enum class OverlayType : uint8_t +{ + OVERLAY_SEQ = 0, // Vector objects are superimposed in the order in which they have arrived + OVERLAY_MAX = 1, // Maximum value from vector objects and map is being chosen + OVERLAY_MIN = 2 // Minimum value from vector objects and map is being chosen +}; + +/** + * @brief Updates map value with shape's one according to the given overlay type + * @param map_val Map value. To be updated with new value if overlay is involved + * @param shape_val Vector object value to be overlaid on map + * @param overlay_type Type of overlay + * @throw std::exception in case of unknown overlay type + */ +inline void processVal( + int8_t & map_val, const int8_t shape_val, + const OverlayType overlay_type) +{ + switch (overlay_type) { + case OverlayType::OVERLAY_SEQ: + map_val = shape_val; + return; + case OverlayType::OVERLAY_MAX: + if (shape_val > map_val) { + map_val = shape_val; + } + return; + case OverlayType::OVERLAY_MIN: + if ((map_val == nav2_util::OCC_GRID_UNKNOWN || shape_val < map_val) && + shape_val != nav2_util::OCC_GRID_UNKNOWN) + { + map_val = shape_val; + } + return; + default: + throw std::runtime_error{"Unknown overlay type"}; + } +} + +/** + * @brief Updates the cell on the map with given shape value according to the given overlay type + * @param map Output map to be updated with + * @param offset Offset to the cell to be updated + * @param shape_val Vector object value to be updated map with + * @param overlay_type Type of overlay + */ +inline void processCell( + nav_msgs::msg::OccupancyGrid::SharedPtr map, + const unsigned int offset, + const int8_t shape_val, + const OverlayType overlay_type) +{ + int8_t map_val = map->data[offset]; + processVal(map_val, shape_val, overlay_type); + map->data[offset] = map_val; +} + +/// @brief Functor class used in raytraceLine algorithm +class MapAction +{ +public: + /** + * @brief MapAction constructor + * @param map Pointer to output map + * @param value Value to put on map + * @param overlay_type Overlay type + */ + MapAction( + nav_msgs::msg::OccupancyGrid::SharedPtr map, + int8_t value, OverlayType overlay_type) + : map_(map), value_(value), overlay_type_(overlay_type) + {} + + /** + * @brief Map' cell updating operator + * @param offset Offset on the map where the cell to be changed + */ + inline void operator()(unsigned int offset) + { + processCell(map_, offset, value_, overlay_type_); + } + +protected: + /// @brief Output map pointer + nav_msgs::msg::OccupancyGrid::SharedPtr map_; + /// @brief Value to put on map + int8_t value_; + /// @brief Overlay type + OverlayType overlay_type_; +}; + +} // namespace nav2_map_server + +#endif // NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_ diff --git a/nav2_map_server/launch/vector_object_server.launch.py b/nav2_map_server/launch/vector_object_server.launch.py new file mode 100644 index 00000000000..f3c2a20ff1b --- /dev/null +++ b/nav2_map_server/launch/vector_object_server.launch.py @@ -0,0 +1,169 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2023 Samsung R&D Institute Russia +# +# 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, GroupAction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, NotEqualsSubstitution, PythonExpression +from launch_ros.actions import LoadComposableNodes, Node, PushROSNamespace, SetParameter +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description() -> LaunchDescription: + # Environment + package_dir = get_package_share_directory('nav2_map_server') + + # Constant parameters + lifecycle_nodes = ['vector_object_server', 'costmap_filter_info_server'] + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Launch arguments + # 1. Create the launch configuration variables + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + params_file = LaunchConfiguration('params_file') + autostart = LaunchConfiguration('autostart') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + + # 2. Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='True', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(package_dir, 'params', 'vector_object_server_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 Vector Object server') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='True', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of container that nodes will load in if use composition') + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + convert_types=True, + ), + allow_substs=True, + ) + + # Declare node launching commands + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + SetParameter('use_sim_time', use_sim_time), + PushROSNamespace( + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), + namespace=namespace), + Node( + package='nav2_map_server', + executable='vector_object_server', + name='vector_object_server', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_map_server', + executable='costmap_filter_info_server', + name='costmap_filter_info_server', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_vo_server', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[{'autostart': autostart}, + {'node_names': lifecycle_nodes}], + remappings=remappings) + ] + ) + + load_composable_nodes = GroupAction( + condition=IfCondition(use_composition), + actions=[ + SetParameter('use_sim_time', use_sim_time), + PushROSNamespace( + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), + namespace=namespace), + LoadComposableNodes( + target_container=container_name, + composable_node_descriptions=[ + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::VectorObjectServer', + name='vector_object_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::CostmapFilterInfoServer', + name='costmap_filter_info_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_vo_server', + parameters=[{'autostart': autostart}, + {'node_names': lifecycle_nodes}], + remappings=remappings) + ], + ) + ] + ) + + ld = LaunchDescription() + + # Launch arguments + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_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_container_name_cmd) + + # Node launching commands + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index f6d5f75a278..44045314fa3 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -14,6 +14,7 @@ nav2_common + backward_ros rclcpp_lifecycle nav_msgs std_msgs @@ -21,12 +22,15 @@ yaml_cpp_vendor launch_ros launch_testing + geometry_msgs tf2 + tf2_ros nav2_msgs nav2_util graphicsmagick lifecycle_msgs nav2_ros_common + uuid ament_lint_common ament_lint_auto diff --git a/nav2_map_server/params/vector_object_server_params.yaml b/nav2_map_server/params/vector_object_server_params.yaml new file mode 100644 index 00000000000..6a6a5fc2b68 --- /dev/null +++ b/nav2_map_server/params/vector_object_server_params.yaml @@ -0,0 +1,30 @@ +vector_object_server: + ros__parameters: + map_topic: "vo_map" + global_frame_id: "map" + resolution: 0.05 + default_value: -1 + overlay_type: 0 + update_frequency: 1.0 + transform_tolerance: 0.1 + shapes: ["Poly", "Circle"] + Poly: + type: "polygon" + frame_id: "map" + closed: True + value: 100 + points: [0.3, 0.5, -0.4, 1.2, -0.4, -0.2] + Circle: + type: "circle" + frame_id: "map" + fill: True + value: 100 + center: [1.5, 0.5] + radius: 0.7 +costmap_filter_info_server: + ros__parameters: + type: 0 + filter_info_topic: "vo_costmap_filter_info" + mask_topic: "vo_map" + base: 0.0 + multiplier: 1.0 diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index e95d192a9af..989f38f1211 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -594,13 +594,15 @@ void tryWriteMapToFile( std::string image_name = mapdatafile.substr(file_name_index + 1); YAML::Emitter e; - e << YAML::Precision(3); + e << YAML::Precision(7); e << YAML::BeginMap; e << YAML::Key << "image" << YAML::Value << image_name; e << YAML::Key << "mode" << YAML::Value << map_mode_to_string(save_parameters.mode); - e << YAML::Key << "resolution" << YAML::Value << map.info.resolution; - e << YAML::Key << "origin" << YAML::Flow << YAML::BeginSeq << map.info.origin.position.x << - map.info.origin.position.y << yaw << YAML::EndSeq; + e << YAML::Key << "resolution" << YAML::Value << to_string_with_precision(map.info.resolution, + 3); + e << YAML::Key << "origin" << YAML::Flow << YAML::BeginSeq << + to_string_with_precision(map.info.origin.position.x, 3) << + to_string_with_precision(map.info.origin.position.y, 3) << yaw << YAML::EndSeq; e << YAML::Key << "negate" << YAML::Value << 0; if (save_parameters.mode == MapMode::Trinary) { @@ -611,8 +613,10 @@ void tryWriteMapToFile( e << YAML::Key << "occupied_thresh" << YAML::Value << 0.65; e << YAML::Key << "free_thresh" << YAML::Value << 0.196; } else { - e << YAML::Key << "occupied_thresh" << YAML::Value << save_parameters.occupied_thresh; - e << YAML::Key << "free_thresh" << YAML::Value << save_parameters.free_thresh; + e << YAML::Key << "occupied_thresh" << YAML::Value << + to_string_with_precision(save_parameters.occupied_thresh, 3); + e << YAML::Key << "free_thresh" << YAML::Value << + to_string_with_precision(save_parameters.free_thresh, 3); } if (!e.good()) { @@ -648,4 +652,12 @@ bool saveMapToFile( return true; } +std::string to_string_with_precision(double value, int precision) +{ + std::ostringstream out; + out << std::fixed << std::setprecision(precision) << value; + + return out.str(); +} + } // namespace nav2_map_server diff --git a/nav2_map_server/src/vo_server/main.cpp b/nav2_map_server/src/vo_server/main.cpp new file mode 100644 index 00000000000..d2352b7aad3 --- /dev/null +++ b/nav2_map_server/src/vo_server/main.cpp @@ -0,0 +1,28 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 "nav2_map_server/vector_object_server.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp new file mode 100644 index 00000000000..89d7c3c825f --- /dev/null +++ b/nav2_map_server/src/vo_server/vector_object_server.cpp @@ -0,0 +1,558 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_map_server/vector_object_server.hpp" + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/create_timer.hpp" + +#include "tf2_ros/create_timer_ros.hpp" + +#include "nav2_util/occ_grid_utils.hpp" +#include "nav2_util/occ_grid_values.hpp" + +using namespace std::placeholders; + +namespace nav2_map_server +{ + +VectorObjectServer::VectorObjectServer(const rclcpp::NodeOptions & options) +: nav2::LifecycleNode("vector_object_server", "", options), process_map_(false) +{} + +nav2::CallbackReturn +VectorObjectServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + // Transform buffer and listener initialization + tf_buffer_ = std::make_shared(this->get_clock()); + auto timer_interface = std::make_shared( + this->get_node_base_interface(), + this->get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + tf_listener_ = std::make_shared(*tf_buffer_); + + // Obtaining ROS parameters + if (!obtainParams()) { + return nav2::CallbackReturn::FAILURE; + } + + map_pub_ = create_publisher( + map_topic_, + nav2::qos::LatchedPublisherQoS()); + + add_shapes_service_ = create_service( + "~/add_shapes", + std::bind(&VectorObjectServer::addShapesCallback, this, _1, _2, _3)); + + get_shapes_service_ = create_service( + "~/get_shapes", + std::bind(&VectorObjectServer::getShapesCallback, this, _1, _2, _3)); + + remove_shapes_service_ = create_service( + "~/remove_shapes", + std::bind(&VectorObjectServer::removeShapesCallback, this, _1, _2, _3)); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +VectorObjectServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + map_pub_->on_activate(); + + // Trigger map to be published + process_map_ = true; + switchMapUpdate(); + + // Creating bond connection + createBond(); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +VectorObjectServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + if (map_timer_) { + map_timer_->cancel(); + map_timer_.reset(); + } + process_map_ = false; + + map_pub_->on_deactivate(); + + // Destroying bond connection + destroyBond(); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +VectorObjectServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + add_shapes_service_.reset(); + get_shapes_service_.reset(); + remove_shapes_service_.reset(); + + map_pub_.reset(); + map_.reset(); + + shapes_.clear(); + + tf_listener_.reset(); + tf_buffer_.reset(); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +VectorObjectServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2::CallbackReturn::SUCCESS; +} + +bool VectorObjectServer::obtainParams() +{ + auto node = shared_from_this(); + + // Main ROS-parameters + map_topic_ = nav2::declare_or_get_parameter(node, "map_topic", std::string{"vo_map"}); + global_frame_id_ = nav2::declare_or_get_parameter(node, "global_frame_id", std::string{"map"}); + resolution_ = nav2::declare_or_get_parameter(node, "resolution", 0.05); + default_value_ = nav2::declare_or_get_parameter(node, "default_value", + static_cast(nav2_util::OCC_GRID_UNKNOWN)); + overlay_type_ = static_cast(nav2::declare_or_get_parameter(node, "overlay_type", + static_cast(OverlayType::OVERLAY_SEQ))); + update_frequency_ = nav2::declare_or_get_parameter(node, "update_frequency", 1.0); + transform_tolerance_ = nav2::declare_or_get_parameter(node, "transform_tolerance", 0.1); + + // Shapes + auto shape_names = nav2::declare_or_get_parameter(node, "shapes", std::vector()); + for (std::string shape_name : shape_names) { + std::string shape_type; + try { + shape_type = nav2::declare_or_get_parameter(node, shape_name + ".type", + rclcpp::PARAMETER_STRING); + } catch (const std::exception & ex) { + RCLCPP_ERROR( + get_logger(), "Error while getting shape %s type: %s", shape_name.c_str(), ex.what()); + return false; + } + + if (shape_type == "polygon") { + auto polygon = std::make_shared(node); + if (!polygon->obtainParams(shape_name)) { + return false; + } + shapes_.push_back(polygon); + } else if (shape_type == "circle") { + auto circle = std::make_shared(node); + if (!circle->obtainParams(shape_name)) { + return false; + } + shapes_.push_back(circle); + } else { + RCLCPP_ERROR(get_logger(), + "Please specify the correct type for shape %s. Supported types are 'polygon' and 'circle'", + shape_name.c_str()); + return false; + } + } + + return true; +} + +std::vector>::iterator +VectorObjectServer::findShape(const unsigned char * uuid) +{ + for (auto it = shapes_.begin(); it != shapes_.end(); it++) { + if ((*it)->isUUID(uuid)) { + return it; + } + } + return shapes_.end(); +} + +bool VectorObjectServer::transformVectorObjects() +{ + for (auto shape : shapes_) { + if (shape->getFrameID() != global_frame_id_ && !shape->getFrameID().empty()) { + // Shape to be updated dynamically + if (!shape->toFrame(global_frame_id_, tf_buffer_, transform_tolerance_)) { + RCLCPP_ERROR( + get_logger(), "Can not transform vector object from %s to %s frame", + shape->getFrameID().c_str(), global_frame_id_.c_str()); + return false; + } + } + } + + return true; +} + +void VectorObjectServer::getMapBoundaries( + double & min_x, double & min_y, double & max_x, double & max_y) const +{ + min_x = std::numeric_limits::max(); + min_y = std::numeric_limits::max(); + max_x = std::numeric_limits::lowest(); + max_y = std::numeric_limits::lowest(); + + double min_p_x, min_p_y, max_p_x, max_p_y; + for (auto shape : shapes_) { + shape->getBoundaries(min_p_x, min_p_y, max_p_x, max_p_y); + min_x = std::min(min_x, min_p_x); + min_y = std::min(min_y, min_p_y); + max_x = std::max(max_x, max_p_x); + max_y = std::max(max_y, max_p_y); + } + + if ( + min_x == std::numeric_limits::max() || + min_y == std::numeric_limits::max() || + max_x == std::numeric_limits::lowest() || + max_y == std::numeric_limits::lowest()) + { + throw std::runtime_error("Can not obtain map boundaries"); + } +} + +void VectorObjectServer::updateMap( + const double & min_x, const double & min_y, const double & max_x, const double & max_y) +{ + // Calculate size of update map + int size_x = static_cast((max_x - min_x) / resolution_) + 1; + int size_y = static_cast((max_y - min_y) / resolution_) + 1; + + if (size_x < 0) { + throw std::runtime_error("Incorrect map x-size"); + } + + if (size_y < 0) { + throw std::runtime_error("Incorrect map y-size"); + } + + if (!map_) { + map_ = std::make_shared(); + } + + if ( + map_->info.width != static_cast(size_x) || + map_->info.height != static_cast(size_y)) + { + // Map size was changed + map_->data = std::vector(size_x * size_y, default_value_); + map_->info.width = size_x; + map_->info.height = size_y; + } else if (size_x > 0 && size_y > 0) { + // Map size was not changed + memset(map_->data.data(), default_value_, size_x * size_y * sizeof(int8_t)); + } + + map_->header.frame_id = global_frame_id_; + map_->info.resolution = resolution_; + map_->info.origin.position.x = min_x; + map_->info.origin.position.y = min_y; +} + +void VectorObjectServer::putVectorObjectsOnMap() +{ + // Filling the shapes + for (auto shape : shapes_) { + if (shape->isFill()) { + // Put filled shape on map + double wx1 = std::numeric_limits::max(); + double wy1 = std::numeric_limits::max(); + double wx2 = std::numeric_limits::lowest(); + double wy2 = std::numeric_limits::lowest(); + unsigned int mx1 = 0; + unsigned int my1 = 0; + unsigned int mx2 = 0; + unsigned int my2 = 0; + + shape->getBoundaries(wx1, wy1, wx2, wy2); + if ( + !nav2_util::worldToMap(map_, wx1, wy1, mx1, my1) || + !nav2_util::worldToMap(map_, wx2, wy2, mx2, my2)) + { + RCLCPP_ERROR( + get_logger(), + "Error to get shape boundaries on map (UUID: %s)", shape->getUUID().c_str()); + return; + } + + unsigned int it; + for (unsigned int my = my1; my <= my2; my++) { + for (unsigned int mx = mx1; mx <= mx2; mx++) { + it = my * map_->info.width + mx; + double wx, wy; + nav2_util::mapToWorld(map_, mx, my, wx, wy); + if (shape->isPointInside(wx, wy)) { + processVal(map_->data[it], shape->getValue(), overlay_type_); + } + } + } + } else { + // Put shape borders on map + shape->putBorders(map_, overlay_type_); + } + } +} + +void VectorObjectServer::publishMap() +{ + if (map_) { + auto map = std::make_unique(*map_); + map_pub_->publish(std::move(map)); + } +} + +void VectorObjectServer::processMap() +{ + if (!process_map_) { + return; + } + + try { + if (shapes_.size() > 0) { + if (!transformVectorObjects()) { + return; + } + double min_x, min_y, max_x, max_y; + + getMapBoundaries(min_x, min_y, max_x, max_y); + updateMap(min_x, min_y, max_x, max_y); + putVectorObjectsOnMap(); + } else { + updateMap(0.0, 0.0, 0.0, 0.0); + } + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Can not update map: %s", ex.what()); + return; + } + + publishMap(); +} + +void VectorObjectServer::switchMapUpdate() +{ + for (auto shape : shapes_) { + if (shape->getFrameID() != global_frame_id_ && !shape->getFrameID().empty()) { + if (!map_timer_) { + map_timer_ = this->create_timer( + std::chrono::duration(1.0 / update_frequency_), + std::bind(&VectorObjectServer::processMap, this)); + } + RCLCPP_INFO(get_logger(), "Publishing map dynamically at %f Hz rate", update_frequency_); + return; + } + } + + if (map_timer_) { + map_timer_->cancel(); + map_timer_.reset(); + } + RCLCPP_INFO(get_logger(), "Publishing map once"); + processMap(); +} + +void VectorObjectServer::addShapesCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response) +{ + // Initialize result with true. If one of the required vector object was not added properly, + // set it to false. + response->success = true; + + auto node = shared_from_this(); + + // Process polygons + for (auto req_poly : request->polygons) { + nav2_msgs::msg::PolygonObject::SharedPtr new_params = + std::make_shared(req_poly); + + auto it = findShape(new_params->uuid.uuid.data()); + if (it != shapes_.end()) { + // Vector Object with given UUID was found: updating it + // Check that found shape has correct type + if ((*it)->getType() != POLYGON) { + RCLCPP_ERROR( + get_logger(), + "Shape (UUID: %s) is not a polygon type for a polygon update. Not adding shape.", + (*it)->getUUID().c_str()); + response->success = false; + // Do not add this shape + continue; + } + + std::shared_ptr polygon = std::static_pointer_cast(*it); + + // Preserving old parameters for the case, if new ones to be incorrect + nav2_msgs::msg::PolygonObject::SharedPtr old_params = polygon->getParams(); + if (!polygon->setParams(new_params)) { + RCLCPP_ERROR( + get_logger(), + "Failed to update existing polygon object (UUID: %s) with new params. " + "Reverting to old polygon params.", + (*it)->getUUID().c_str()); + // Restore old parameters + polygon->setParams(old_params); + // ... and set the failure to return + response->success = false; + } + } else { + // Vector Object with given UUID was not found: creating a new one + std::shared_ptr polygon = std::make_shared(node); + if (polygon->setParams(new_params)) { + shapes_.push_back(polygon); + } else { + RCLCPP_ERROR( + get_logger(), "Failed to create a new polygon object using the provided params."); + response->success = false; + } + } + } + + // Process circles + for (auto req_crcl : request->circles) { + nav2_msgs::msg::CircleObject::SharedPtr new_params = + std::make_shared(req_crcl); + + auto it = findShape(new_params->uuid.uuid.data()); + if (it != shapes_.end()) { + // Vector object with given UUID was found: updating it + // Check that found shape has correct type + if ((*it)->getType() != CIRCLE) { + RCLCPP_ERROR( + get_logger(), + "Shape (UUID: %s) is not a circle type for a circle update. Not adding shape.", + (*it)->getUUID().c_str()); + response->success = false; + // Do not add this shape + continue; + } + + std::shared_ptr circle = std::static_pointer_cast(*it); + + // Preserving old parameters for the case, if new ones to be incorrect + nav2_msgs::msg::CircleObject::SharedPtr old_params = circle->getParams(); + if (!circle->setParams(new_params)) { + RCLCPP_ERROR( + get_logger(), + "Failed to update existing circle object (UUID: %s) with new params. " + "Reverting to old circle params.", + (*it)->getUUID().c_str()); + // Restore old parameters + circle->setParams(old_params); + // ... and set the failure to return + response->success = false; + } + } else { + // Vector Object with given UUID was not found: creating a new one + std::shared_ptr circle = std::make_shared(node); + if (circle->setParams(new_params)) { + shapes_.push_back(circle); + } else { + RCLCPP_ERROR( + get_logger(), "Failed to create a new circle object using the provided params."); + response->success = false; + } + } + } + + switchMapUpdate(); +} + +void VectorObjectServer::getShapesCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response) +{ + std::shared_ptr polygon; + std::shared_ptr circle; + + for (auto shape : shapes_) { + switch (shape->getType()) { + case POLYGON: + polygon = std::static_pointer_cast(shape); + response->polygons.push_back(*(polygon->getParams())); + break; + case CIRCLE: + circle = std::static_pointer_cast(shape); + response->circles.push_back(*(circle->getParams())); + break; + default: + RCLCPP_WARN(get_logger(), "Unknown shape type (UUID: %s)", shape->getUUID().c_str()); + } + } +} + +void VectorObjectServer::removeShapesCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response) +{ + // Initialize result with true. If one of the required vector object was not found, + // set it to false. + response->success = true; + + if (request->all_objects) { + // Clear all objects + shapes_.clear(); + } else { + // Find objects to remove + for (auto req_uuid : request->uuids) { + auto it = findShape(req_uuid.uuid.data()); + if (it != shapes_.end()) { + // Shape with given UUID was found: remove it + (*it).reset(); + shapes_.erase(it); + } else { + // Required vector object was not found + RCLCPP_ERROR( + get_logger(), + "Can not find shape to remove with UUID: %s", + unparseUUID(req_uuid.uuid.data()).c_str()); + response->success = false; + } + } + } + + switchMapUpdate(); +} + +} // namespace nav2_map_server + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_map_server::VectorObjectServer) diff --git a/nav2_map_server/src/vo_server/vector_object_shapes.cpp b/nav2_map_server/src/vo_server/vector_object_shapes.cpp new file mode 100644 index 00000000000..72466486c01 --- /dev/null +++ b/nav2_map_server/src/vo_server/vector_object_shapes.cpp @@ -0,0 +1,559 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_map_server/vector_object_shapes.hpp" + +#include +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "nav2_util/occ_grid_utils.hpp" +#include "nav2_util/occ_grid_values.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/raytrace_line_2d.hpp" +#include "nav2_util/robot_utils.hpp" + +namespace nav2_map_server +{ + +// ---------- Shape ---------- + +Shape::Shape(const nav2::LifecycleNode::WeakPtr & node) +: type_(UNKNOWN), node_(node) +{} + +Shape::~Shape() +{} + +ShapeType Shape::getType() +{ + return type_; +} + +bool Shape::obtainShapeUUID(const std::string & shape_name, unsigned char * out_uuid) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + try { + // Try to get shape UUID from ROS-parameters + std::string uuid_str = nav2::declare_or_get_parameter( + node, shape_name + ".uuid", rclcpp::PARAMETER_STRING); + if (uuid_parse(uuid_str.c_str(), out_uuid) != 0) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Can not parse UUID string for shape: %s", + shape_name.c_str(), uuid_str.c_str()); + return false; + } + } catch (const std::exception &) { + // If no UUID was specified, generate a new one + uuid_generate(out_uuid); + + char uuid_str[37]; + uuid_unparse(out_uuid, uuid_str); + RCLCPP_INFO( + node->get_logger(), + "[%s] No UUID is specified for shape. Generating a new one: %s", + shape_name.c_str(), uuid_str); + } + + return true; +} + +// ---------- Polygon ---------- + +Polygon::Polygon( + const nav2::LifecycleNode::WeakPtr & node) +: Shape::Shape(node) +{ + type_ = POLYGON; +} + +int8_t Polygon::getValue() const +{ + return params_->value; +} + +std::string Polygon::getFrameID() const +{ + return params_->header.frame_id; +} + +std::string Polygon::getUUID() const +{ + return unparseUUID(params_->uuid.uuid.data()); +} + +bool Polygon::isUUID(const unsigned char * uuid) const +{ + return uuid_compare(params_->uuid.uuid.data(), uuid) == 0; +} + +bool Polygon::isFill() const +{ + return params_->closed; +} + +bool Polygon::obtainParams(const std::string & shape_name) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!params_) { + params_ = std::make_shared(); + } + if (!polygon_) { + polygon_ = std::make_shared(); + } + + params_->header.frame_id = nav2::declare_or_get_parameter( + node, shape_name + ".frame_id", std::string{"map"}); + params_->value = nav2::declare_or_get_parameter( + node, shape_name + ".value", static_cast(nav2_util::OCC_GRID_OCCUPIED)); + params_->closed = nav2::declare_or_get_parameter( + node, shape_name + ".closed", true); + + std::vector poly_row; + try { + poly_row = nav2::declare_or_get_parameter>( + node, shape_name + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY); + } catch (const std::exception & ex) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Error while getting polygon parameters: %s", + shape_name.c_str(), ex.what()); + return false; + } + // Check for points format correctness + if (poly_row.size() < 6 || poly_row.size() % 2 != 0) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Polygon has incorrect points description", + shape_name.c_str()); + return false; + } + + // Obtain polygon vertices + geometry_msgs::msg::Point32 point; + bool first = true; + for (double val : poly_row) { + if (first) { + point.x = val; + } else { + point.y = val; + params_->points.push_back(point); + } + first = !first; + } + + // Filling the polygon_ with obtained points in map's frame + polygon_->points = params_->points; + + // Getting shape UUID + return obtainShapeUUID(shape_name, params_->uuid.uuid.data()); +} + +nav2_msgs::msg::PolygonObject::SharedPtr Polygon::getParams() const +{ + return params_; +} + +bool Polygon::setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params) +{ + params_ = params; + + if (!polygon_) { + polygon_ = std::make_shared(); + } + polygon_->points = params_->points; + + // If no UUID was specified, generate a new one + if (uuid_is_null(params_->uuid.uuid.data())) { + uuid_generate(params_->uuid.uuid.data()); + } + + return checkConsistency(); +} + +bool Polygon::toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance) +{ + geometry_msgs::msg::PoseStamped from_pose, to_pose; + from_pose.header = params_->header; + for (unsigned int i = 0; i < params_->points.size(); i++) { + from_pose.pose.position.x = params_->points[i].x; + from_pose.pose.position.y = params_->points[i].y; + from_pose.pose.position.z = params_->points[i].z; + if ( + nav2_util::transformPoseInTargetFrame( + from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance)) + { + polygon_->points[i].x = to_pose.pose.position.x; + polygon_->points[i].y = to_pose.pose.position.y; + polygon_->points[i].z = to_pose.pose.position.z; + } else { + return false; + } + } + + return true; +} + +void Polygon::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) +{ + min_x = std::numeric_limits::max(); + min_y = std::numeric_limits::max(); + max_x = std::numeric_limits::lowest(); + max_y = std::numeric_limits::lowest(); + + for (auto point : polygon_->points) { + min_x = std::min(min_x, static_cast(point.x)); + min_y = std::min(min_y, static_cast(point.y)); + max_x = std::max(max_x, static_cast(point.x)); + max_y = std::max(max_y, static_cast(point.y)); + } +} + +bool Polygon::isPointInside(const double px, const double py) const +{ + return nav2_util::geometry_utils::isPointInsidePolygon(px, py, polygon_->points); +} + +void Polygon::putBorders( + nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) +{ + unsigned int mx0, my0, mx1, my1; + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!nav2_util::worldToMap(map, polygon_->points[0].x, polygon_->points[0].y, mx1, my1)) { + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Can not convert (%f, %f) point to map", + getUUID().c_str(), polygon_->points[0].x, polygon_->points[0].y); + return; + } + + MapAction ma(map, params_->value, overlay_type); + for (unsigned int i = 1; i < polygon_->points.size(); i++) { + mx0 = mx1; + my0 = my1; + if (!nav2_util::worldToMap(map, polygon_->points[i].x, polygon_->points[i].y, mx1, my1)) { + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Can not convert (%f, %f) point to map", + getUUID().c_str(), polygon_->points[i].x, polygon_->points[i].y); + return; + } + nav2_util::raytraceLine(ma, mx0, my0, mx1, my1, map->info.width); + } +} + +bool Polygon::checkConsistency() +{ + if (params_->points.size() < 3) { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Polygon has incorrect number of vertices: %li", + getUUID().c_str(), params_->points.size()); + return false; + } + + return true; +} + +// ---------- Circle ---------- + +Circle::Circle( + const nav2::LifecycleNode::WeakPtr & node) +: Shape::Shape(node) +{ + type_ = CIRCLE; +} + +int8_t Circle::getValue() const +{ + return params_->value; +} + +std::string Circle::getFrameID() const +{ + return params_->header.frame_id; +} + +std::string Circle::getUUID() const +{ + return unparseUUID(params_->uuid.uuid.data()); +} + +bool Circle::isUUID(const unsigned char * uuid) const +{ + return uuid_compare(params_->uuid.uuid.data(), uuid) == 0; +} + +bool Circle::isFill() const +{ + return params_->fill; +} + +bool Circle::obtainParams(const std::string & shape_name) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!params_) { + params_ = std::make_shared(); + } + if (!center_) { + center_ = std::make_shared(); + } + + params_->header.frame_id = nav2::declare_or_get_parameter( + node, shape_name + ".frame_id", std::string{"map"}); + params_->value = nav2::declare_or_get_parameter( + node, shape_name + ".value", static_cast(nav2_util::OCC_GRID_OCCUPIED)); + params_->fill = nav2::declare_or_get_parameter( + node, shape_name + ".fill", true); + + std::vector center_row; + try { + center_row = nav2::declare_or_get_parameter>( + node, shape_name + ".center", rclcpp::PARAMETER_DOUBLE_ARRAY); + params_->radius = nav2::declare_or_get_parameter( + node, shape_name + ".radius", rclcpp::PARAMETER_DOUBLE); + if (params_->radius < 0) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Circle has incorrect radius less than zero", + shape_name.c_str()); + return false; + } + } catch (const std::exception & ex) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Error while getting circle parameters: %s", + shape_name.c_str(), ex.what()); + return false; + } + // Check for points format correctness + if (center_row.size() != 2) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Circle has incorrect center description", + shape_name.c_str()); + return false; + } + + // Obtain circle center + params_->center.x = center_row[0]; + params_->center.y = center_row[1]; + // Setting the center_ with obtained circle center in map's frame + *center_ = params_->center; + + // Getting shape UUID + return obtainShapeUUID(shape_name, params_->uuid.uuid.data()); +} + +nav2_msgs::msg::CircleObject::SharedPtr Circle::getParams() const +{ + return params_; +} + +bool Circle::setParams(const nav2_msgs::msg::CircleObject::SharedPtr params) +{ + params_ = params; + + if (!center_) { + center_ = std::make_shared(); + } + *center_ = params_->center; + + // If no UUID was specified, generate a new one + if (uuid_is_null(params_->uuid.uuid.data())) { + uuid_generate(params_->uuid.uuid.data()); + } + + return checkConsistency(); +} + +bool Circle::toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance) +{ + geometry_msgs::msg::PoseStamped from_pose, to_pose; + from_pose.header = params_->header; + from_pose.pose.position.x = params_->center.x; + from_pose.pose.position.y = params_->center.y; + from_pose.pose.position.z = params_->center.z; + if ( + nav2_util::transformPoseInTargetFrame( + from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance)) + { + center_->x = to_pose.pose.position.x; + center_->y = to_pose.pose.position.y; + center_->z = to_pose.pose.position.z; + } else { + return false; + } + + return true; +} + +void Circle::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) +{ + min_x = center_->x - params_->radius; + min_y = center_->y - params_->radius; + max_x = center_->x + params_->radius; + max_y = center_->y + params_->radius; +} + +bool Circle::isPointInside(const double px, const double py) const +{ + return ( (px - center_->x) * (px - center_->x) + (py - center_->y) * (py - center_->y) ) <= + params_->radius * params_->radius; +} + +void Circle::putBorders( + nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) +{ + unsigned int mcx, mcy; + if (!centerToMap(map, mcx, mcy)) { + return; + } + + // Implementation of the circle generation algorithm, based on the following work: + // Berthold K.P. Horn "Circle generators for display devices" + // Computer Graphics and Image Processing 5.2 (1976): 280-288. + + // Inputs initialization + const int r = static_cast(std::round(params_->radius / map->info.resolution)); + int x = r; + int y = 1; + + // Error initialization + int s = -r; + + // Calculation algorithm + while (x > y) { // Calculating only first circle octant + // Put 8 points in each octant reflecting symmetrically + putPoint(mcx + x, mcy + y, map, overlay_type); + putPoint(mcx + y, mcy + x, map, overlay_type); + putPoint(mcx - x + 1, mcy + y, map, overlay_type); + putPoint(mcx + y, mcy - x + 1, map, overlay_type); + putPoint(mcx - x + 1, mcy - y + 1, map, overlay_type); + putPoint(mcx - y + 1, mcy - x + 1, map, overlay_type); + putPoint(mcx + x, mcy - y + 1, map, overlay_type); + putPoint(mcx - y + 1, mcy + x, map, overlay_type); + + s = s + 2 * y + 1; + y++; + if (s > 0) { + s = s - 2 * x + 2; + x--; + } + } + + // Corner case for x == y: do not put end points twice + if (x == y) { + putPoint(mcx + x, mcy + y, map, overlay_type); + putPoint(mcx - x + 1, mcy + y, map, overlay_type); + putPoint(mcx - x + 1, mcy - y + 1, map, overlay_type); + putPoint(mcx + x, mcy - y + 1, map, overlay_type); + } +} + +bool Circle::checkConsistency() +{ + if (params_->radius < 0.0) { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Circle has incorrect radius less than zero", + getUUID().c_str()); + return false; + } + return true; +} + +bool Circle::centerToMap( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + unsigned int & mcx, unsigned int & mcy) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // Get center of circle in map coordinates + if (center_->x < map->info.origin.position.x || center_->y < map->info.origin.position.y) { + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Can not convert (%f, %f) circle center to map", + getUUID().c_str(), center_->x, center_->y); + return false; + } + // We need the circle center to be always shifted one cell less its logical center + // and to avoid any FP-accuracy losing on small values, so we are using another + // than nav2_util::worldToMap() approach + mcx = static_cast( + std::round((center_->x - map->info.origin.position.x) / map->info.resolution)) - 1; + mcy = static_cast( + std::round((center_->y - map->info.origin.position.y) / map->info.resolution)) - 1; + if (mcx >= map->info.width || mcy >= map->info.height) { + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Can not convert (%f, %f) point to map", + getUUID().c_str(), center_->x, center_->y); + return false; + } + + return true; +} + +inline void Circle::putPoint( + unsigned int mx, unsigned int my, + nav_msgs::msg::OccupancyGrid::SharedPtr map, + const OverlayType overlay_type) +{ + processCell(map, my * map->info.width + mx, params_->value, overlay_type); +} + +} // namespace nav2_map_server diff --git a/nav2_map_server/test/unit/CMakeLists.txt b/nav2_map_server/test/unit/CMakeLists.txt index 4140b14d6f0..06a4b2228a0 100644 --- a/nav2_map_server/test/unit/CMakeLists.txt +++ b/nav2_map_server/test/unit/CMakeLists.txt @@ -18,3 +18,25 @@ target_link_libraries(test_costmap_filter_info_server ${library_name} ${map_io_library_name} ) + +# test_vector_object_shapes unit test +ament_add_gtest(test_vector_object_shapes + test_vector_object_shapes.cpp +) + +target_link_libraries(test_vector_object_shapes ${vo_server_dependencies}) + +target_link_libraries(test_vector_object_shapes + ${vo_library_name} +) + +# test_vector_object_server unit test +ament_add_gtest(test_vector_object_server + test_vector_object_server.cpp +) + +target_link_libraries(test_vector_object_server ${vo_server_dependencies}) + +target_link_libraries(test_vector_object_server + ${vo_library_name} +) diff --git a/nav2_map_server/test/unit/test_vector_object_server.cpp b/nav2_map_server/test/unit/test_vector_object_server.cpp new file mode 100644 index 00000000000..88bfd697d0a --- /dev/null +++ b/nav2_map_server/test/unit/test_vector_object_server.cpp @@ -0,0 +1,1349 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "unique_identifier_msgs/msg/uuid.hpp" + +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" + +#include "nav2_msgs/srv/add_shapes.hpp" +#include "nav2_msgs/srv/get_shapes.hpp" +#include "nav2_msgs/srv/remove_shapes.hpp" +#include "nav2_msgs/msg/polygon_object.hpp" +#include "nav2_msgs/msg/circle_object.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_util/occ_grid_utils.hpp" + +#include "nav2_map_server/vector_object_server.hpp" + +using namespace std::chrono_literals; + +static constexpr double EPSILON = std::numeric_limits::epsilon(); + +static const char POLYGON_NAME[]{"Polygon"}; +static const char CIRCLE_NAME[]{"Circle"}; +static const char GLOBAL_FRAME_ID[]{"map"}; +static const char SHAPE_FRAME_ID[]{"shape"}; +static const double UPDATE_FREQUENCY{10.0}; +static const int8_t POLYGON_VAL{nav2_util::OCC_GRID_OCCUPIED}; +static const int8_t CIRCLE_VAL{nav2_util::OCC_GRID_OCCUPIED / 2}; + +class VOServerWrapper : public nav2_map_server::VectorObjectServer +{ +public: + void start() + { + ASSERT_EQ(on_configure(get_current_state()), nav2::CallbackReturn::SUCCESS); + ASSERT_EQ(on_activate(get_current_state()), nav2::CallbackReturn::SUCCESS); + } + + void configureFail() + { + ASSERT_EQ(on_configure(get_current_state()), nav2::CallbackReturn::FAILURE); + } + + void stop() + { + ASSERT_EQ(on_deactivate(get_current_state()), nav2::CallbackReturn::SUCCESS); + ASSERT_EQ(on_cleanup(get_current_state()), nav2::CallbackReturn::SUCCESS); + ASSERT_EQ(on_shutdown(get_current_state()), nav2::CallbackReturn::SUCCESS); + } + + void cleanup() + { + ASSERT_EQ(on_cleanup(get_current_state()), nav2::CallbackReturn::SUCCESS); + ASSERT_EQ(on_shutdown(get_current_state()), nav2::CallbackReturn::SUCCESS); + } + + void setProcessMap(const bool process_map) + { + process_map_ = process_map; + } + + void getMapBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) const + { + VectorObjectServer::getMapBoundaries(min_x, min_y, max_x, max_y); + } + + void updateMap( + const double & min_x, const double & min_y, const double & max_x, const double & max_y) + { + VectorObjectServer::updateMap(min_x, min_y, max_x, max_y); + } + + void putVectorObjectsOnMap() + { + VectorObjectServer::putVectorObjectsOnMap(); + } +}; // VOServerWrapper + +class Tester : public ::testing::Test +{ +public: + Tester(); + ~Tester(); + + void setVOServerParams(); + void setShapesParams(const std::string & poly_uuid, const std::string & circle_uuid); + void setPolygonParams(const std::string & uuid); + void setCircleParams(const std::string & uuid); + + nav2_msgs::msg::PolygonObject::SharedPtr makePolygonObject( + const std::vector & uuid); + nav2_msgs::msg::CircleObject::SharedPtr makeCircleObject( + const std::vector & uuid); + + void sendTransform(const double frame_shift); + + template + typename T::Response::SharedPtr sendRequest( + typename nav2::ServiceClient::SharedPtr client, + typename T::Request::SharedPtr request, + const std::chrono::nanoseconds & timeout); + + void mapCallback(nav_msgs::msg::OccupancyGrid::SharedPtr map); + bool waitMap(const std::chrono::nanoseconds & timeout); + void verifyMap(bool is_circle, double poly_x_end = 1.0, double circle_cx = 3.0); + + void comparePolygonObjects( + nav2_msgs::msg::PolygonObject::SharedPtr p1, + nav2_msgs::msg::PolygonObject::SharedPtr p2); + void compareCircleObjects( + nav2_msgs::msg::CircleObject::SharedPtr c1, + nav2_msgs::msg::CircleObject::SharedPtr c2); + +protected: + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Service clients for calling AddShapes.srv, GetShapes.srv, RemoveShapes.srv + nav2::ServiceClient::SharedPtr add_shapes_client_; + nav2::ServiceClient::SharedPtr get_shapes_client_; + nav2::ServiceClient::SharedPtr remove_shapes_client_; + + // Output map subscriber + rclcpp::Subscription::SharedPtr vo_map_sub_; + // Output map published by VectorObjectServer + nav_msgs::msg::OccupancyGrid::SharedPtr map_; + + // Vector Object server node + std::shared_ptr vo_server_; +}; // Tester + +Tester::Tester() +: map_(nullptr) +{ + vo_server_ = std::make_shared(); + + add_shapes_client_ = vo_server_->create_client( + std::string(vo_server_->get_name()) + "/add_shapes"); + + get_shapes_client_ = vo_server_->create_client( + std::string(vo_server_->get_name()) + "/get_shapes"); + + remove_shapes_client_ = vo_server_->create_client( + std::string(vo_server_->get_name()) + "/remove_shapes"); + + vo_map_sub_ = vo_server_->create_subscription( + "vo_map", std::bind(&Tester::mapCallback, this, std::placeholders::_1), + nav2::qos::LatchedSubscriptionQoS()); + + // Transform buffer and listener initialization + tf_buffer_ = std::make_shared(vo_server_->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + tf_listener_ = std::make_shared(*tf_buffer_); +} + +Tester::~Tester() +{ + vo_map_sub_.reset(); + + add_shapes_client_.reset(); + get_shapes_client_.reset(); + remove_shapes_client_.reset(); + + vo_server_.reset(); + + tf_listener_.reset(); + tf_buffer_.reset(); +} + +void Tester::setVOServerParams() +{ + vo_server_->declare_parameter( + "map_topic", rclcpp::ParameterValue("vo_map")); + vo_server_->set_parameter( + rclcpp::Parameter("map_topic", "vo_map")); + + vo_server_->declare_parameter( + "global_frame_id", rclcpp::ParameterValue("map")); + vo_server_->set_parameter( + rclcpp::Parameter("global_frame_id", "map")); + + vo_server_->declare_parameter( + "resolution", rclcpp::ParameterValue(0.1)); + vo_server_->set_parameter( + rclcpp::Parameter("resolution", 0.1)); + + vo_server_->declare_parameter( + "default_value", rclcpp::ParameterValue(nav2_util::OCC_GRID_UNKNOWN)); + vo_server_->set_parameter( + rclcpp::Parameter("default_value", nav2_util::OCC_GRID_UNKNOWN)); + + vo_server_->declare_parameter( + "overlay_type", + rclcpp::ParameterValue(static_cast(nav2_map_server::OverlayType::OVERLAY_SEQ))); + vo_server_->set_parameter( + rclcpp::Parameter( + "overlay_type", + static_cast(nav2_map_server::OverlayType::OVERLAY_SEQ))); + + vo_server_->declare_parameter( + "update_frequency", rclcpp::ParameterValue(UPDATE_FREQUENCY)); + vo_server_->set_parameter( + rclcpp::Parameter("update_frequency", UPDATE_FREQUENCY)); + + vo_server_->declare_parameter( + "transform_tolerance", rclcpp::ParameterValue(0.1)); + vo_server_->set_parameter( + rclcpp::Parameter("transform_tolerance", 0.1)); +} + +void Tester::setShapesParams(const std::string & poly_uuid, const std::string & circle_uuid) +{ + vo_server_->declare_parameter( + "shapes", rclcpp::ParameterValue(std::vector{POLYGON_NAME, CIRCLE_NAME})); + vo_server_->set_parameter( + rclcpp::Parameter("shapes", std::vector{POLYGON_NAME, CIRCLE_NAME})); + + setPolygonParams(poly_uuid); + setCircleParams(circle_uuid); +} + +void Tester::setPolygonParams(const std::string & uuid) +{ + const std::string polygon_name(POLYGON_NAME); + + vo_server_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("polygon")); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "polygon")); + + vo_server_->declare_parameter( + polygon_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".frame_id", GLOBAL_FRAME_ID)); + + vo_server_->declare_parameter( + polygon_name + ".closed", rclcpp::ParameterValue(true)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".closed", true)); + + vo_server_->declare_parameter( + polygon_name + ".value", rclcpp::ParameterValue(POLYGON_VAL)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".value", POLYGON_VAL)); + + std::vector points{1.0, 1.0, -1.0, 1.0, -1.0, -1.0, 1.0, -1.0}; + vo_server_->declare_parameter( + polygon_name + ".points", rclcpp::ParameterValue(points)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", points)); + + if (!uuid.empty()) { + vo_server_->declare_parameter( + polygon_name + ".uuid", rclcpp::ParameterValue(uuid)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".uuid", uuid)); + } +} + +void Tester::setCircleParams(const std::string & uuid) +{ + const std::string circle_name(CIRCLE_NAME); + + vo_server_->declare_parameter( + circle_name + ".type", rclcpp::ParameterValue("circle")); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".type", "circle")); + + vo_server_->declare_parameter( + circle_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".frame_id", GLOBAL_FRAME_ID)); + + vo_server_->declare_parameter( + circle_name + ".fill", rclcpp::ParameterValue(true)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".fill", true)); + + vo_server_->declare_parameter( + circle_name + ".value", rclcpp::ParameterValue(CIRCLE_VAL)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".value", CIRCLE_VAL)); + + vo_server_->declare_parameter( + circle_name + ".center", rclcpp::ParameterValue(std::vector{3.0, 0.0})); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".center", std::vector{3.0, 0.0})); + + vo_server_->declare_parameter( + circle_name + ".radius", rclcpp::ParameterValue(1.0)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".radius", 1.0)); + + if (!uuid.empty()) { + vo_server_->declare_parameter( + circle_name + ".uuid", rclcpp::ParameterValue(uuid)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".uuid", uuid)); + } +} + +void Tester::sendTransform(const double frame_shift) +{ + std::shared_ptr tf_broadcaster = + std::make_shared(vo_server_); + + geometry_msgs::msg::TransformStamped transform; + + // global_frame -> shape_frame transform + transform.header.frame_id = GLOBAL_FRAME_ID; + transform.child_frame_id = SHAPE_FRAME_ID; + + transform.header.stamp = vo_server_->now(); + transform.transform.translation.x = frame_shift; + transform.transform.translation.y = 0.0; + transform.transform.translation.z = 0.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + tf_broadcaster->sendTransform(transform); +} + +nav2_msgs::msg::PolygonObject::SharedPtr Tester::makePolygonObject( + const std::vector & uuid) +{ + nav2_msgs::msg::PolygonObject::SharedPtr po = std::make_shared(); + po->header.frame_id = GLOBAL_FRAME_ID; + if (uuid.size() == 16) { + // uuid could be optionally specified + std::array uuid_array; + std::copy(uuid.begin(), uuid.end(), uuid_array.begin()); + po->uuid.uuid = uuid_array; + } + geometry_msgs::msg::Point32 p; + p.x = 1.0; + p.y = 1.0; + po->points.push_back(p); + p.x = -1.0; + po->points.push_back(p); + p.y = -1.0; + po->points.push_back(p); + p.x = 1.0; + po->points.push_back(p); + po->closed = true; + po->value = POLYGON_VAL; + + return po; +} + +nav2_msgs::msg::CircleObject::SharedPtr Tester::makeCircleObject( + const std::vector & uuid) +{ + nav2_msgs::msg::CircleObject::SharedPtr co = std::make_shared(); + co->header.frame_id = GLOBAL_FRAME_ID; + if (uuid.size() == 16) { + // uuid could be optionally specified + std::array uuid_array; + std::copy(uuid.begin(), uuid.end(), uuid_array.begin()); + co->uuid.uuid = uuid_array; + } + co->center.x = 3.0; + co->center.y = 0.0; + co->radius = 1.0; + co->fill = true; + co->value = CIRCLE_VAL; + + return co; +} + +template +typename T::Response::SharedPtr Tester::sendRequest( + typename nav2::ServiceClient::SharedPtr client, + typename T::Request::SharedPtr request, + const std::chrono::nanoseconds & timeout) +{ + auto result_future = client->async_call(request); + + rclcpp::Time start_time = vo_server_->now(); + while (rclcpp::ok() && vo_server_->now() - start_time <= rclcpp::Duration(timeout)) { + std::future_status status = result_future.wait_for(10ms); + if (status == std::future_status::ready) { + return result_future.get(); + } + rclcpp::spin_some(vo_server_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return nullptr; +} + +void Tester::mapCallback(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + map_ = map; +} + +bool Tester::waitMap(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = vo_server_->now(); + while (rclcpp::ok() && vo_server_->now() - start_time <= rclcpp::Duration(timeout)) { + if (map_) { + return true; + } + rclcpp::spin_some(vo_server_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +void Tester::verifyMap(bool is_circle, double poly_x_end, double circle_cx) +{ + // Wait for map to be received + const std::chrono::nanoseconds timeout = + std::chrono::duration_cast(1s * 1.5 / UPDATE_FREQUENCY); + ASSERT_TRUE(waitMap(timeout)); + + // Map should contain polygon and circle and will look like: + // + // polygon {x1, y1} (and map's origin): should be always {-1.0, -1.0} + // V + // xxxxxx....xxx. + // xxxxxx...xxxxx < circle is optionally placed on map + // xxxxxx...xxxxx + // xxxxxx....xxx. + // ^ + // polygon {x2, y2}. x2 = poly_x_end; y2 - is always == 1.0 + + // Polygon {x2, y2} in map coordinates + const unsigned int m_poly_x2 = 9 + poly_x_end * 10; + const unsigned int m_poly_y2 = 19; + + // Center of the circle in map coordinates (expressed in floating-point for best precision) + const double m_circle_cx = 9 + circle_cx * 10 + 0.5; // cell's origin + 0.5 center of cell + const double m_circle_cy = 9 + 0.5; // cell's origin + 0.5 center of cell + + // Radius of the circle in map coordinates + const double m_circle_rad = 10.0; + + for (unsigned int my = 0; my < map_->info.height; my++) { + for (unsigned int mx = 0; mx < map_->info.width; mx++) { + if (mx <= m_poly_x2 && my <= m_poly_y2) { + // Point belongs to the polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + } else if (is_circle && std::hypot(m_circle_cx - mx, m_circle_cy - my) <= m_circle_rad) { + // Point belongs to the circle + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + } else { + // Point does not belong to any shape + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + } + } + } +} + +void Tester::comparePolygonObjects( + nav2_msgs::msg::PolygonObject::SharedPtr p1, + nav2_msgs::msg::PolygonObject::SharedPtr p2) +{ + ASSERT_EQ(p1->header.frame_id, p2->header.frame_id); + ASSERT_EQ(p1->uuid.uuid, p2->uuid.uuid); + ASSERT_EQ(p1->points.size(), p2->points.size()); + for (unsigned int i = 0; i < p1->points.size(); i++) { + ASSERT_NEAR(p1->points[i].x, p2->points[i].x, EPSILON); + ASSERT_NEAR(p1->points[i].y, p2->points[i].y, EPSILON); + } + ASSERT_EQ(p1->closed, p2->closed); + ASSERT_EQ(p1->value, p2->value); +} + +void Tester::compareCircleObjects( + nav2_msgs::msg::CircleObject::SharedPtr c1, + nav2_msgs::msg::CircleObject::SharedPtr c2) +{ + ASSERT_EQ(c1->header.frame_id, c2->header.frame_id); + ASSERT_EQ(c1->uuid.uuid, c2->uuid.uuid); + ASSERT_NEAR(c1->center.x, c2->center.x, EPSILON); + ASSERT_NEAR(c1->center.y, c2->center.y, EPSILON); + ASSERT_NEAR(c1->radius, c2->radius, EPSILON); + ASSERT_EQ(c1->fill, c2->fill); + ASSERT_EQ(c1->value, c2->value); +} + +// ---------- ROS-parameters tests ---------- +TEST_F(Tester, testObtainParams) +{ + setVOServerParams(); + setShapesParams( + "01010101-0101-0101-0101-010101010101", + "01010101-0101-0101-0101-010101010102"); + vo_server_->start(); + + verifyMap(true); + + vo_server_->stop(); +} + +TEST_F(Tester, testObtainNoShapes) +{ + setVOServerParams(); + // Set shapes array, but does not set shapes themselves + vo_server_->declare_parameter( + "shapes", rclcpp::ParameterValue(std::vector{POLYGON_NAME, CIRCLE_NAME})); + vo_server_->set_parameter( + rclcpp::Parameter("shapes", std::vector{POLYGON_NAME, CIRCLE_NAME})); + + // Configured that way, vo_server_ should give the failure on configuration stage + vo_server_->configureFail(); + + // Cleaning-up vo_server_ w/o deactivation + vo_server_->cleanup(); +} + +TEST_F(Tester, testIncorrectShapeType) +{ + setVOServerParams(); + setShapesParams("", ""); + // Setting incorrect type of circle + vo_server_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".type", "incorrect_type")); + + // Configured that way, vo_server_ should give the failure on configuration stage + vo_server_->configureFail(); + + // Cleaning-up vo_server_ w/o deactivation + vo_server_->cleanup(); +} + +TEST_F(Tester, testIncorrectPolygonParams) +{ + setVOServerParams(); + // Setting incorrect UUID. Other incorrect params to be checked inside Vector Object Shapes test + setShapesParams( + "incorrect_polygon_uuid", + "01010101-0101-0101-0101-010101010102"); + + // Set that way, vo_server_ should give the failure on configuration stage + vo_server_->configureFail(); + + // Cleaning-up vo_server_ w/o deactivation + vo_server_->cleanup(); +} + +TEST_F(Tester, testIncorrectCircleParams) +{ + setVOServerParams(); + // Setting incorrect UUID. Other incorrect params to be checked inside Vector Object Shapes test + setShapesParams( + "01010101-0101-0101-0101-010101010101", + "incorrect_circle_uuid"); + + // Set that way, vo_server_ should give the failure on configuration stage + vo_server_->configureFail(); + + // Cleaning-up vo_server_ w/o deactivation + vo_server_->cleanup(); +} + +// ---------- Service call tests ---------- +TEST_F(Tester, testAddShapes) +{ + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check the map has been updated correctly + verifyMap(true); + + // Check that getShapes.srv will return correct shapes + auto get_shapes_msg = std::make_shared(); + auto get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + auto p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + auto c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + // Now move X-coordinate of polygon's border and + // add a new circle fully placed inside first one + // through AddShapes.srv: + // Update polygon x2-coordinate to 1.5 + po_msg->points[0].x = 1.5; + po_msg->points[3].x = 1.5; + // Prepare second circle object (fully placed inside first circle) + auto co2_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 3}); + co2_msg->radius = 0.5; + + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + add_shapes_msg->circles.push_back(*co2_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check the map has been updated correctly + verifyMap(true, 1.5); + + // Check that getShapes.srv will return correct shapes + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circles + ASSERT_EQ(get_shapes_result->circles.size(), 2u); + c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + c_check = std::make_shared(get_shapes_result->circles[1]); + compareCircleObjects(c_check, co2_msg); + + vo_server_->stop(); +} + +TEST_F(Tester, testRemoveShapes) +{ + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that getShapes.srv will return correct shapes + auto get_shapes_msg = std::make_shared(); + auto get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + auto p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + auto c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + // Now remove circle from map + auto remove_shapes_msg = std::make_shared(); + remove_shapes_msg->all_objects = false; + unique_identifier_msgs::msg::UUID uuid; + uuid.uuid = std::array{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}; + remove_shapes_msg->uuids.push_back(uuid); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_TRUE(remove_shapes_result->success); + + // Check that there is only polygon remained on map + verifyMap(false); + + // Check that getShapes.srv will return only polygon + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 0u); + + // Then call RemoveShapes.srv with enabled all_objects parameter + remove_shapes_msg->all_objects = true; + remove_shapes_msg->uuids.clear(); + map_.reset(); // Resetting the map to ensure that updated one is received later + remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_TRUE(remove_shapes_result->success); + + // Check that map is empty + ASSERT_EQ(map_->info.width, 1); + ASSERT_EQ(map_->info.height, 1); + ASSERT_EQ(map_->data[0], nav2_util::OCC_GRID_UNKNOWN); + + // getShapes.srv should return empty vectors + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + ASSERT_EQ(get_shapes_result->polygons.size(), 0u); + ASSERT_EQ(get_shapes_result->circles.size(), 0u); + + vo_server_->stop(); +} + +TEST_F(Tester, testAddIncorrectShapes) +{ + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that getShapes.srv will return correct shapes + auto get_shapes_msg = std::make_shared(); + auto get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + auto p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + auto c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + // Then try to update polygon with circle's uuid + po_msg->uuid.uuid[15] = 2; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + po_msg->uuid.uuid[15] = 1; // Restoring back for further usage + + // Then try to update circle with polygon's uuid + co_msg->uuid.uuid[15] = 1; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->circles.push_back(*co_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + co_msg->uuid.uuid[15] = 2; // Restoring back for further usage + + // Try to update polygon with incorrect number of points + po_msg->points.resize(2); + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + + // Try to add new incorrect polygon + po_msg->uuid.uuid[15] = 100; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + // Restoring back for further usage + po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + + // Try to update circle with incorrect radius + co_msg->radius = -1.0; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->circles.push_back(*co_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + + // Try to add new incorrect circle + co_msg->uuid.uuid[15] = 100; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->circles.push_back(*co_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + co_msg->radius = 1.0; // Restoring back for further usage + co_msg->uuid.uuid[15] = 2; // Restoring back for further usage + + // Verify that map did not corrupted after all false-manipulations + verifyMap(true); + + // Check that getShapes.srv will return correct shapes after all false-manipulations + get_shapes_msg = std::make_shared(); + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + vo_server_->stop(); +} + +TEST_F(Tester, testRemoveIncorrectShapes) +{ + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that getShapes.srv will return correct shapes + auto get_shapes_msg = std::make_shared(); + auto get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + auto p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + auto c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + // Then try to remove two shapes: non-existing shape and circle + auto remove_shapes_msg = std::make_shared(); + remove_shapes_msg->all_objects = false; + unique_identifier_msgs::msg::UUID uuid; + uuid.uuid = std::array{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 100}; + remove_shapes_msg->uuids.push_back(uuid); + uuid.uuid = std::array{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}; + remove_shapes_msg->uuids.push_back(uuid); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_FALSE(remove_shapes_result->success); + + // Check that despite on the error, the circle was removed from map + verifyMap(false); + + // Check that getShapes.srv will return only polygon + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 0u); + + vo_server_->stop(); +} + +// ---------- Overlay tests ---------- +TEST_F(Tester, testOverlayMax) +{ + setVOServerParams(); + vo_server_->set_parameter( + rclcpp::Parameter( + "overlay_type", + static_cast(nav2_map_server::OverlayType::OVERLAY_MAX))); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + // Make circle to be overlapped with polygon + co_msg->center.x = 1.0; + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Wait for the map + waitMap(500ms); + + // Verify that overlap on map generated correctly + double my = 9; + double mx = 1; // inside polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 14; // in the overlapping area + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 20; // inside circle + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + my = 18; // outside any shape + mx = 28; // outside any shape + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + + vo_server_->stop(); +} + +TEST_F(Tester, testOverlayMin) +{ + setVOServerParams(); + vo_server_->set_parameter( + rclcpp::Parameter( + "overlay_type", + static_cast(nav2_map_server::OverlayType::OVERLAY_MIN))); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + // Make circle to be overlapped with polygon + co_msg->center.x = 1.0; + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Wait for the map + waitMap(500ms); + + // Verify that overlap on map generated correctly + double my = 9; + double mx = 1; // inside polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 14; // in the overlapping area + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + mx = 20; // inside circle + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + my = 18; // outside any shape + mx = 28; // outside any shape + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + + vo_server_->stop(); +} + +TEST_F(Tester, testOverlaySeq) +{ + setVOServerParams(); + vo_server_->set_parameter( + rclcpp::Parameter( + "overlay_type", + static_cast(nav2_map_server::OverlayType::OVERLAY_SEQ))); + vo_server_->start(); + + // Sequentially add polygon and then circle overlapped with it on map + auto add_shapes_msg = std::make_shared(); + + // Prepare first polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + add_shapes_msg->polygons.push_back(*po_msg); + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Then prepare circle object to add over the polygon + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + // Make circle to be overlapped with polygon + co_msg->center.x = 1.0; + // Also check that putBorders part works correctly + co_msg->fill = false; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Wait for the map + waitMap(500ms); + + // Verify that overlap on map generated correctly + double my = 9; + double mx = 1; // inside polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 10; // on the circle border laying over the polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + mx = 14; // inside circle and polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 24; // inside circle only + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + mx = 29; // on the circle border laying outside the polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + my = 18; // outside any shape + mx = 28; // outside any shape + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + + vo_server_->stop(); +} + +TEST_F(Tester, testOverlayUnknown) +{ + setVOServerParams(); + vo_server_->set_parameter( + rclcpp::Parameter("overlay_type", static_cast(1000))); + vo_server_->start(); + + // Try to add polygon on map + auto add_shapes_msg = std::make_shared(); + auto po_msg = makePolygonObject(std::vector()); + add_shapes_msg->polygons.push_back(*po_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Wait for the map + waitMap(500ms); + + // Check that polygon was not added on map: map is empty + ASSERT_EQ(map_->info.width, 1); + ASSERT_EQ(map_->info.height, 1); + ASSERT_EQ(map_->data[0], nav2_util::OCC_GRID_UNKNOWN); + + vo_server_->stop(); +} + +// ---------- Transform/dynamic tests ---------- +TEST_F(Tester, testShapesMove) +{ + setVOServerParams(); + vo_server_->start(); + + // Wait for the initial map to be received to not get it in later updates + const std::chrono::nanoseconds timeout = + std::chrono::duration_cast(1s * 1.5 / UPDATE_FREQUENCY); + ASSERT_TRUE(waitMap(timeout)); + + // No shift between polygon and map + sendTransform(0.0); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Polygon and circle to be in another moving frame + po_msg->header.frame_id = SHAPE_FRAME_ID; + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that polygon and circle are in correct positions on map + verifyMap(true); + + // Move frame with polygon + sendTransform(0.5); + + // Map is being published dynamically. Wait for the map to be published one more time + map_.reset(); + // Check that polygon and circle are in correct positions on map + verifyMap(true, 1.0, 2.5); // Polygon is moved 0.5m towards to the circle + // Check that map's origin was updated in accordance to polygon movement + ASSERT_NEAR(map_->info.origin.position.x, -0.5, EPSILON); // -1.0 + 0.5 + ASSERT_NEAR(map_->info.origin.position.y, -1.0, EPSILON); + + // Then try to publish polygon in incorrect frame + add_shapes_msg->polygons[0].header.frame_id = "incorrect_frame"; + map_.reset(); // Resetting the map to ensure that updated one is received later + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Having incorrect transform, map should not be published + ASSERT_FALSE(waitMap(timeout)); + + vo_server_->stop(); +} + +TEST_F(Tester, testSwitchDynamicStatic) +{ + setVOServerParams(); + vo_server_->start(); + + // Wait for the initial map to be received to not get it in later updates + const std::chrono::nanoseconds timeout = + std::chrono::duration_cast(1s * 1.5 / UPDATE_FREQUENCY); + ASSERT_TRUE(waitMap(timeout)); + + // 0.5m shift between polygon and map + sendTransform(0.5); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Polygon to be in different frame + po_msg->header.frame_id = SHAPE_FRAME_ID; + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that polygon and circle are in correct positions on map + verifyMap(true, 1.0, 2.5); // Polygon is moved 0.5m towards to the circle + // Check that map's origin was updated in accordance to polygon movement + ASSERT_NEAR(map_->info.origin.position.x, -0.5, EPSILON); // -1.0 + 0.5 + ASSERT_NEAR(map_->info.origin.position.y, -1.0, EPSILON); + + // Then return to the static model and ensure the everything works correctly + add_shapes_msg->polygons[0].header.frame_id = GLOBAL_FRAME_ID; + map_.reset(); // Resetting the map to ensure that updated one is received later + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + verifyMap(true); + + vo_server_->stop(); +} + +// ---------- Corner cases ---------- +TEST_F(Tester, switchProcessMap) +{ + setVOServerParams(); + vo_server_->start(); + + // Wait for the initial map to be received to not get it in later updates + const std::chrono::nanoseconds timeout = + std::chrono::duration_cast(1s * 1.5 / UPDATE_FREQUENCY); + ASSERT_TRUE(waitMap(timeout)); + + // Check that received default map is empty + ASSERT_EQ(map_->header.frame_id, "map"); + ASSERT_NEAR(map_->info.resolution, 0.1, EPSILON); + ASSERT_EQ(map_->info.width, 1); + ASSERT_EQ(map_->info.height, 1); + + // Disable map processing and trying to obtain the map + vo_server_->setProcessMap(false); + + // Switching map update by calling remove service + auto remove_shapes_msg = std::make_shared(); + remove_shapes_msg->all_objects = true; + map_.reset(); // Resetting the map to ensure that updated one is received later + auto remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_TRUE(remove_shapes_result->success); + + // Having process_map_ disabled, map should not be published + ASSERT_FALSE(waitMap(timeout)); + + // Then enable map processing and trying to obtain the map + vo_server_->setProcessMap(true); + + // Switching map update by calling remove service + map_.reset(); // Resetting the map to ensure that updated one is received later + remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_TRUE(remove_shapes_result->success); + + // Ensure that map is being updated + ASSERT_TRUE(waitMap(timeout)); + + // ... and it is empty + ASSERT_EQ(map_->header.frame_id, "map"); + ASSERT_NEAR(map_->info.resolution, 0.1, EPSILON); + ASSERT_EQ(map_->info.width, 1); + ASSERT_EQ(map_->info.height, 1); + + vo_server_->stop(); +} + +TEST_F(Tester, testIncorrectMapBoundaries) { + setVOServerParams(); + vo_server_->start(); + + // Set min_x > max_x + EXPECT_THROW(vo_server_->updateMap(1.0, 0.1, 0.1, 1.0), std::runtime_error); + + // Set min_y > max_y + EXPECT_THROW(vo_server_->updateMap(0.1, 1.0, 1.0, 0.1), std::runtime_error); +} + +TEST_F(Tester, testIncorrectMapBoundariesWhenNoShapes) { + setVOServerParams(); + vo_server_->start(); + double min_x, min_y, max_x, max_y; + EXPECT_THROW(vo_server_->getMapBoundaries(min_x, min_y, max_x, max_y), std::runtime_error); +} + +TEST_F(Tester, testShapeOutsideMap) { + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check the map has been updated correctly + verifyMap(true); + + // Modify the map to have a shape outside the map + vo_server_->updateMap(2.0, 2.0, 4.0, 4.0); + + // Try to put the shapes back in the map + vo_server_->putVectorObjectsOnMap(); + + // Verify that map did not corrupted after all false-manipulations + verifyMap(true); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + + return test_result; +} diff --git a/nav2_map_server/test/unit/test_vector_object_shapes.cpp b/nav2_map_server/test/unit/test_vector_object_shapes.cpp new file mode 100644 index 00000000000..43a6f43a58d --- /dev/null +++ b/nav2_map_server/test/unit/test_vector_object_shapes.cpp @@ -0,0 +1,805 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" + +#include "nav2_msgs/msg/polygon_object.hpp" +#include "nav2_msgs/msg/circle_object.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_util/occ_grid_utils.hpp" + +#include "nav2_map_server/vector_object_shapes.hpp" +#include "nav2_map_server/vector_object_utils.hpp" + +static constexpr double EPSILON = std::numeric_limits::epsilon(); + +static const char POLYGON_NAME[]{"Polygon"}; +static const char CIRCLE_NAME[]{"Circle"}; +static const char GLOBAL_FRAME_ID[]{"map"}; +static const char SHAPE_FRAME_ID[]{"shape"}; +static double FRAME_SHIFT = 0.1; +static std::vector POINTS{1.0, 1.0, -1.0, 1.0, -1.0, -1.0, 1.0, -1.0}; +static std::vector CENTER{0.0, 0.0}; + +class PolygonWrapper : public nav2_map_server::Polygon +{ +public: + explicit PolygonWrapper(const nav2::LifecycleNode::WeakPtr & node) + : Polygon(node) + {} + + geometry_msgs::msg::Polygon::SharedPtr getPoly() + { + return polygon_; + } +}; // PolygonWrapper + +class CircleWrapper : public nav2_map_server::Circle +{ +public: + explicit CircleWrapper(const nav2::LifecycleNode::WeakPtr & node) + : Circle(node) + {} + + geometry_msgs::msg::Point32::SharedPtr getCenter() + { + return center_; + } +}; // CircleWrapper + +class Tester : public ::testing::Test +{ +public: + Tester(); + ~Tester(); + +protected: + void setPolygonParams(const std::string & uuid); + void setCircleParams(const std::string & uuid); + nav2_msgs::msg::PolygonObject::SharedPtr makePolygonObject( + const std::vector & uuid); + nav2_msgs::msg::CircleObject::SharedPtr makeCircleObject( + const std::vector & uuid); + + void sendTransform(); + + nav_msgs::msg::OccupancyGrid::SharedPtr makeMap(); + void verifyPolygonBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map); + void verifyCircleBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map); + void verifyMapEmpty(nav_msgs::msg::OccupancyGrid::SharedPtr map); + + void comparePolygonObjects( + nav2_msgs::msg::PolygonObject::SharedPtr p1, + nav2_msgs::msg::PolygonObject::SharedPtr p2); + void compareCircleObjects( + nav2_msgs::msg::CircleObject::SharedPtr c1, + nav2_msgs::msg::CircleObject::SharedPtr c2); + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + std::shared_ptr polygon_; + std::shared_ptr circle_; + + nav2::LifecycleNode::SharedPtr node_; +}; // Tester + +Tester::Tester() +{ + node_ = std::make_shared("test_node"); + + // Create shapes + polygon_ = std::make_shared(node_); + circle_ = std::make_shared(node_); + + // Transform buffer and listener initialization + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + tf_listener_ = std::make_shared(*tf_buffer_); +} + +Tester::~Tester() +{ + polygon_.reset(); + circle_.reset(); + + node_.reset(); + + tf_listener_.reset(); + tf_buffer_.reset(); +} + +void Tester::setPolygonParams(const std::string & uuid) +{ + const std::string polygon_name(POLYGON_NAME); + + node_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("polygon")); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "polygon")); + + node_->declare_parameter( + polygon_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".frame_id", GLOBAL_FRAME_ID)); + + node_->declare_parameter( + polygon_name + ".closed", rclcpp::ParameterValue(true)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".closed", true)); + + node_->declare_parameter( + polygon_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".value", nav2_util::OCC_GRID_OCCUPIED)); + + node_->declare_parameter( + polygon_name + ".points", rclcpp::ParameterValue(POINTS)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", POINTS)); + + if (!uuid.empty()) { + node_->declare_parameter( + polygon_name + ".uuid", rclcpp::ParameterValue(uuid)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".uuid", uuid)); + } +} + +void Tester::setCircleParams(const std::string & uuid) +{ + const std::string circle_name(CIRCLE_NAME); + + node_->declare_parameter( + circle_name + ".type", rclcpp::ParameterValue("circle")); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".type", "circle")); + + node_->declare_parameter( + circle_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".frame_id", GLOBAL_FRAME_ID)); + + node_->declare_parameter( + circle_name + ".fill", rclcpp::ParameterValue(true)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".fill", true)); + + node_->declare_parameter( + circle_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".value", nav2_util::OCC_GRID_OCCUPIED)); + + node_->declare_parameter( + circle_name + ".center", rclcpp::ParameterValue(CENTER)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".center", CENTER)); + + node_->declare_parameter( + circle_name + ".radius", rclcpp::ParameterValue(1.0)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".radius", 1.0)); + + if (!uuid.empty()) { + node_->declare_parameter( + circle_name + ".uuid", rclcpp::ParameterValue(uuid)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".uuid", uuid)); + } +} + +nav2_msgs::msg::PolygonObject::SharedPtr Tester::makePolygonObject( + const std::vector & uuid) +{ + nav2_msgs::msg::PolygonObject::SharedPtr po = std::make_shared(); + po->header.frame_id = GLOBAL_FRAME_ID; + if (uuid.size() == 16) { + // uuid could be optionally specified + std::array uuid_array; + std::copy(uuid.begin(), uuid.end(), uuid_array.begin()); + po->uuid.uuid = uuid_array; + } + geometry_msgs::msg::Point32 p; + p.x = 1.0; + p.y = 1.0; + po->points.push_back(p); + p.x = -1.0; + po->points.push_back(p); + p.y = -1.0; + po->points.push_back(p); + p.x = 1.0; + po->points.push_back(p); + po->closed = true; + po->value = nav2_util::OCC_GRID_OCCUPIED; + + return po; +} + +nav2_msgs::msg::CircleObject::SharedPtr Tester::makeCircleObject( + const std::vector & uuid) +{ + nav2_msgs::msg::CircleObject::SharedPtr co = std::make_shared(); + co->header.frame_id = GLOBAL_FRAME_ID; + if (uuid.size() == 16) { + // uuid could be optionally specified + std::array uuid_array; + std::copy(uuid.begin(), uuid.end(), uuid_array.begin()); + co->uuid.uuid = uuid_array; + } + co->center.x = 0.0; + co->center.y = 0.0; + co->radius = 1.0; + co->fill = true; + co->value = nav2_util::OCC_GRID_OCCUPIED; + + return co; +} + +void Tester::sendTransform() +{ + std::shared_ptr tf_broadcaster = + std::make_shared(node_); + + geometry_msgs::msg::TransformStamped transform; + + // global_frame -> shape_frame transform + transform.header.frame_id = GLOBAL_FRAME_ID; + transform.child_frame_id = SHAPE_FRAME_ID; + + transform.header.stamp = node_->now(); + transform.transform.translation.x = FRAME_SHIFT; + transform.transform.translation.y = FRAME_SHIFT; + transform.transform.translation.z = 0.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + tf_broadcaster->sendTransform(transform); +} + +nav_msgs::msg::OccupancyGrid::SharedPtr Tester::makeMap() +{ + nav_msgs::msg::OccupancyGrid::SharedPtr map = std::make_shared(); + map->header.frame_id = GLOBAL_FRAME_ID; + map->info.resolution = 0.1; + map->info.width = 40; + map->info.height = 40; + map->info.origin.position.x = -2.0; + map->info.origin.position.y = -2.0; + map->data = std::vector(400 * 400, nav2_util::OCC_GRID_FREE); + + return map; +} + +void Tester::verifyPolygonBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + // Map is expected to be as follows: + // 0 0 0 0 0 0 0 0 + // 0 0 x x x x 0 0 + // 0 0 x 0 0 0 0 0 + // 0 0 x 0 0 0 0 0 + // 0 0 x x x x 0 0 + // 0 0 0 0 0 0 0 0 + const unsigned int map_center_x = 19; + const unsigned int map_center_y = 19; + + for (unsigned int my = 0; my < map->info.height; my++) { + for (unsigned int mx = 0; mx < map->info.width; mx++) { + if (my == map_center_y - 10 && mx >= map_center_x - 10 && mx <= map_center_x + 10) { + // 1st border + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_OCCUPIED); + } else if (mx == map_center_x - 10 && my >= map_center_y - 10 && my <= map_center_y + 10) { + // 2nd border + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_OCCUPIED); + } else if (my == map_center_y + 10 && mx >= map_center_x - 10 && mx <= map_center_x + 10) { + // 3rd border + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_OCCUPIED); + } else { + // Does not belong to any border + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_FREE); + } + } + } +} + +void Tester::verifyCircleBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + // Circle center in cell coordinates + const double circle_center_x = 19.5; // 19 cell's origin + 0.5 center of cell + const double circle_center_y = 19.5; // 19 cell's origin + 0.5 center of cell + + double radius; + for (unsigned int my = 0; my < map->info.height; my++) { + for (unsigned int mx = 0; mx < map->info.width; mx++) { + if (map->data[my * map->info.width + mx] == nav2_util::OCC_GRID_OCCUPIED) { + radius = std::hypot(circle_center_x - mx, circle_center_y - my); + ASSERT_NEAR(radius, 10.0, 1.0); // Border drift no more than once cell + } + } + } +} + +void Tester::verifyMapEmpty(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + for (unsigned int my = 0; my < map->info.height; my++) { + for (unsigned int mx = 0; mx < map->info.width; mx++) { + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_FREE); + } + } +} + +void Tester::comparePolygonObjects( + nav2_msgs::msg::PolygonObject::SharedPtr p1, + nav2_msgs::msg::PolygonObject::SharedPtr p2) +{ + ASSERT_EQ(p1->header.frame_id, p2->header.frame_id); + ASSERT_EQ(p1->uuid.uuid, p2->uuid.uuid); + ASSERT_EQ(p1->points.size(), p2->points.size()); + for (unsigned int i = 0; i < p1->points.size(); i++) { + ASSERT_NEAR(p1->points[i].x, p2->points[i].x, EPSILON); + ASSERT_NEAR(p1->points[i].y, p2->points[i].y, EPSILON); + } + ASSERT_EQ(p1->closed, p2->closed); + ASSERT_EQ(p1->value, p2->value); +} + +void Tester::compareCircleObjects( + nav2_msgs::msg::CircleObject::SharedPtr c1, + nav2_msgs::msg::CircleObject::SharedPtr c2) +{ + ASSERT_EQ(c1->header.frame_id, c2->header.frame_id); + ASSERT_EQ(c1->uuid.uuid, c2->uuid.uuid); + ASSERT_NEAR(c1->center.x, c2->center.x, EPSILON); + ASSERT_NEAR(c1->center.y, c2->center.y, EPSILON); + ASSERT_NEAR(c1->radius, c2->radius, EPSILON); + ASSERT_EQ(c1->fill, c2->fill); + ASSERT_EQ(c1->value, c2->value); +} + +//---------- Polygon testcases ---------- + +TEST_F(Tester, testPolygonObtainParams) +{ + setPolygonParams("01010101-0101-0101-0101-010101010101"); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + ASSERT_EQ(polygon_->getType(), nav2_map_server::POLYGON); + ASSERT_EQ(polygon_->getValue(), nav2_util::OCC_GRID_OCCUPIED); + ASSERT_EQ(polygon_->getFrameID(), GLOBAL_FRAME_ID); + ASSERT_EQ(polygon_->isFill(), true); + ASSERT_EQ(polygon_->getUUID(), "01010101-0101-0101-0101-010101010101"); +} + +TEST_F(Tester, testPolygonObtainIncorrectParams) +{ + // Setting polygon parameters w/o points + const std::string polygon_name(POLYGON_NAME); + + node_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("polygon")); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "polygon")); + + node_->declare_parameter( + polygon_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".frame_id", GLOBAL_FRAME_ID)); + + node_->declare_parameter( + polygon_name + ".closed", rclcpp::ParameterValue(true)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".closed", true)); + + node_->declare_parameter( + polygon_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".value", nav2_util::OCC_GRID_OCCUPIED)); + + node_->declare_parameter( + polygon_name + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY); + + // Check that points is mandatory parameter for the polygon + ASSERT_FALSE(polygon_->obtainParams(polygon_name)); + + // Setting incorrect number of points + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", std::vector{1.0, 2.0, 3.0})); + ASSERT_FALSE(polygon_->obtainParams(polygon_name)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", POINTS)); + + // Setting incorrect UUID + node_->declare_parameter( + polygon_name + ".uuid", rclcpp::ParameterValue("incorrect-uuid")); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".uuid", "incorrect-uuid")); + ASSERT_FALSE(polygon_->obtainParams(polygon_name)); +} + +TEST_F(Tester, testPolygonSetParams) +{ + nav2_msgs::msg::PolygonObject::SharedPtr po = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2}); + + // Check that parameters were set correctly + ASSERT_TRUE(polygon_->setParams(po)); + ASSERT_EQ(polygon_->getType(), nav2_map_server::POLYGON); + ASSERT_EQ(polygon_->getValue(), nav2_util::OCC_GRID_OCCUPIED); + ASSERT_EQ(polygon_->getFrameID(), GLOBAL_FRAME_ID); + ASSERT_EQ(polygon_->isFill(), true); + ASSERT_EQ(polygon_->getUUID(), "01010101-0101-0101-0101-020202020202"); + + // Verify that getting the parameters also works correctly + nav2_msgs::msg::PolygonObject::SharedPtr params = polygon_->getParams(); + comparePolygonObjects(params, po); +} + +TEST_F(Tester, testPolygonSetIncorrectParams) +{ + nav2_msgs::msg::PolygonObject::SharedPtr po = makePolygonObject( + std::vector()); + // Setting incorrect number of vertices + po->points.resize(2); + + // And check that it is failed to set these parameters + ASSERT_FALSE(polygon_->setParams(po)); +} + +TEST_F(Tester, testPolygonBoundaries) +{ + setPolygonParams(""); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + double min_x, min_y, max_x, max_y; + polygon_->getBoundaries(min_x, min_y, max_x, max_y); + + ASSERT_NEAR(min_x, -1.0, EPSILON); + ASSERT_NEAR(min_y, -1.0, EPSILON); + ASSERT_NEAR(max_x, 1.0, EPSILON); + ASSERT_NEAR(max_y, 1.0, EPSILON); +} + +TEST_F(Tester, testPolygonPoints) +{ + setPolygonParams(""); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + ASSERT_TRUE(polygon_->isPointInside(0.0, 0.0)); + ASSERT_FALSE(polygon_->isPointInside(-2.0, -2.0)); + ASSERT_FALSE(polygon_->isPointInside(2.0, 2.0)); +} + +TEST_F(Tester, testPolygonBorders) +{ + setPolygonParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".closed", false)); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + polygon_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyPolygonBorders(map); +} + +TEST_F(Tester, testPolygonBordersOutOfBoundaries) +{ + setPolygonParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".closed", false)); + // Set polygon to be out of map + node_->set_parameter( + rclcpp::Parameter( + std::string(POLYGON_NAME) + ".points", + std::vector{5.0, 5.0, 6.0, 5.0, 5.0, 6.0})); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + polygon_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyMapEmpty(map); +} + +TEST_F(Tester, testPolygonBordersOnePointInsideBoundaries) +{ + setPolygonParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".closed", false)); + // Now, set the first point inside the map and the others out of the map + node_->set_parameter( + rclcpp::Parameter( + std::string(POLYGON_NAME) + ".points", + std::vector{0.5, 0.5, 6.0, 5.0, 5.0, 6.0})); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + polygon_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyMapEmpty(map); +} + +TEST_F(Tester, testPolygonDifferentFrame) +{ + setPolygonParams(""); + // Change shape's frame ID to differ from global one + node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".frame_id", SHAPE_FRAME_ID)); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + sendTransform(); + + // Check that shape coordinates are not transformed + geometry_msgs::msg::Polygon::SharedPtr poly = polygon_->getPoly(); + ASSERT_NEAR(poly->points[0].x, 1.0, EPSILON); + ASSERT_NEAR(poly->points[0].y, 1.0, EPSILON); + ASSERT_NEAR(poly->points[1].x, -1.0, EPSILON); + ASSERT_NEAR(poly->points[1].y, 1.0, EPSILON); + ASSERT_NEAR(poly->points[2].x, -1.0, EPSILON); + ASSERT_NEAR(poly->points[2].y, -1.0, EPSILON); + ASSERT_NEAR(poly->points[3].x, 1.0, EPSILON); + ASSERT_NEAR(poly->points[3].y, -1.0, EPSILON); + + // Transform shape coordinates to global frame + ASSERT_TRUE(polygon_->toFrame(GLOBAL_FRAME_ID, tf_buffer_, 1.0)); + + // Verify that shape coordinates were transformed to global frame successfully + poly = polygon_->getPoly(); + ASSERT_NEAR(poly->points[0].x, 1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[0].y, 1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[1].x, -1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[1].y, 1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[2].x, -1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[2].y, -1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[3].x, 1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[3].y, -1.0 + FRAME_SHIFT, EPSILON); + + // Try to transform to incorrect frame + ASSERT_FALSE(polygon_->toFrame("incorrect_frame", tf_buffer_, 0.1)); +} + +//---------- Circles testcases ---------- + +TEST_F(Tester, testCircleObtainParams) +{ + setCircleParams("01010101-0101-0101-0101-010101010101"); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + ASSERT_EQ(circle_->getType(), nav2_map_server::CIRCLE); + ASSERT_EQ(circle_->getValue(), nav2_util::OCC_GRID_OCCUPIED); + ASSERT_EQ(circle_->getFrameID(), GLOBAL_FRAME_ID); + ASSERT_EQ(circle_->isFill(), true); + ASSERT_EQ(circle_->getUUID(), "01010101-0101-0101-0101-010101010101"); +} + +TEST_F(Tester, testCircleObtainIncorrectParams) +{ + const std::string circle_name(CIRCLE_NAME); + + // Setting circle parameters w/o center and radius + node_->declare_parameter( + circle_name + ".type", rclcpp::ParameterValue("circle")); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".type", "circle")); + + node_->declare_parameter( + circle_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".frame_id", GLOBAL_FRAME_ID)); + + node_->declare_parameter( + circle_name + ".fill", rclcpp::ParameterValue(true)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".fill", true)); + + node_->declare_parameter( + circle_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".value", nav2_util::OCC_GRID_OCCUPIED)); + + node_->declare_parameter( + circle_name + ".center", rclcpp::PARAMETER_DOUBLE_ARRAY); + node_->declare_parameter( + circle_name + ".radius", rclcpp::PARAMETER_DOUBLE); + + // Check that center and radius are mandatory parameter for the circle + ASSERT_FALSE(circle_->obtainParams(circle_name)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".center", CENTER)); + + // Setting incorrect radius + node_->set_parameter( + rclcpp::Parameter(circle_name + ".radius", -1.0)); + ASSERT_FALSE(circle_->obtainParams(circle_name)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".radius", 1.0)); + + // Setting incorrect center format + node_->set_parameter( + rclcpp::Parameter(circle_name + ".center", std::vector{0.0})); + ASSERT_FALSE(circle_->obtainParams(circle_name)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".center", CENTER)); + + // Setting incorrect UUID + node_->declare_parameter( + circle_name + ".uuid", rclcpp::ParameterValue("incorrect-uuid")); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".uuid", "incorrect-uuid")); + ASSERT_FALSE(circle_->obtainParams(circle_name)); +} + +TEST_F(Tester, testCircleSetParams) +{ + nav2_msgs::msg::CircleObject::SharedPtr co = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2}); + + // Check that parameters were set correctly + ASSERT_TRUE(circle_->setParams(co)); + ASSERT_EQ(circle_->getType(), nav2_map_server::CIRCLE); + ASSERT_EQ(circle_->getValue(), nav2_util::OCC_GRID_OCCUPIED); + ASSERT_EQ(circle_->getFrameID(), GLOBAL_FRAME_ID); + ASSERT_EQ(circle_->isFill(), true); + ASSERT_EQ(circle_->getUUID(), "01010101-0101-0101-0101-020202020202"); + + // Verify that getting the parameters also works correctly + nav2_msgs::msg::CircleObject::SharedPtr params = circle_->getParams(); + compareCircleObjects(params, co); +} + +TEST_F(Tester, testCircleSetIncorrectParams) +{ + nav2_msgs::msg::CircleObject::SharedPtr co = makeCircleObject( + std::vector()); + // Setting incorrect radius + co->radius = -1.0; + + // And check that it is failed to set these parameters + ASSERT_FALSE(circle_->setParams(co)); +} + +TEST_F(Tester, testCircleBoundaries) +{ + setCircleParams(""); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + double min_x, min_y, max_x, max_y; + circle_->getBoundaries(min_x, min_y, max_x, max_y); + + ASSERT_NEAR(min_x, -1.0, EPSILON); + ASSERT_NEAR(min_y, -1.0, EPSILON); + ASSERT_NEAR(max_x, 1.0, EPSILON); + ASSERT_NEAR(max_y, 1.0, EPSILON); +} + +TEST_F(Tester, testCirclePoints) +{ + setCircleParams(""); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + ASSERT_TRUE(circle_->isPointInside(0.0, 0.0)); + ASSERT_FALSE(circle_->isPointInside(-1.0, -1.0)); + ASSERT_FALSE(circle_->isPointInside(1.0, 1.0)); +} + +TEST_F(Tester, testCircleBorders) +{ + setCircleParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".fill", false)); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + circle_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyCircleBorders(map); +} + +TEST_F(Tester, testCircleBordersOutOfBoundaries) +{ + setCircleParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".fill", false)); + // Set circle to be out of map + node_->set_parameter( + rclcpp::Parameter( + std::string(CIRCLE_NAME) + ".center", + std::vector{5.0, 5.0})); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + circle_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyMapEmpty(map); +} + +TEST_F(Tester, testCircleBordersOutsideMap) +{ + setCircleParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".fill", false)); + // Set circle to be out of map + node_->set_parameter( + rclcpp::Parameter( + std::string(CIRCLE_NAME) + ".center", + std::vector{-3.0, -3.0})); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + circle_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyMapEmpty(map); +} + +TEST_F(Tester, testCircleDifferentFrame) +{ + setCircleParams(""); + // Change shape's frame ID to differ from global one + node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".frame_id", SHAPE_FRAME_ID)); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + sendTransform(); + + // Check that shape coordinates are not transformed + geometry_msgs::msg::Point32::SharedPtr center = circle_->getCenter(); + ASSERT_NEAR(center->x, 0.0, EPSILON); + ASSERT_NEAR(center->y, 0.0, EPSILON); + // Transform shape coordinates to global frame + ASSERT_TRUE(circle_->toFrame(GLOBAL_FRAME_ID, tf_buffer_, 1.0)); + + // Verify that shape coordinates were transformed to global frame successfully + center = circle_->getCenter(); + ASSERT_NEAR(center->x, FRAME_SHIFT, EPSILON); + ASSERT_NEAR(center->y, FRAME_SHIFT, EPSILON); + + // Try to transform to incorrect frame + ASSERT_FALSE(circle_->toFrame("incorrect_frame", tf_buffer_, 0.1)); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + + return test_result; +} diff --git a/nav2_mppi_controller/CMakeLists copy.txt b/nav2_mppi_controller/CMakeLists copy.txt new file mode 100644 index 00000000000..fe98d0d2af7 --- /dev/null +++ b/nav2_mppi_controller/CMakeLists copy.txt @@ -0,0 +1,199 @@ +cmake_minimum_required(VERSION 3.8) +project(nav2_mppi_controller) + +find_package(ament_cmake REQUIRED) +find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav2_util REQUIRED) +find_package(nav_msgs 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) +find_package(nav2_ros_common REQUIRED) +find_package(Eigen3 REQUIRED) + + +include_directories( + include + ${EIGEN3_INCLUDE_DIR} +) + +nav2_package() + +include(CheckCXXCompilerFlag) + +check_cxx_compiler_flag("-mfma" COMPILER_SUPPORTS_FMA) + +if(COMPILER_SUPPORTS_FMA) + add_compile_options(-mfma) +endif() + +# If building one the same hardware to be deployed on, try `-march=native`! + +add_library(mppi_controller SHARED + src/controller.cpp + src/collision_checker.cpp + src/critic_manager.cpp + src/noise_generator.cpp + src/optimizer.cpp + src/parameters_handler.cpp + src/path_handler.cpp + src/trajectory_visualizer.cpp +) +target_compile_options(mppi_controller PUBLIC -O3) +target_include_directories(mppi_controller + PUBLIC + "$" + "$") +target_link_libraries(mppi_controller PUBLIC + angles::angles + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_ros_common::nav2_ros_common + nav2_costmap_2d::layers + nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${std_msgs_TARGETS} + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} +) + +add_library(mppi_critics SHARED + src/critics/constraint_critic.cpp + src/critics/cost_critic.cpp + src/critics/goal_critic.cpp + src/critics/goal_angle_critic.cpp + src/critics/obstacles_critic.cpp + src/critics/path_align_critic.cpp + src/critics/path_angle_critic.cpp + src/critics/path_follow_critic.cpp + src/critics/prefer_forward_critic.cpp + src/critics/twirling_critic.cpp + src/critics/velocity_deadband_critic.cpp + src/critics/waypoint_critic.cpp +) +if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND APPLE) + # Apple Clang: use C++20 and optimization, omit -fconcepts + target_compile_features(mppi_critics PUBLIC cxx_std_20) + target_compile_options(mppi_critics PUBLIC -O3) +else() + target_compile_options(mppi_critics PUBLIC -fconcepts -O3) +endif() +target_include_directories(mppi_critics + PUBLIC + "$" + "$") +target_link_libraries(mppi_critics PUBLIC + angles::angles + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_ros_common::nav2_ros_common + nav2_costmap_2d::layers + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${std_msgs_TARGETS} + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} + mppi_controller +) +target_link_libraries(mppi_critics PRIVATE + pluginlib::pluginlib +) + +add_library(mppi_trajectory_validators SHARED + src/trajectory_validators/optimal_trajectory_validator.cpp +) +target_compile_options(mppi_trajectory_validators PUBLIC -O3) +target_include_directories(mppi_trajectory_validators + PUBLIC + "$" + "$" + "$") +target_link_libraries(mppi_trajectory_validators PUBLIC + angles::angles + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::layers + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${std_msgs_TARGETS} + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} +) +target_link_libraries(mppi_trajectory_validators PRIVATE + pluginlib::pluginlib +) + +install(TARGETS mppi_controller mppi_critics mppi_trajectory_validators + EXPORT nav2_mppi_controller + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + ament_find_gtest() + + add_subdirectory(test) + add_subdirectory(benchmark) +endif() + +ament_export_libraries(${libraries}) +ament_export_dependencies( + angles + geometry_msgs + nav2_core + nav2_costmap_2d + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros + visualization_msgs + nav2_ros_common + Eigen3 +) +ament_export_include_directories(include/${PROJECT_NAME}) +ament_export_targets(nav2_mppi_controller) + +pluginlib_export_plugin_description_file(nav2_core mppic.xml) +pluginlib_export_plugin_description_file(nav2_mppi_controller critics.xml) +pluginlib_export_plugin_description_file(nav2_mppi_controller trajectory_validators.xml) + +ament_package() diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 5197a8859de..f4abe8dd80a 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -1,12 +1,14 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_mppi_controller) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(nav_msgs REQUIRED) find_package(pluginlib REQUIRED) @@ -20,6 +22,7 @@ find_package(visualization_msgs REQUIRED) find_package(nav2_ros_common REQUIRED) find_package(Eigen3 REQUIRED) + include_directories( include ${EIGEN3_INCLUDE_DIR} @@ -58,6 +61,7 @@ target_link_libraries(mppi_controller PUBLIC nav2_ros_common::nav2_ros_common nav2_costmap_2d::layers nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} ${nav_msgs_TARGETS} pluginlib::pluginlib rclcpp::rclcpp @@ -81,8 +85,15 @@ add_library(mppi_critics SHARED src/critics/prefer_forward_critic.cpp src/critics/twirling_critic.cpp src/critics/velocity_deadband_critic.cpp + src/critics/waypoint_critic.cpp ) -target_compile_options(mppi_critics PUBLIC -fconcepts -O3) +if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND APPLE) + # Apple Clang: use C++20 and optimization, omit -fconcepts + target_compile_features(mppi_critics PUBLIC cxx_std_20) + target_compile_options(mppi_critics PUBLIC -O3) +else() + target_compile_options(mppi_critics PUBLIC -fconcepts -O3) +endif() target_include_directories(mppi_critics PUBLIC "$" @@ -109,6 +120,7 @@ target_link_libraries(mppi_critics PRIVATE add_library(mppi_trajectory_validators SHARED src/trajectory_validators/optimal_trajectory_validator.cpp + src/trajectory_validators/fixing_trajectory_validator.cpp ) target_compile_options(mppi_trajectory_validators PUBLIC -O3) target_include_directories(mppi_trajectory_validators diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 68db5b73719..630aa19f214 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -59,6 +59,7 @@ This process is then repeated a number of times and returns a converged solution | retry_attempt_limit | int | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure. | | regenerate_noises | bool | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. | | publish_optimal_trajectory | bool | Publishes the full optimal trajectory sequence each control iteration for downstream control systems, collision checkers, etc to have context beyond the next timestep. | + | publish_critics_stats | bool | Default false. Whether to publish statistics about each critic's performance. When enabled, publishes a `nav2_msgs::msg::CriticsStats` message containing critic names, whether they changed costs, and the sum of costs added by each critic. Useful for debugging and tuning critic behavior. | #### Trajectory Visualizer @@ -293,6 +294,7 @@ controller_server: |---------------------------|----------------------------------|-----------------------------------------------------------------------| | `trajectories` | `visualization_msgs/MarkerArray` | Randomly generated trajectories, including resulting control sequence | | `transformed_global_plan` | `nav_msgs/Path` | Part of global plan considered by local planner | +| `critics_stats` | `nav2_msgs/CriticsStats` | Statistics about each critic's performance (published when `publish_critics_stats` is enabled) | ## Notes to Users @@ -328,6 +330,26 @@ As you increase or decrease your weights on the Obstacle, you may notice the afo Once you have your obstacle avoidance behavior tuned and matched with an appropriate path following penalty, tune the Path Align critic to align with the path. If you design exact-path-alignment behavior, its possible to skip the obstacle critic step as highly tuning the system to follow the path will give it less ability to deviate to avoid obstacles (though it'll slow and stop). Tuning the critic weight for the Obstacle critic high will do the job to avoid near-collisions but the repulsion weight is largely unnecessary to you. For others wanting more dynamic behavior, it _can_ be beneficial to slowly lower the weight on the obstacle critic to give the path alignment critic some more room to work. If your path was generated with a cost-aware planner (like all provided by Nav2) and providing paths sufficiently far from obstacles for your satisfaction, the impact of a slightly reduced Obstacle critic with a Path Alignment critic will do you well. Not over-weighting the path align critic will allow the robot to deviate from the path to get around dynamic obstacles in the scene or other obstacles not previous considered during path planning. It is subjective as to the best behavior for your application, but it has been shown that MPPI can be an exact path tracker and/or avoid dynamic obstacles very fluidly and everywhere in between. The defaults provided are in the generally right regime for a balanced initial trade-off. +### Critic costs debugging + +The `publish_critics_stats` parameter enables publishing of statistics about each critic's performance, which can be visualized using tools like PlotJuggler or Foxglove to analyze and debug critic behavior. + +The published `nav2_msgs::msg::CriticsStats` message contains the following fields: + +- **stamp**: Timestamp of when the statistics were computed +- **critics**: Array of critic names that were evaluated (e.g., "ConstraintCritic", "GoalCritic", "ObstaclesCritic") +- **changed**: Boolean array indicating whether each critic modified the trajectory costs. `true` means the critic added non-zero costs, `false` means it had no effect +- **costs_sum**: Array of the total cost contribution from each critic. This represents the sum of all costs added by that specific critic across all trajectory candidates + +This data is invaluable for understanding: +- Which critics are actively influencing trajectory selection +- The relative impact of each critic on the final trajectory choice +- Whether critics are working as expected or if parameter tuning is needed (e.g. `threshold_to_consider`) + +More detailed statistics could be added in the future. + +![critics_stats](media/critics_stats.png) + ### MFMA and AVX2 Optimizations This MPPI is made possible to run on CPU-only by using a very well optimized implementation that rely on CPU vectorization through AVX2 and MFMA. All even remotely modern computers support this (2013+), but if using a very old computer you may not be able to use the plugin. Note that MPC is computationally heavy to begin with, so computers circa-2013 even if it were to have those compiler flags available probably wouldn't run it at a satisfactory rate anyway. diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp index c59f28e2fad..afe8e83eea9 100644 --- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -28,7 +28,7 @@ #include #include #include -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_mppi_controller/optimizer.hpp" #include "nav2_mppi_controller/motion_models.hpp" diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index deae2c11cb9..e738b3455a4 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -45,5 +45,11 @@ mppi critic for restricting command velocities in deadband range + + + mppi critic for following sparse waypoints with position and orientation tolerances + + + diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp new file mode 100644 index 00000000000..09fe82f0649 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp @@ -0,0 +1,148 @@ +// Copyright (c) 2025, Angsa Robotics GmbH +// +// 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. Reserved. + +#ifndef NAV2_MPPI_CONTROLLER__COLLISION_CHECKER_HPP_ +#define NAV2_MPPI_CONTROLLER__COLLISION_CHECKER_HPP_ + +#include +#include +#include + +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + + +namespace nav2_mppi_controller +{ + +/** + * @struct CollisionResult + * @brief Result of collision checking + */ +struct CollisionResult +{ + bool in_collision; + std::vector center_cost; + std::vector footprint_cost; +}; + +/** + * @class nav2_mppi_controller::MPPICollisionChecker + * @brief A costmap grid collision checker + */ +class MPPICollisionChecker + : public nav2_costmap_2d::FootprintCollisionChecker +{ +public: + /** + * @brief A constructor for nav2_mppi_controller::MPPICollisionChecker + * @param costmap The costmap to collision check against + * @param node Node to extract clock and logger from + */ + MPPICollisionChecker( + std::shared_ptr costmap, + rclcpp_lifecycle::LifecycleNode::SharedPtr node); + + /** + * @brief A constructor for nav2_mppi_controller::MPPICollisionChecker + * for use when irregular bin intervals are appropriate + * @param costmap The costmap to collision check against + * @param angles The vector of possible angle bins to precompute for + * orientations for to speed up collision checking, in radians + */ + // MPPICollisionChecker( + // nav2_costmap_2d::Costmap2D * costmap, + // std::vector & angles); + + /** + * @brief Set the footprint to use with collision checker + * @param footprint The footprint to collision check against + * @param radius Whether or not the footprint is a circle and use radius collision checking + * @param inflation_layer_name Optional name of inflation layer for cost calculation + */ + void setFootprint( + const nav2_costmap_2d::Footprint & footprint, + const bool & radius, + const std::string & inflation_layer_name = ""); + + /** + * @brief Check if in collision with costmap and footprint at poses (batch processing) + * @param x Vector of X coordinates of poses to check against + * @param y Vector of Y coordinates of poses to check against + * @param yaw Vector of yaw angles of poses to check against in radians + * @param traverse_unknown Whether or not to traverse in unknown space + * @return CollisionResult struct with collision status and vectors of center cost, footprint cost, and area cost + */ + CollisionResult inCollision( + const std::vector & x, + const std::vector & y, + const std::vector & yaw, + const bool & traverse_unknown); + + /** + * @brief Get costmap ros object for inflation layer params + * @return Costmap ros + */ + std::shared_ptr getCostmapROS() {return costmap_ros_;} + + /** + * @brief Check if value outside the range + * @param max Maximum value of the range + * @param value the value to check if it is within the range + * @return boolean if in range or not + */ + bool outsideRange(const unsigned int & max, const float & value) const; + + /** + * @brief Create convex hull from a set of points + * @param points Input points to create convex hull from + * @return Convex hull as a footprint + */ + nav2_costmap_2d::Footprint createConvexHull( + const std::vector & points); + + /** + * @brief Find the circumscribed cost for collision checking optimization + * @param inflation_layer_name Optional name of inflation layer + * @return The circumscribed cost value + */ + float findCircumscribedCost(const std::string & inflation_layer_name = ""); + +private: + /** + * @brief Transform footprint to given orientation + * @param footprint The base footprint to transform + * @param yaw The yaw angle to transform to + * @return Transformed footprint + */ + nav2_costmap_2d::Footprint transformFootprint( + const nav2_costmap_2d::Footprint & footprint, + float yaw) const; + +protected: + std::shared_ptr costmap_ros_; + nav2_costmap_2d::Footprint unoriented_footprint_; + bool footprint_is_radius_{false}; + float possible_collision_cost_{-1}; + float circumscribed_radius_{-1.0f}; + float circumscribed_cost_{-1.0f}; + rclcpp::Logger logger_{rclcpp::get_logger("MPPICollisionChecker")}; + rclcpp::Clock::SharedPtr clock_; +}; + +} // namespace nav2_mppi_controller + +#endif // NAV2_MPPI_CONTROLLER__COLLISION_CHECKER_HPP_ \ No newline at end of file diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index 1c52df96f68..40bb7613c40 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -101,31 +101,18 @@ class MPPIController : public nav2_core::Controller void setSpeedLimit(const double & speed_limit, const bool & percentage) override; protected: - /** - * @brief Visualize trajectories - * @param transformed_plan Transformed input plan - * @param cmd_stamp Command stamp - * @param optimal_trajectory Optimal trajectory, if already computed - */ - void visualize( - nav_msgs::msg::Path transformed_plan, - const builtin_interfaces::msg::Time & cmd_stamp, - const Eigen::ArrayXXf & optimal_trajectory); + std::string name_; nav2::LifecycleNode::WeakPtr parent_; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; std::shared_ptr costmap_ros_; std::shared_ptr tf_buffer_; - nav2::Publisher::SharedPtr opt_traj_pub_; std::unique_ptr parameters_handler_; Optimizer optimizer_; PathHandler path_handler_; TrajectoryVisualizer trajectory_visualizer_; - - bool visualize_; - bool publish_optimal_trajectory_; }; } // namespace nav2_mppi_controller diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data copy 2.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data copy 2.hpp new file mode 100644 index 00000000000..8476dde055c --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data copy 2.hpp @@ -0,0 +1,58 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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_MPPI_CONTROLLER__CRITIC_DATA_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_ + +#include + +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_core/goal_checker.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/models/trajectories.hpp" +#include "nav2_mppi_controller/models/path.hpp" +#include "nav2_mppi_controller/motion_models.hpp" + + +namespace mppi +{ + +/** + * @struct mppi::CriticData + * @brief Data to pass to critics for scoring, including state, trajectories, + * pruned path, global goal, costs, and important parameters to share + */ +struct CriticData +{ + const models::State & state; + const models::Trajectories & trajectories; + const models::Path & path; + const geometry_msgs::msg::Pose & goal; + + Eigen::ArrayXf & costs; + float & model_dt; + + bool fail_flag; + nav2_core::GoalChecker * goal_checker; + std::shared_ptr motion_model; + std::optional> path_pts_valid; + std::optional furthest_reached_path_point; +}; + +} // namespace mppi + +#endif // NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp index 8476dde055c..db849ec9536 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp @@ -18,6 +18,8 @@ #include #include +#include +#include #include #include "geometry_msgs/msg/pose_stamped.hpp" @@ -44,9 +46,11 @@ struct CriticData const geometry_msgs::msg::Pose & goal; Eigen::ArrayXf & costs; + std::optional>> individual_critics_cost; float & model_dt; bool fail_flag; + std::optional> trajectories_in_collision; nav2_core::GoalChecker * goal_checker; std::shared_ptr motion_model; std::optional> path_pts_valid; @@ -55,4 +59,4 @@ struct CriticData } // namespace mppi -#endif // NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_ +#endif // NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_ \ No newline at end of file diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_function.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_function.hpp index 9b7d17aaea5..dda4a14f96e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_function.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_function.hpp @@ -70,7 +70,9 @@ class CriticFunction ParametersHandler * param_handler) { parent_ = parent; - logger_ = parent_.lock()->get_logger(); + auto node = parent_.lock(); + logger_ = node->get_logger(); + clock_ = node->get_clock(); name_ = name; parent_name_ = parent_name; costmap_ros_ = costmap_ros; @@ -111,6 +113,7 @@ class CriticFunction ParametersHandler * parameters_handler_; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; + rclcpp::Clock::SharedPtr clock_; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp index fed85867905..447757d73db 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp @@ -22,6 +22,7 @@ #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav2_msgs/msg/critics_stats.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -96,6 +97,10 @@ class CriticManager std::unique_ptr> loader_; Critics critics_; + nav2::Publisher::SharedPtr critics_effect_pub_; + bool publish_critics_stats_; + bool visualize_per_critic_costs_; + rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic copy.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic copy.hpp new file mode 100644 index 00000000000..969cd9b94d8 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic copy.hpp @@ -0,0 +1,109 @@ +// Copyright (c) 2023 Robocc Brice Renaudeau +// +// 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_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ + +#include +#include + +#include "nav2_costmap_2d/inflation_layer.hpp" + +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" +#include "nav2_mppi_controller/collision_checker.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::CostCritic + * @brief Critic objective function for avoiding obstacles using costmap's inflated cost + */ +class CostCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic + */ + void initialize() override; + + /** + * @brief Evaluate cost related to obstacle avoidance + * + * @param costs [out] add obstacle cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + /** + * @brief An implementation of worldToMap fully using floats + * @param wx Float world X coord + * @param wy Float world Y coord + * @param mx unsigned int map X coord + * @param my unsigned into map Y coord + * @return if successful + */ + inline bool worldToMapFloat(float wx, float wy, unsigned int & mx, unsigned int & my) const + { + if (wx < origin_x_ || wy < origin_y_) { + return false; + } + + mx = static_cast((wx - origin_x_) / resolution_); + my = static_cast((wy - origin_y_) / resolution_); + + if (mx < size_x_ && my < size_y_) { + return true; + } + return false; + } + + /** + * @brief A local implementation of getIndex + * @param mx unsigned int map X coord + * @param my unsigned into map Y coord + * @return Index + */ + inline unsigned int getIndex(unsigned int mx, unsigned int my) const + { + return my * size_x_ + mx; + } + + std::unique_ptr collision_checker_; + float possible_collision_cost_; + + bool consider_footprint_{true}; + bool is_tracking_unknown_{true}; + float collision_cost_{0.0f}; + float critical_cost_{0.0f}; + unsigned int near_collision_cost_{253}; + float weight_{0}; + unsigned int trajectory_point_step_; + + float origin_x_, origin_y_, resolution_; + unsigned int size_x_, size_y_; + + float near_goal_distance_; + std::string inflation_layer_name_; + + + + unsigned int power_{0}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp index 2646c98fd9e..3ebaf54400e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp @@ -48,38 +48,6 @@ class CostCritic : public CriticFunction void score(CriticData & data) override; protected: - /** - * @brief Checks if cost represents a collision - * @param cost Point cost at pose center - * @param x X of pose - * @param y Y of pose - * @param theta theta of pose - * @return bool if in collision - */ - inline bool inCollision(float cost, float x, float y, float theta) - { - // If consider_footprint_ check footprint scort for collision - float score_cost = cost; - if (consider_footprint_ && - (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f)) - { - score_cost = static_cast(collision_checker_.footprintCostAtPose( - static_cast(x), static_cast(y), static_cast(theta), - costmap_ros_->getRobotFootprint())); - } - - switch (static_cast(score_cost)) { - case (nav2_costmap_2d::LETHAL_OBSTACLE): - return true; - case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): - return consider_footprint_ ? false : true; - case (nav2_costmap_2d::NO_INFORMATION): - return is_tracking_unknown_ ? false : true; - } - - return false; - } - /** * @brief Find the min cost of the inflation decay function for which the robot MAY be * in collision in any orientation @@ -144,9 +112,8 @@ class CostCritic : public CriticFunction std::string inflation_layer_name_; unsigned int power_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics -#endif // NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ +#endif // NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ \ No newline at end of file diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp index 58c108b21cd..08ec354cf3b 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp @@ -46,7 +46,6 @@ class GoalAngleCritic : public CriticFunction float threshold_to_consider_{0}; unsigned int power_{0}; float weight_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp index 6b248f29945..8fb8fb688ae 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp @@ -46,7 +46,6 @@ class GoalCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; float threshold_to_consider_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic copy 3.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic copy 3.hpp new file mode 100644 index 00000000000..b1c52f361d2 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic copy 3.hpp @@ -0,0 +1,108 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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_MPPI_CONTROLLER__CRITICS__OBSTACLES_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLES_CRITIC_HPP_ + +#include +#include + +//#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" +#include "nav2_mppi_controller/collision_checker.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::ConstraintCritic + * @brief Critic objective function for avoiding obstacles, allowing it to deviate off + * the planned path. This is important to tune in tandem with PathAlign to make a balance + * between path-tracking and dynamic obstacle avoidance capabilities as desirable for a + * particular application + */ +class ObstaclesCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic + */ + void initialize() override; + + /** + * @brief Evaluate cost related to obstacle avoidance + * + * @param costs [out] add obstacle cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + /** + * @brief Checks if cost represents a collision + * @param cost Costmap cost + * @return bool if in collision + */ + inline bool inCollision(float cost) const; + + /** + * @brief cost at a robot pose + * @param x X of pose + * @param y Y of pose + * @param theta theta of pose + * @return Collision information at pose + */ + //inline CollisionCost costAtPose(float x, float y, float theta); + + /** + * @brief Distance to obstacle from cost + * @param cost Costmap cost + * @return float Distance to the obstacle represented by cost + */ + inline float distanceToObstacle(const CollisionCost & cost); + + /** + * @brief Find the min cost of the inflation decay function for which the robot MAY be + * in collision in any orientation + * @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation) + * @return double circumscribed cost, any higher than this and need to do full footprint collision checking + * since some element of the robot could be in collision + */ + float findCircumscribedCost(std::shared_ptr costmap); + +protected: + // nav2_costmap_2d::FootprintCollisionChecker + // collision_checker_{nullptr}; + std::unique_ptr collision_checker_; + int trajectory_point_step_{1}; + + bool consider_footprint_{true}; + float collision_cost_{0}; + float inflation_scale_factor_{0}, inflation_radius_{0}; + + float possible_collision_cost_; + float collision_margin_distance_; + float near_goal_distance_; + float circumscribed_cost_{0}, circumscribed_radius_{0}; + + unsigned int power_{0}; + float repulsion_weight_, critical_weight_{0}; + std::string inflation_layer_name_; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLES_CRITIC_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp index 23fb3db2ca0..31b6a3d0df8 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -97,7 +97,6 @@ class ObstaclesCritic : public CriticFunction unsigned int power_{0}; float repulsion_weight_, critical_weight_{0}; - bool enforce_path_inversion_{false}; std::string inflation_layer_name_; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index 133447a8774..ac29e1035a8 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -18,6 +18,9 @@ #include "nav2_mppi_controller/critic_function.hpp" #include "nav2_mppi_controller/models/state.hpp" #include "nav2_mppi_controller/tools/utils.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "nav2_ros_common/publisher.hpp" namespace mppi::critics { @@ -52,7 +55,9 @@ class PathAlignCritic : public CriticFunction bool use_path_orientations_{false}; unsigned int power_{0}; float weight_{0}; - bool enforce_path_inversion_{false}; + + bool visualize_furthest_point_{false}; + nav2::Publisher::SharedPtr furthest_point_pub_; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp index 85b9d48eb83..a265c21e864 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp @@ -20,6 +20,9 @@ #include "nav2_mppi_controller/models/state.hpp" #include "nav2_mppi_controller/tools/utils.hpp" #include "nav2_core/controller_exceptions.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "nav2_ros_common/publisher.hpp" namespace mppi::critics { @@ -81,7 +84,8 @@ class PathAngleCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; - bool enforce_path_inversion_{false}; + bool visualize_furthest_point_{false}; + nav2::Publisher::SharedPtr furthest_point_pub_; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp index aacec055ccf..c1d3e157104 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp @@ -19,6 +19,10 @@ #include "nav2_mppi_controller/critic_function.hpp" #include "nav2_mppi_controller/models/state.hpp" #include "nav2_mppi_controller/tools/utils.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "nav2_ros_common/publisher.hpp" + namespace mppi::critics { @@ -53,7 +57,8 @@ class PathFollowCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; - bool enforce_path_inversion_{false}; + bool visualize_furthest_point_{false}; + nav2::Publisher::SharedPtr furthest_point_pub_; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp index 811ba9ae9a6..e17ad235e5c 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp @@ -45,7 +45,6 @@ class PreferForwardCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; float threshold_to_consider_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp index bc40b194404..3e316a567e0 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp @@ -44,7 +44,6 @@ class TwirlingCritic : public CriticFunction protected: unsigned int power_{0}; float weight_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/waypoint_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/waypoint_critic.hpp new file mode 100644 index 00000000000..46fe0e2ed2a --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/waypoint_critic.hpp @@ -0,0 +1,112 @@ +// Copyright (c) 2025 +// +// 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_MPPI_CONTROLLER__CRITICS__WAYPOINT_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__WAYPOINT_CRITIC_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" + +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::WaypointCritic + * @brief Critic objective function for following sparse waypoints + * + * This critic subscribes to a nav_msgs/Path topic containing sparse waypoints + * and penalizes trajectories that don't pass through these waypoints with + * configurable position and orientation tolerances. + */ +class WaypointCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic + */ + void initialize() override; + + /** + * @brief Evaluate cost related to waypoint following + * + * @param costs [out] add waypoint cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + /** + * @brief Callback for waypoint path subscription + * @param msg Received path message containing waypoints + */ + void waypointCallback(const nav_msgs::msg::Path::SharedPtr msg); + + /** + * @brief Get the current target waypoint (always the first one in the list) + * @return Index of target waypoint (0 for first), or -1 if no waypoints available + */ + int getTargetWaypoint(); + + /** + * @brief Calculate distance between two points + * @param x1 X coordinate of first point + * @param y1 Y coordinate of first point + * @param x2 X coordinate of second point + * @param y2 Y coordinate of second point + * @return Euclidean distance + */ + double calculateDistance(double x1, double y1, double x2, double y2); + + /** + * @brief Calculate angular difference between two angles + * @param angle1 First angle in radians + * @param angle2 Second angle in radians + * @return Angular difference in radians (-pi to pi) + */ + double calculateAngularDifference(double angle1, double angle2); + + // ROS subscription + rclcpp::Subscription::SharedPtr waypoint_sub_; + + // Waypoint data + std::vector waypoints_; + std::mutex waypoints_mutex_; + + // Parameters + unsigned int power_{1}; + float weight_{1.0f}; + float position_tolerance_{0.5f}; // meters + float yaw_tolerance_{0.5f}; // radians + std::string waypoint_topic_{"truncated_path"}; + bool use_orientation_{true}; // whether to consider orientation + float max_waypoint_distance_{10.0f}; // max distance to consider a waypoint + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::string waypoints_frame_; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__WAYPOINT_CRITIC_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/fixing_trajectory_validator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/fixing_trajectory_validator.hpp new file mode 100644 index 00000000000..93cb60f53f9 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/fixing_trajectory_validator.hpp @@ -0,0 +1,71 @@ +// include/nav2_mppi_controller/validators/auto_fixing_trajectory_validator.hpp +#ifndef NAV2_MPPI_CONTROLLER__AUTO_FIXING_TRAJECTORY_VALIDATOR_HPP_ +#define NAV2_MPPI_CONTROLLER__AUTO_FIXING_TRAJECTORY_VALIDATOR_HPP_ + +#include "nav2_mppi_controller/optimal_trajectory_validator.hpp" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include + +namespace mppi { + +class AutoFixingTrajectoryValidator : public OptimalTrajectoryValidator +{ +public: + using Ptr = std::shared_ptr; + AutoFixingTrajectoryValidator() = default; + ~AutoFixingTrajectoryValidator() override = default; + + void initialize( + const nav2::LifecycleNode::WeakPtr & parent, + const std::string & name, + const std::shared_ptr costmap, + ParametersHandler * param_handler, + const std::shared_ptr tf_buffer, + const models::OptimizerSettings & settings) override; + + ValidationResult validateTrajectory( + const Eigen::ArrayXXf & optimal_trajectory, + const models::ControlSequence & control_sequence, + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::Twist & robot_speed, + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal) override; + + // Po SUCCESS można pobrać naprawioną trajektorię (jeśli była korygowana) + bool hasFixedTrajectory() const { return has_fixed_; } + const Eigen::ArrayXXf & getFixedTrajectory() const { return fixed_traj_; } + +private: + // --- narzędzia kolizyjne / naprawcze --- + bool isCollisionAt(double x, double y, double theta) const; + bool isSafe(double x, double y, double theta) const; + double clearanceAt(double x, double y) const; + std::optional gradientAt(double x, double y) const; + + bool tryRepair(Eigen::ArrayXXf & traj, int start_idx, int end_idx) const; + void laplacianSmoothXY(Eigen::ArrayXXf & traj, const std::vector &locked, int iters) const; + void smoothYaw(Eigen::ArrayXXf & traj) const; + + // --- stan --- + nav2_costmap_2d::Costmap2D * costmap_{nullptr}; + std::unique_ptr> fp_checker_; + + // --- parametry --- + bool consider_footprint_{true}; + double safe_clearance_{0.12}; + double step_gain_{0.6}; + double max_step_{0.05}; + double max_total_disp_{0.25}; + int max_iters_{6}; + int smooth_iters_{2}; + float model_dt_{0.f}; + float auto_fix_ttc_window_{0.8f}; + + // --- wynik naprawy --- + mutable bool has_fixed_{false}; + mutable Eigen::ArrayXXf fixed_traj_; +}; + +} // namespace mppi + +#endif // NAV2_MPPI_CONTROLLER__AUTO_FIXING_TRAJECTORY_VALIDATOR_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp index a8c81c3b07d..50f9d400a91 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp @@ -40,6 +40,7 @@ struct State geometry_msgs::msg::PoseStamped pose; geometry_msgs::msg::Twist speed; + float local_path_length; /** * @brief Reset state data diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index c10684b0c79..3115cc4e0a1 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -89,13 +89,15 @@ class MotionModel 0).select(state.vx.col(i - 1) + max_delta_vx, state.vx.col(i - 1) - min_delta_vx); - state.vx.col(i) = state.cvx.col(i - 1) + state.cvx.col(i - 1) = state.cvx.col(i - 1) .cwiseMax(lower_bound_vx) .cwiseMin(upper_bound_vx); + state.vx.col(i) = state.cvx.col(i - 1); - state.wz.col(i) = state.cwz.col(i - 1) + state.cwz.col(i - 1) = state.cwz.col(i - 1) .cwiseMax(state.wz.col(i - 1) - max_delta_wz) .cwiseMin(state.wz.col(i - 1) + max_delta_wz); + state.wz.col(i) = state.cwz.col(i - 1); if (is_holo) { auto lower_bound_vy = (state.vy.col(i - 1) > @@ -104,10 +106,10 @@ class MotionModel auto upper_bound_vy = (state.vy.col(i - 1) > 0).select(state.vy.col(i - 1) + max_delta_vy, state.vy.col(i - 1) - min_delta_vy); - - state.vy.col(i) = state.cvy.col(i - 1) + state.cvy.col(i - 1) = state.cvy.col(i - 1) .cwiseMax(lower_bound_vy) .cwiseMin(upper_bound_vy); + state.vy.col(i) = state.cvy.col(i - 1); } } } diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp index b5d88008ab1..b3f7b03faab 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp @@ -21,7 +21,7 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/path.hpp" diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer copy 2.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer copy 2.hpp new file mode 100644 index 00000000000..1914abde647 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer copy 2.hpp @@ -0,0 +1,295 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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_MPPI_CONTROLLER__OPTIMIZER_HPP_ +#define NAV2_MPPI_CONTROLLER__OPTIMIZER_HPP_ + +#include + +#include +#include +#include + +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_core/goal_checker.hpp" +#include "nav2_core/controller_exceptions.hpp" +#include "tf2_ros/buffer.hpp" +#include "pluginlib/class_loader.hpp" + +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/geometry_utils.hpp" + +#include "nav2_mppi_controller/models/optimizer_settings.hpp" +#include "nav2_mppi_controller/motion_models.hpp" +#include "nav2_mppi_controller/critic_manager.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/models/trajectories.hpp" +#include "nav2_mppi_controller/models/path.hpp" +#include "nav2_mppi_controller/tools/noise_generator.hpp" +#include "nav2_mppi_controller/tools/parameters_handler.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" +#include "nav2_mppi_controller/optimal_trajectory_validator.hpp" + +namespace mppi +{ + +/** + * @class mppi::Optimizer + * @brief Main algorithm optimizer of the MPPI Controller + */ +class Optimizer +{ +public: + /** + * @brief Constructor for mppi::Optimizer + */ + Optimizer() = default; + + /** + * @brief Destructor for mppi::Optimizer + */ + ~Optimizer() {shutdown();} + + + /** + * @brief Initializes optimizer on startup + * @param parent WeakPtr to node + * @param name Name of plugin + * @param costmap_ros Costmap2DROS object of environment + * @param dynamic_parameter_handler Parameter handler object + * @param tf_buffer TF buffer for transformations + */ + void initialize( + nav2::LifecycleNode::WeakPtr parent, const std::string & name, + std::shared_ptr costmap_ros, + std::shared_ptr tf_buffer, + ParametersHandler * dynamic_parameters_handler); + + /** + * @brief Shutdown for optimizer at process end + */ + void shutdown(); + + /** + * @brief Compute control using MPPI algorithm + * @param robot_pose Pose of the robot at given time + * @param robot_speed Speed of the robot at given time + * @param plan Path plan to track + * @param goal Given Goal pose to reach. + * @param goal_checker Object to check if goal is completed + * @return Tuple of [TwistStamped command, optimal trajectory] + */ + std::tuple evalControl( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker); + + /** + * @brief Get the trajectories generated in a cycle for visualization + * @return Set of trajectories evaluated in cycle + */ + models::Trajectories & getGeneratedTrajectories(); + + /** + * @brief Get the optimal trajectory for a cycle for visualization + * @return Optimal trajectory + */ + Eigen::ArrayXXf getOptimizedTrajectory(); + + /** + * @brief Get the optimal control sequence for a cycle for visualization + * @return Optimal control sequence + */ + const models::ControlSequence & getOptimalControlSequence(); + + /** + * @brief Set the maximum speed based on the speed limits callback + * @param speed_limit Limit of the speed for use + * @param percentage Whether the speed limit is absolute or relative + */ + void setSpeedLimit(double speed_limit, bool percentage); + + /** + * @brief Reset the optimization problem to initial conditions + * @param Whether to reset the constraints to its base values + */ + void reset(bool reset_dynamic_speed_limits = true); + + /** + * @brief Get the motion model time step + * @return Time step of the model + */ + const models::OptimizerSettings & getSettings() const + { + return settings_; + } + +protected: + /** + * @brief Main function to generate, score, and return trajectories + */ + void optimize(); + + /** + * @brief Prepare state information on new request for trajectory rollouts + * @param robot_pose Pose of the robot at given time + * @param robot_speed Speed of the robot at given time + * @param plan Path plan to track + * @param goal_checker Object to check if goal is completed + */ + void prepare( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::Twist & robot_speed, + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker); + + /** + * @brief Obtain the main controller's parameters + */ + void getParams(); + + /** + * @brief Set the motion model of the vehicle platform + * @param model Model string to use + */ + void setMotionModel(const std::string & model); + + /** + * @brief Shift the optimal control sequence after processing for + * next iterations initial conditions after execution + */ + void shiftControlSequence(); + + /** + * @brief updates generated trajectories with noised trajectories + * from the last cycle's optimal control + */ + void generateNoisedTrajectories(); + + /** + * @brief Apply hard vehicle constraints on control sequence + */ + void applyControlSequenceConstraints(); + + /** + * @brief Update velocities in state + * @param state fill state with velocities on each step + */ + void updateStateVelocities(models::State & state) const; + + /** + * @brief Update initial velocity in state + * @param state fill state + */ + void updateInitialStateVelocities(models::State & state) const; + + /** + * @brief predict velocities in state using model + * for time horizon equal to timesteps + * @param state fill state + */ + void propagateStateVelocitiesFromInitials(models::State & state) const; + + /** + * @brief Rollout velocities in state to poses + * @param trajectories to rollout + * @param state fill state + */ + void integrateStateVelocities( + models::Trajectories & trajectories, + const models::State & state) const; + + /** + * @brief Rollout velocities in state to poses + * @param trajectories to rollout + * @param state fill state + */ + void integrateStateVelocities( + Eigen::Array & trajectories, + const Eigen::ArrayXXf & state) const; + + /** + * @brief Update control sequence with state controls weighted by costs + * using softmax function + */ + void updateControlSequence(); + + /** + * @brief Convert control sequence to a twist command + * @param stamp Timestamp to use + * @return TwistStamped of command to send to robot base + */ + geometry_msgs::msg::TwistStamped + getControlFromSequenceAsTwist(const builtin_interfaces::msg::Time & stamp); + + /** + * @brief Whether the motion model is holonomic + * @return Bool if holonomic to populate `y` axis of state + */ + bool isHolonomic() const; + + /** + * @brief Using control frequencies and time step size, determine if trajectory + * offset should be used to populate initial state of the next cycle + */ + void setOffset(double controller_frequency); + + /** + * @brief Perform fallback behavior to try to recover from a set of trajectories in collision + * @param fail Whether the system failed to recover from + */ + bool fallback(bool fail); + +protected: + nav2::LifecycleNode::WeakPtr parent_; + std::shared_ptr costmap_ros_; + nav2_costmap_2d::Costmap2D * costmap_; + std::string name_; + std::shared_ptr tf_buffer_; + + std::shared_ptr motion_model_; + + ParametersHandler * parameters_handler_; + CriticManager critic_manager_; + NoiseGenerator noise_generator_; + + std::unique_ptr> validator_loader_; + OptimalTrajectoryValidator::Ptr trajectory_validator_; + + models::OptimizerSettings settings_; + + models::State state_; + models::ControlSequence control_sequence_; + std::array control_history_; + models::Trajectories generated_trajectories_; + models::Path path_; + geometry_msgs::msg::Pose goal_; + Eigen::ArrayXf costs_; + + CriticData critics_data_ = { + state_, generated_trajectories_, path_, goal_, + costs_, settings_.model_dt, false, nullptr, nullptr, + std::nullopt, std::nullopt}; /// Caution, keep references + + rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; +}; + +} // namespace mppi + +#endif // NAV2_MPPI_CONTROLLER__OPTIMIZER_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 5eba18b9d5d..4da02024bfa 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -20,19 +20,22 @@ #include #include #include +#include +#include #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_core/goal_checker.hpp" #include "nav2_core/controller_exceptions.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "pluginlib/class_loader.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/path.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_mppi_controller/models/optimizer_settings.hpp" #include "nav2_mppi_controller/motion_models.hpp" @@ -117,6 +120,37 @@ class Optimizer */ const models::ControlSequence & getOptimalControlSequence(); + /** + * @brief Get the costs for trajectories for visualization + * @return Costs array + */ + const Eigen::ArrayXf & getCosts() const + { + return costs_; + } + + /** + * @brief Get the per-critic costs for trajectories for visualization + * @return Vector of (critic_name, costs) pairs + */ + const std::vector> & getCriticCosts() const + { + if (critics_data_.individual_critics_cost) { + return *critics_data_.individual_critics_cost; + } + static const std::vector> empty; + return empty; + } + + /** + * @brief Get the collision status for trajectories for visualization + * @return Optional vector of collision flags for each trajectory + */ + const std::optional> & getTrajectoriesInCollision() const + { + return critics_data_.trajectories_in_collision; + } + /** * @brief Set the maximum speed based on the speed limits callback * @param speed_limit Limit of the speed for use @@ -283,7 +317,7 @@ class Optimizer CriticData critics_data_ = { state_, generated_trajectories_, path_, goal_, - costs_, settings_.model_dt, false, nullptr, nullptr, + costs_, std::nullopt, settings_.model_dt, false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; @@ -291,4 +325,4 @@ class Optimizer } // namespace mppi -#endif // NAV2_MPPI_CONTROLLER__OPTIMIZER_HPP_ +#endif // NAV2_MPPI_CONTROLLER__OPTIMIZER_HPP_ \ No newline at end of file diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler copy.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler copy.hpp new file mode 100644 index 00000000000..bb5320ebe62 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler copy.hpp @@ -0,0 +1,159 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// Copyright (c) 2023 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. + +#ifndef NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ +#define NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ + +#include +#include +#include +#include + +#include "nav2_ros_common/lifecycle_node.hpp" +#include "tf2_ros/buffer.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "builtin_interfaces/msg/time.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_core/controller_exceptions.hpp" + +#include "nav2_mppi_controller/tools/parameters_handler.hpp" + +namespace mppi +{ + +using PathIterator = std::vector::iterator; +using PathRange = std::pair; + +/** + * @class mppi::PathHandler + * @brief Manager of incoming reference paths for transformation and processing + */ + +class PathHandler +{ +public: + /** + * @brief Constructor for mppi::PathHandler + */ + PathHandler() = default; + + /** + * @brief Destructor for mppi::PathHandler + */ + ~PathHandler() = default; + + /** + * @brief Initialize path handler on bringup + * @param parent WeakPtr to node + * @param name Name of plugin + * @param costmap_ros Costmap2DROS object of environment + * @param tf TF buffer for transformations + * @param dynamic_parameter_handler Parameter handler object + */ + void initialize( + nav2::LifecycleNode::WeakPtr parent, const std::string & name, + std::shared_ptr, + std::shared_ptr, ParametersHandler *); + + /** + * @brief Set new reference path + * @param Plan Path to use + */ + void setPath(const nav_msgs::msg::Path & plan); + + /** + * @brief Get reference path + * @return Path + */ + nav_msgs::msg::Path & getPath(); + + /** + * @brief transform global plan to local applying constraints, + * then prune global plan + * @param robot_pose Pose of robot + * @return global plan in local frame + */ + nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose); + + /** + * @brief Get the global goal pose transformed to the local frame + * @param stamp Time to get the goal pose at + * @return Transformed goal pose + */ + geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp); + +protected: + /** + * @brief Get largest dimension of costmap (radially) + * @return Max distance from center of costmap to edge + */ + double getMaxCostmapDist(); + + /** + * @brief Transform a pose to the global reference frame + * @param pose Current pose + * @return output poose in global reference frame + */ + geometry_msgs::msg::PoseStamped + transformToGlobalPlanFrame(const geometry_msgs::msg::PoseStamped & pose); + + /** + * @brief Get global plan within window of the local costmap size + * @param global_pose Robot pose + * @return plan transformed in the costmap frame and iterator to the first pose of the global + * plan (for pruning) + */ + std::pair getGlobalPlanConsideringBoundsInCostmapFrame( + const geometry_msgs::msg::PoseStamped & global_pose); + + /** + * @brief Prune a path to only interesting portions + * @param plan Plan to prune + * @param end Final path iterator + */ + void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end); + + /** + * @brief Check if the robot pose is within the set inversion or rotation tolerances + * @param robot_pose Robot's current pose to check + * @return bool If the robot pose is within the tolerances for the next path constraint + */ + bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose); + + std::string name_; + std::shared_ptr costmap_; + std::shared_ptr tf_buffer_; + ParametersHandler * parameters_handler_; + + nav_msgs::msg::Path global_plan_; + nav_msgs::msg::Path global_plan_up_to_inversion_; + rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; + + double max_robot_pose_search_dist_{0}; + double prune_distance_{0}; + double transform_tolerance_{0}; + float inversion_xy_tolerance_{0.2}; + float inversion_yaw_tolerance_{0.4}; + float minimum_rotation_angle_{0.785}; + bool enforce_path_inversion_{false}; + bool enforce_path_rotation_{false}; + unsigned int inversion_locale_{0u}; +}; +} // namespace mppi + +#endif // NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ \ No newline at end of file diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp index cfa87f5ace4..bb5320ebe62 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp @@ -23,7 +23,7 @@ #include #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/path.hpp" #include "builtin_interfaces/msg/time.hpp" @@ -98,17 +98,6 @@ class PathHandler geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp); protected: - /** - * @brief Transform a pose to another frame - * @param frame Frame to transform to - * @param in_pose Input pose - * @param out_pose Output pose - * @return Bool if successful - */ - bool transformPose( - const std::string & frame, const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose) const; - /** * @brief Get largest dimension of costmap (radially) * @return Max distance from center of costmap to edge @@ -140,9 +129,9 @@ class PathHandler void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end); /** - * @brief Check if the robot pose is within the set inversion tolerances + * @brief Check if the robot pose is within the set inversion or rotation tolerances * @param robot_pose Robot's current pose to check - * @return bool If the robot pose is within the set inversion tolerances + * @return bool If the robot pose is within the tolerances for the next path constraint */ bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose); @@ -159,10 +148,12 @@ class PathHandler double prune_distance_{0}; double transform_tolerance_{0}; float inversion_xy_tolerance_{0.2}; - float inversion_yaw_tolerance{0.4}; + float inversion_yaw_tolerance_{0.4}; + float minimum_rotation_angle_{0.785}; bool enforce_path_inversion_{false}; + bool enforce_path_rotation_{false}; unsigned int inversion_locale_{0u}; }; } // namespace mppi -#endif // NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ +#endif // NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ \ No newline at end of file diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_mod.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_mod.hpp new file mode 100644 index 00000000000..bb5320ebe62 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_mod.hpp @@ -0,0 +1,159 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// Copyright (c) 2023 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. + +#ifndef NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ +#define NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ + +#include +#include +#include +#include + +#include "nav2_ros_common/lifecycle_node.hpp" +#include "tf2_ros/buffer.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "builtin_interfaces/msg/time.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_core/controller_exceptions.hpp" + +#include "nav2_mppi_controller/tools/parameters_handler.hpp" + +namespace mppi +{ + +using PathIterator = std::vector::iterator; +using PathRange = std::pair; + +/** + * @class mppi::PathHandler + * @brief Manager of incoming reference paths for transformation and processing + */ + +class PathHandler +{ +public: + /** + * @brief Constructor for mppi::PathHandler + */ + PathHandler() = default; + + /** + * @brief Destructor for mppi::PathHandler + */ + ~PathHandler() = default; + + /** + * @brief Initialize path handler on bringup + * @param parent WeakPtr to node + * @param name Name of plugin + * @param costmap_ros Costmap2DROS object of environment + * @param tf TF buffer for transformations + * @param dynamic_parameter_handler Parameter handler object + */ + void initialize( + nav2::LifecycleNode::WeakPtr parent, const std::string & name, + std::shared_ptr, + std::shared_ptr, ParametersHandler *); + + /** + * @brief Set new reference path + * @param Plan Path to use + */ + void setPath(const nav_msgs::msg::Path & plan); + + /** + * @brief Get reference path + * @return Path + */ + nav_msgs::msg::Path & getPath(); + + /** + * @brief transform global plan to local applying constraints, + * then prune global plan + * @param robot_pose Pose of robot + * @return global plan in local frame + */ + nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose); + + /** + * @brief Get the global goal pose transformed to the local frame + * @param stamp Time to get the goal pose at + * @return Transformed goal pose + */ + geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp); + +protected: + /** + * @brief Get largest dimension of costmap (radially) + * @return Max distance from center of costmap to edge + */ + double getMaxCostmapDist(); + + /** + * @brief Transform a pose to the global reference frame + * @param pose Current pose + * @return output poose in global reference frame + */ + geometry_msgs::msg::PoseStamped + transformToGlobalPlanFrame(const geometry_msgs::msg::PoseStamped & pose); + + /** + * @brief Get global plan within window of the local costmap size + * @param global_pose Robot pose + * @return plan transformed in the costmap frame and iterator to the first pose of the global + * plan (for pruning) + */ + std::pair getGlobalPlanConsideringBoundsInCostmapFrame( + const geometry_msgs::msg::PoseStamped & global_pose); + + /** + * @brief Prune a path to only interesting portions + * @param plan Plan to prune + * @param end Final path iterator + */ + void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end); + + /** + * @brief Check if the robot pose is within the set inversion or rotation tolerances + * @param robot_pose Robot's current pose to check + * @return bool If the robot pose is within the tolerances for the next path constraint + */ + bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose); + + std::string name_; + std::shared_ptr costmap_; + std::shared_ptr tf_buffer_; + ParametersHandler * parameters_handler_; + + nav_msgs::msg::Path global_plan_; + nav_msgs::msg::Path global_plan_up_to_inversion_; + rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; + + double max_robot_pose_search_dist_{0}; + double prune_distance_{0}; + double transform_tolerance_{0}; + float inversion_xy_tolerance_{0.2}; + float inversion_yaw_tolerance_{0.4}; + float minimum_rotation_angle_{0.785}; + bool enforce_path_inversion_{false}; + bool enforce_path_rotation_{false}; + unsigned int inversion_locale_{0u}; +}; +} // namespace mppi + +#endif // NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ \ No newline at end of file diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_mod_final.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_mod_final.hpp new file mode 100644 index 00000000000..bb5320ebe62 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_mod_final.hpp @@ -0,0 +1,159 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// Copyright (c) 2023 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. + +#ifndef NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ +#define NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ + +#include +#include +#include +#include + +#include "nav2_ros_common/lifecycle_node.hpp" +#include "tf2_ros/buffer.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "builtin_interfaces/msg/time.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_core/controller_exceptions.hpp" + +#include "nav2_mppi_controller/tools/parameters_handler.hpp" + +namespace mppi +{ + +using PathIterator = std::vector::iterator; +using PathRange = std::pair; + +/** + * @class mppi::PathHandler + * @brief Manager of incoming reference paths for transformation and processing + */ + +class PathHandler +{ +public: + /** + * @brief Constructor for mppi::PathHandler + */ + PathHandler() = default; + + /** + * @brief Destructor for mppi::PathHandler + */ + ~PathHandler() = default; + + /** + * @brief Initialize path handler on bringup + * @param parent WeakPtr to node + * @param name Name of plugin + * @param costmap_ros Costmap2DROS object of environment + * @param tf TF buffer for transformations + * @param dynamic_parameter_handler Parameter handler object + */ + void initialize( + nav2::LifecycleNode::WeakPtr parent, const std::string & name, + std::shared_ptr, + std::shared_ptr, ParametersHandler *); + + /** + * @brief Set new reference path + * @param Plan Path to use + */ + void setPath(const nav_msgs::msg::Path & plan); + + /** + * @brief Get reference path + * @return Path + */ + nav_msgs::msg::Path & getPath(); + + /** + * @brief transform global plan to local applying constraints, + * then prune global plan + * @param robot_pose Pose of robot + * @return global plan in local frame + */ + nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose); + + /** + * @brief Get the global goal pose transformed to the local frame + * @param stamp Time to get the goal pose at + * @return Transformed goal pose + */ + geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp); + +protected: + /** + * @brief Get largest dimension of costmap (radially) + * @return Max distance from center of costmap to edge + */ + double getMaxCostmapDist(); + + /** + * @brief Transform a pose to the global reference frame + * @param pose Current pose + * @return output poose in global reference frame + */ + geometry_msgs::msg::PoseStamped + transformToGlobalPlanFrame(const geometry_msgs::msg::PoseStamped & pose); + + /** + * @brief Get global plan within window of the local costmap size + * @param global_pose Robot pose + * @return plan transformed in the costmap frame and iterator to the first pose of the global + * plan (for pruning) + */ + std::pair getGlobalPlanConsideringBoundsInCostmapFrame( + const geometry_msgs::msg::PoseStamped & global_pose); + + /** + * @brief Prune a path to only interesting portions + * @param plan Plan to prune + * @param end Final path iterator + */ + void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end); + + /** + * @brief Check if the robot pose is within the set inversion or rotation tolerances + * @param robot_pose Robot's current pose to check + * @return bool If the robot pose is within the tolerances for the next path constraint + */ + bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose); + + std::string name_; + std::shared_ptr costmap_; + std::shared_ptr tf_buffer_; + ParametersHandler * parameters_handler_; + + nav_msgs::msg::Path global_plan_; + nav_msgs::msg::Path global_plan_up_to_inversion_; + rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; + + double max_robot_pose_search_dist_{0}; + double prune_distance_{0}; + double transform_tolerance_{0}; + float inversion_xy_tolerance_{0.2}; + float inversion_yaw_tolerance_{0.4}; + float minimum_rotation_angle_{0.785}; + bool enforce_path_inversion_{false}; + bool enforce_path_rotation_{false}; + unsigned int inversion_locale_{0u}; +}; +} // namespace mppi + +#endif // NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ \ No newline at end of file diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_original.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_original.hpp new file mode 100644 index 00000000000..4233c91ef28 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_original.hpp @@ -0,0 +1,157 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// Copyright (c) 2023 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. + +#ifndef NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ +#define NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ + +#include +#include +#include +#include + +#include "nav2_ros_common/lifecycle_node.hpp" +#include "tf2_ros/buffer.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "builtin_interfaces/msg/time.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_core/controller_exceptions.hpp" + +#include "nav2_mppi_controller/tools/parameters_handler.hpp" + +namespace mppi +{ + +using PathIterator = std::vector::iterator; +using PathRange = std::pair; + +/** + * @class mppi::PathHandler + * @brief Manager of incoming reference paths for transformation and processing + */ + +class PathHandler +{ +public: + /** + * @brief Constructor for mppi::PathHandler + */ + PathHandler() = default; + + /** + * @brief Destructor for mppi::PathHandler + */ + ~PathHandler() = default; + + /** + * @brief Initialize path handler on bringup + * @param parent WeakPtr to node + * @param name Name of plugin + * @param costmap_ros Costmap2DROS object of environment + * @param tf TF buffer for transformations + * @param dynamic_parameter_handler Parameter handler object + */ + void initialize( + nav2::LifecycleNode::WeakPtr parent, const std::string & name, + std::shared_ptr, + std::shared_ptr, ParametersHandler *); + + /** + * @brief Set new reference path + * @param Plan Path to use + */ + void setPath(const nav_msgs::msg::Path & plan); + + /** + * @brief Get reference path + * @return Path + */ + nav_msgs::msg::Path & getPath(); + + /** + * @brief transform global plan to local applying constraints, + * then prune global plan + * @param robot_pose Pose of robot + * @return global plan in local frame + */ + nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose); + + /** + * @brief Get the global goal pose transformed to the local frame + * @param stamp Time to get the goal pose at + * @return Transformed goal pose + */ + geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp); + +protected: + /** + * @brief Get largest dimension of costmap (radially) + * @return Max distance from center of costmap to edge + */ + double getMaxCostmapDist(); + + /** + * @brief Transform a pose to the global reference frame + * @param pose Current pose + * @return output poose in global reference frame + */ + geometry_msgs::msg::PoseStamped + transformToGlobalPlanFrame(const geometry_msgs::msg::PoseStamped & pose); + + /** + * @brief Get global plan within window of the local costmap size + * @param global_pose Robot pose + * @return plan transformed in the costmap frame and iterator to the first pose of the global + * plan (for pruning) + */ + std::pair getGlobalPlanConsideringBoundsInCostmapFrame( + const geometry_msgs::msg::PoseStamped & global_pose); + + /** + * @brief Prune a path to only interesting portions + * @param plan Plan to prune + * @param end Final path iterator + */ + void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end); + + /** + * @brief Check if the robot pose is within the set inversion tolerances + * @param robot_pose Robot's current pose to check + * @return bool If the robot pose is within the set inversion tolerances + */ + bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose); + + std::string name_; + std::shared_ptr costmap_; + std::shared_ptr tf_buffer_; + ParametersHandler * parameters_handler_; + + nav_msgs::msg::Path global_plan_; + nav_msgs::msg::Path global_plan_up_to_inversion_; + rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; + + double max_robot_pose_search_dist_{0}; + double prune_distance_{0}; + double transform_tolerance_{0}; + float inversion_xy_tolerance_{0.2}; + float inversion_yaw_tolerance{0.4}; + bool enforce_path_inversion_{false}; + unsigned int inversion_locale_{0u}; +}; +} // namespace mppi + +#endif // NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer copy 2.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer copy 2.hpp new file mode 100644 index 00000000000..f57d9b6b2b2 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer copy 2.hpp @@ -0,0 +1,118 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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_MPPI_CONTROLLER__TOOLS__TRAJECTORY_VISUALIZER_HPP_ +#define NAV2_MPPI_CONTROLLER__TOOLS__TRAJECTORY_VISUALIZER_HPP_ + +#include + +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +#include "nav2_mppi_controller/tools/parameters_handler.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" +#include "nav2_mppi_controller/models/trajectories.hpp" + +namespace mppi +{ + +/** + * @class mppi::TrajectoryVisualizer + * @brief Visualizes trajectories for debugging + */ +class TrajectoryVisualizer +{ +public: + /** + * @brief Constructor for mppi::TrajectoryVisualizer + */ + TrajectoryVisualizer() = default; + + /** + * @brief Configure trajectory visualizer + * @param parent WeakPtr to node + * @param name Name of plugin + * @param frame_id Frame to publish trajectories in + * @param dynamic_parameter_handler Parameter handler object + */ + void on_configure( + nav2::LifecycleNode::WeakPtr parent, const std::string & name, + const std::string & frame_id, ParametersHandler * parameters_handler); + + /** + * @brief Cleanup object on shutdown + */ + void on_cleanup(); + + /** + * @brief Activate object + */ + void on_activate(); + + /** + * @brief Deactivate object + */ + void on_deactivate(); + + /** + * @brief Add an optimal trajectory to visualize + * @param trajectory Optimal trajectory + */ + void add( + const Eigen::ArrayXXf & trajectory, const std::string & marker_namespace, + const builtin_interfaces::msg::Time & cmd_stamp); + + /** + * @brief Add candidate trajectories to visualize + * @param trajectories Candidate trajectories + */ + void add(const models::Trajectories & trajectories, const std::string & marker_namespace); + + /** + * @brief Visualize the plan + * @param plan Plan to visualize + */ + void visualize(const nav_msgs::msg::Path & plan); + + /** + * @brief Reset object + */ + void reset(); + +protected: + std::string frame_id_; + nav2::Publisher::SharedPtr + trajectories_publisher_; + nav2::Publisher::SharedPtr transformed_path_pub_; + nav2::Publisher::SharedPtr optimal_path_pub_; + + std::unique_ptr optimal_path_; + std::unique_ptr points_; + int marker_id_ = 0; + + ParametersHandler * parameters_handler_; + + size_t trajectory_step_{0}; + size_t time_step_{0}; + + rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; +}; + +} // namespace mppi + +#endif // NAV2_MPPI_CONTROLLER__TOOLS__TRAJECTORY_VISUALIZER_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp index f57d9b6b2b2..b0a1afadfff 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp @@ -19,14 +19,20 @@ #include #include +#include +#include #include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/trajectory.hpp" #include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/footprint.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_mppi_controller/tools/parameters_handler.hpp" #include "nav2_mppi_controller/tools/utils.hpp" #include "nav2_mppi_controller/models/trajectories.hpp" +#include "nav2_mppi_controller/models/control_sequence.hpp" namespace mppi { @@ -78,16 +84,50 @@ class TrajectoryVisualizer const builtin_interfaces::msg::Time & cmd_stamp); /** - * @brief Add candidate trajectories to visualize + * @brief Add candidate trajectories with costs to visualize * @param trajectories Candidate trajectories + * @param total_costs Total cost array for each trajectory + * @param individual_critics_cost Optional vector of (critic_name, cost_array) pairs for per-critic visualization + * @param cmd_stamp Timestamp for the markers + * @param trajectories_in_collision Optional collision flags for each trajectory */ - void add(const models::Trajectories & trajectories, const std::string & marker_namespace); + void add( + const models::Trajectories & trajectories, + const Eigen::ArrayXf & total_costs, + const std::vector> & individual_critics_cost = {}, + const builtin_interfaces::msg::Time & cmd_stamp = builtin_interfaces::msg::Time(), + const std::optional> & trajectories_in_collision = std::nullopt); + + /** + * @brief Visualize all trajectory data in one call + * @param plan Transformed plan to visualize + * @param optimal_trajectory Optimal trajectory + * @param control_sequence Control sequence for optimal trajectory + * @param model_dt Model time step + * @param stamp Timestamp for the visualization + * @param costmap_ros Costmap ROS pointer + * @param candidate_trajectories Generated candidate trajectories + * @param costs Total costs for each trajectory + * @param critic_costs Per-critic costs for each trajectory + * @param trajectories_in_collision Optional collision flags for each trajectory + */ + void visualize( + nav_msgs::msg::Path plan, + const Eigen::ArrayXXf & optimal_trajectory, + const models::ControlSequence & control_sequence, + float model_dt, + const builtin_interfaces::msg::Time & stamp, + std::shared_ptr costmap_ros, + const models::Trajectories & candidate_trajectories, + const Eigen::ArrayXf & costs, + const std::vector> & critic_costs, + const std::optional> & trajectories_in_collision = std::nullopt); /** - * @brief Visualize the plan - * @param plan Plan to visualize + * @brief Visualize without optimizer (for testing) + * @param plan Transformed plan to visualize */ - void visualize(const nav_msgs::msg::Path & plan); + void visualize(nav_msgs::msg::Path plan); /** * @brief Reset object @@ -95,11 +135,40 @@ class TrajectoryVisualizer void reset(); protected: + /** + * @brief Create trajectory markers with cost-based coloring + * @param trajectories Set of trajectories to visualize + * @param costs Cost array for each trajectory + * @param ns Namespace for the markers + * @param cmd_stamp Timestamp for the markers + * @param trajectories_in_collision Optional collision flags for each trajectory + */ + void createTrajectoryMarkers( + const models::Trajectories & trajectories, + const Eigen::ArrayXf & costs, + const std::string & ns, + const builtin_interfaces::msg::Time & cmd_stamp, + const std::optional> & trajectories_in_collision = std::nullopt); + + /** + * @brief Create footprint markers from trajectory + * @param trajectory Optimal trajectory + * @param header Message header + * @param costmap_ros Costmap ROS pointer for footprint and frame information + * @return MarkerArray containing footprint polygons + */ + visualization_msgs::msg::MarkerArray createFootprintMarkers( + const Eigen::ArrayXXf & trajectory, + const std_msgs::msg::Header & header, + std::shared_ptr costmap_ros); + std::string frame_id_; nav2::Publisher::SharedPtr trajectories_publisher_; nav2::Publisher::SharedPtr transformed_path_pub_; nav2::Publisher::SharedPtr optimal_path_pub_; + nav2::Publisher::SharedPtr optimal_footprints_pub_; + nav2::Publisher::SharedPtr optimal_trajectory_msg_pub_; std::unique_ptr optimal_path_; std::unique_ptr points_; @@ -109,10 +178,17 @@ class TrajectoryVisualizer size_t trajectory_step_{0}; size_t time_step_{0}; + bool publish_trajectories_with_total_cost_{false}; + bool publish_trajectories_with_individual_cost_{false}; + bool publish_optimal_footprints_{false}; + bool publish_optimal_trajectory_msg_{false}; + bool publish_transformed_path_{false}; + bool publish_optimal_path_{false}; + int footprint_downsample_factor_{3}; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; }; } // namespace mppi -#endif // NAV2_MPPI_CONTROLLER__TOOLS__TRAJECTORY_VISUALIZER_HPP_ +#endif // NAV2_MPPI_CONTROLLER__TOOLS__TRAJECTORY_VISUALIZER_HPP_ \ No newline at end of file diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy 2.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy 2.hpp new file mode 100644 index 00000000000..4a3cc6b7c06 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy 2.hpp @@ -0,0 +1,689 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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. + +#ifndef NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ +#define NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include "angles/angles.h" + +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/trajectory.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "nav2_ros_common/node_utils.hpp" +#include "nav2_core/goal_checker.hpp" + +#include "nav2_mppi_controller/models/optimizer_settings.hpp" +#include "nav2_mppi_controller/models/control_sequence.hpp" +#include "nav2_mppi_controller/models/path.hpp" +#include "builtin_interfaces/msg/time.hpp" +#include "nav2_mppi_controller/critic_data.hpp" + +#define M_PIF 3.141592653589793238462643383279502884e+00F +#define M_PIF_2 1.5707963267948966e+00F + +namespace mppi::utils +{ +/** + * @brief Convert data into pose + * @param x X position + * @param y Y position + * @param z Z position + * @return Pose object + */ +inline geometry_msgs::msg::Pose createPose(double x, double y, double z) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.position.z = z; + pose.orientation.w = 1; + pose.orientation.x = 0; + pose.orientation.y = 0; + pose.orientation.z = 0; + return pose; +} + +/** + * @brief Convert data into scale + * @param x X scale + * @param y Y scale + * @param z Z scale + * @return Scale object + */ +inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z) +{ + geometry_msgs::msg::Vector3 scale; + scale.x = x; + scale.y = y; + scale.z = z; + return scale; +} + +/** + * @brief Convert data into color + * @param r Red component + * @param g Green component + * @param b Blue component + * @param a Alpha component (transparency) + * @return Color object + */ +inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a) +{ + std_msgs::msg::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +/** + * @brief Convert data into a Maarker + * @param id Marker ID + * @param pose Marker pose + * @param scale Marker scale + * @param color Marker color + * @param frame Reference frame to use + * @return Visualization Marker + */ +inline visualization_msgs::msg::Marker createMarker( + int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale, + const std_msgs::msg::ColorRGBA & color, const std::string & frame_id, const std::string & ns) +{ + using visualization_msgs::msg::Marker; + Marker marker; + marker.header.frame_id = frame_id; + marker.header.stamp = rclcpp::Time(0, 0); + marker.ns = ns; + marker.id = id; + marker.type = Marker::SPHERE; + marker.action = Marker::ADD; + + marker.pose = pose; + marker.scale = scale; + marker.color = color; + return marker; +} + +/** + * @brief Convert data into TwistStamped + * @param vx X velocity + * @param wz Angular velocity + * @param stamp Timestamp + * @param frame Reference frame to use + */ +inline geometry_msgs::msg::TwistStamped toTwistStamped( + float vx, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame) +{ + geometry_msgs::msg::TwistStamped twist; + twist.header.frame_id = frame; + twist.header.stamp = stamp; + twist.twist.linear.x = vx; + twist.twist.angular.z = wz; + + return twist; +} + +/** + * @brief Convert data into TwistStamped + * @param vx X velocity + * @param vy Y velocity + * @param wz Angular velocity + * @param stamp Timestamp + * @param frame Reference frame to use + */ +inline geometry_msgs::msg::TwistStamped toTwistStamped( + float vx, float vy, float wz, const builtin_interfaces::msg::Time & stamp, + const std::string & frame) +{ + auto twist = toTwistStamped(vx, wz, stamp, frame); + twist.twist.linear.y = vy; + + return twist; +} + +inline std::unique_ptr toTrajectoryMsg( + const Eigen::ArrayXXf & trajectory, + const models::ControlSequence & control_sequence, + const double & model_dt, + const std_msgs::msg::Header & header) +{ + auto trajectory_msg = std::make_unique(); + trajectory_msg->header = header; + trajectory_msg->points.resize(trajectory.rows()); + + for (int i = 0; i < trajectory.rows(); ++i) { + auto & curr_pt = trajectory_msg->points[i]; + curr_pt.time_from_start = rclcpp::Duration::from_seconds(i * model_dt); + curr_pt.pose.position.x = trajectory(i, 0); + curr_pt.pose.position.y = trajectory(i, 1); + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, trajectory(i, 2)); + curr_pt.pose.orientation = tf2::toMsg(quat); + curr_pt.velocity.linear.x = control_sequence.vx(i); + curr_pt.velocity.angular.z = control_sequence.wz(i); + if (control_sequence.vy.size() > 0) { + curr_pt.velocity.linear.y = control_sequence.vy(i); + } + } + + return trajectory_msg; +} + +/** + * @brief Convert path to a tensor + * @param path Path to convert + * @return Path tensor + */ +inline models::Path toTensor(const nav_msgs::msg::Path & path) +{ + auto result = models::Path{}; + result.reset(path.poses.size()); + + for (size_t i = 0; i < path.poses.size(); ++i) { + result.x(i) = path.poses[i].pose.position.x; + result.y(i) = path.poses[i].pose.position.y; + result.yaws(i) = tf2::getYaw(path.poses[i].pose.orientation); + } + + return result; +} + +/** + * @brief Get the last pose from a path + * @param path Reference to the path + * @return geometry_msgs::msg::Pose Last pose in the path + */ +inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path) +{ + const unsigned int path_last_idx = path.x.size() - 1; + + auto last_orientation = path.yaws(path_last_idx); + + tf2::Quaternion pose_orientation; + pose_orientation.setRPY(0.0, 0.0, last_orientation); + + geometry_msgs::msg::Pose pathPose; + pathPose.position.x = path.x(path_last_idx); + pathPose.position.y = path.y(path_last_idx); + pathPose.orientation.x = pose_orientation.x(); + pathPose.orientation.y = pose_orientation.y(); + pathPose.orientation.z = pose_orientation.z(); + pathPose.orientation.w = pose_orientation.w(); + + return pathPose; +} + +/** + * @brief normalize + * Normalizes the angle to be -M_PIF circle to +M_PIF circle + * It takes and returns radians. + * @param angles Angles to normalize + * @return normalized angles + */ +template +auto normalize_angles(const T & angles) +{ + return (angles + M_PIF).unaryExpr([&](const float x) { + float remainder = std::fmod(x, 2.0f * M_PIF); + return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF; + }); +} + +/** + * @brief shortest_angular_distance + * + * Given 2 angles, this returns the shortest angular + * difference. The inputs and outputs are of course radians. + * + * The result + * would always be -pi <= result <= pi. Adding the result + * to "from" will always get you an equivalent angle to "to". + * @param from Start angle + * @param to End angle + * @return Shortest distance between angles + */ +template +auto shortest_angular_distance( + const F & from, + const T & to) +{ + return normalize_angles(to - from); +} + +/** + * @brief Evaluate furthest point idx of data.path which is + * nearest to some trajectory in data.trajectories + * @param data Data to use + * @return Idx of furthest path point reached by a set of trajectories + */ +inline size_t findPathFurthestReachedPoint(const CriticData & data) +{ + int traj_cols = data.trajectories.x.cols(); + const auto traj_x = data.trajectories.x.col(traj_cols - 1); + const auto traj_y = data.trajectories.y.col(traj_cols - 1); + + const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1).colwise() - traj_x; + const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1).colwise() - traj_y; + + const auto dists = dx * dx + dy * dy; + + int max_id_by_trajectories = 0, min_id_by_path = 0; + float min_distance_by_path = std::numeric_limits::max(); + size_t n_rows = dists.rows(); + size_t n_cols = dists.cols(); + for (size_t i = 0; i != n_rows; i++) { + min_id_by_path = 0; + min_distance_by_path = std::numeric_limits::max(); + for (size_t j = max_id_by_trajectories; j != n_cols; j++) { + const float cur_dist = dists(i, j); + if (cur_dist < min_distance_by_path) { + min_distance_by_path = cur_dist; + min_id_by_path = j; + } + } + max_id_by_trajectories = std::max(max_id_by_trajectories, min_id_by_path); + } + return max_id_by_trajectories; +} + +/** + * @brief evaluate path furthest point if it is not set + * @param data Data to use + */ +inline void setPathFurthestPointIfNotSet(CriticData & data) +{ + if (!data.furthest_reached_path_point) { + data.furthest_reached_path_point = findPathFurthestReachedPoint(data); + } +} + +/** + * @brief evaluate path costs + * @param data Data to use + */ +inline void findPathCosts( + CriticData & data, + std::shared_ptr costmap_ros) +{ + auto * costmap = costmap_ros->getCostmap(); + unsigned int map_x, map_y; + const size_t path_segments_count = data.path.x.size() - 1; + data.path_pts_valid = std::vector(path_segments_count, false); + const bool tracking_unknown = costmap_ros->getLayeredCostmap()->isTrackingUnknown(); + for (unsigned int idx = 0; idx < path_segments_count; idx++) { + if (!costmap->worldToMap(data.path.x(idx), data.path.y(idx), map_x, map_y)) { + (*data.path_pts_valid)[idx] = false; + continue; + } + + switch (costmap->getCost(map_x, map_y)) { + case (nav2_costmap_2d::LETHAL_OBSTACLE): + (*data.path_pts_valid)[idx] = false; + continue; + case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): + (*data.path_pts_valid)[idx] = false; + continue; + case (nav2_costmap_2d::NO_INFORMATION): + (*data.path_pts_valid)[idx] = tracking_unknown ? true : false; + continue; + } + + (*data.path_pts_valid)[idx] = true; + } +} + +/** + * @brief evaluate path costs if it is not set + * @param data Data to use + */ +inline void setPathCostsIfNotSet( + CriticData & data, + std::shared_ptr costmap_ros) +{ + if (!data.path_pts_valid) { + findPathCosts(data, costmap_ros); + } +} + +/** + * @brief evaluate angle from pose (have angle) to point (no angle) + * @param pose pose + * @param point_x Point to find angle relative to X axis + * @param point_y Point to find angle relative to Y axis + * @param forward_preference If reversing direction is valid + * @return Angle between two points + */ +inline float posePointAngle( + const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) +{ + float pose_x = pose.position.x; + float pose_y = pose.position.y; + float pose_yaw = tf2::getYaw(pose.orientation); + + float yaw = atan2f(point_y - pose_y, point_x - pose_x); + + // If no preference for forward, return smallest angle either in heading or 180 of heading + if (!forward_preference) { + return std::min( + fabs(angles::shortest_angular_distance(yaw, pose_yaw)), + fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PIF)))); + } + + return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); +} + +/** + * @brief evaluate angle from pose (have angle) to point (no angle) + * @param pose pose + * @param point_x Point to find angle relative to X axis + * @param point_y Point to find angle relative to Y axis + * @param point_yaw Yaw of the point to consider along Z axis + * @return Angle between two points + */ +inline float posePointAngle( + const geometry_msgs::msg::Pose & pose, + double point_x, double point_y, double point_yaw) +{ + float pose_x = static_cast(pose.position.x); + float pose_y = static_cast(pose.position.y); + float pose_yaw = static_cast(tf2::getYaw(pose.orientation)); + + float yaw = atan2f(static_cast(point_y) - pose_y, static_cast(point_x) - pose_x); + + if (fabs(angles::shortest_angular_distance(yaw, static_cast(point_yaw))) > M_PIF_2) { + yaw = angles::normalize_angle(yaw + M_PIF); + } + + return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); +} + +/** + * @brief Apply Savisky-Golay filter to optimal trajectory + * @param control_sequence Sequence to apply filter to + * @param control_history Recent set of controls for edge-case handling + * @param Settings Settings to use + */ +inline void savitskyGolayFilter( + models::ControlSequence & control_sequence, + std::array & control_history, + const models::OptimizerSettings & settings) +{ + // Savitzky-Golay Quadratic, 9-point Coefficients + Eigen::Array filter = {-21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f, + -21.0f}; + filter /= 231.0f; + + // Too short to smooth meaningfully + const unsigned int num_sequences = control_sequence.vx.size() - 1; + if (num_sequences < 20) { + return; + } + + auto applyFilter = [&](const Eigen::Array & data) -> float { + return (data * filter).eval().sum(); + }; + + auto applyFilterOverAxis = + [&](Eigen::ArrayXf & sequence, const Eigen::ArrayXf & initial_sequence, + const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void + { + float pt_m4 = hist_0; + float pt_m3 = hist_1; + float pt_m2 = hist_2; + float pt_m1 = hist_3; + float pt = initial_sequence(0); + float pt_p1 = initial_sequence(1); + float pt_p2 = initial_sequence(2); + float pt_p3 = initial_sequence(3); + float pt_p4 = initial_sequence(4); + + for (unsigned int idx = 0; idx != num_sequences; idx++) { + sequence(idx) = applyFilter({pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4}); + pt_m4 = pt_m3; + pt_m3 = pt_m2; + pt_m2 = pt_m1; + pt_m1 = pt; + pt = pt_p1; + pt_p1 = pt_p2; + pt_p2 = pt_p3; + pt_p3 = pt_p4; + + if (idx + 5 < num_sequences) { + pt_p4 = initial_sequence(idx + 5); + } else { + // Return the last point + pt_p4 = initial_sequence(num_sequences); + } + } + }; + + // Filter trajectories + const models::ControlSequence initial_control_sequence = control_sequence; + applyFilterOverAxis( + control_sequence.vx, initial_control_sequence.vx, control_history[0].vx, + control_history[1].vx, control_history[2].vx, control_history[3].vx); + applyFilterOverAxis( + control_sequence.vy, initial_control_sequence.vy, control_history[0].vy, + control_history[1].vy, control_history[2].vy, control_history[3].vy); + applyFilterOverAxis( + control_sequence.wz, initial_control_sequence.wz, control_history[0].wz, + control_history[1].wz, control_history[2].wz, control_history[3].wz); + + // Update control history + unsigned int offset = settings.shift_control_sequence ? 1 : 0; + control_history[0] = control_history[1]; + control_history[1] = control_history[2]; + control_history[2] = control_history[3]; + control_history[3] = { + control_sequence.vx(offset), + control_sequence.vy(offset), + control_sequence.wz(offset)}; +} + +/** + * @brief Find the iterator of the first pose at which there is an inversion on the path, + * @param path to check for inversion + * @return the first point after the inversion found in the path + */ +inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) +{ + // At least 3 poses for a possible inversion + if (path.poses.size() < 3) { + return path.poses.size(); + } + + // Iterating through the path to determine the position of the path inversion + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + float oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + float oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + float ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + float ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + // Checking for the existence of cusp, in the path, using the dot product. + float dot_product = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_product < 0.0f) { + return idx + 1; + } + } + + return path.poses.size(); +} + +/** + * @brief Find and remove poses after the first inversion in the path + * @param path to check for inversion + * @return The location of the inversion, return 0 if none exist + */ +inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) +{ + nav_msgs::msg::Path cropped_path = path; + const unsigned int first_after_inversion = findFirstPathInversion(cropped_path); + if (first_after_inversion == path.poses.size()) { + return 0u; + } + + cropped_path.poses.erase( + cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end()); + path = cropped_path; + return first_after_inversion; +} + +/** + * @brief Compare to trajectory points to find closest path point along integrated distances + * @param vec Vect to check + * @return dist Distance to look for + * @return init Starting index to indec from + */ +inline unsigned int findClosestPathPt( + const std::vector & vec, const float dist, const unsigned int init = 0u) +{ + float distim1 = init != 0u ? vec[init] : 0.0f; // First is 0, no accumulated distance yet + float disti = 0.0f; + const unsigned int size = vec.size(); + for (unsigned int i = init + 1; i != size; i++) { + disti = vec[i]; + if (disti > dist) { + if (i > 0 && dist - distim1 < disti - dist) { + return i - 1; + } + return i; + } + distim1 = disti; + } + return size - 1; +} + +// A struct to hold pose data in floating point resolution +struct Pose2D +{ + float x, y, theta; +}; + +/** + * @brief Shift the columns of a 2D Eigen Array or scalar values of + * 1D Eigen Array by 1 place. + * @param e Eigen Array + * @param direction direction in which Array will be shifted. + * 1 for shift in right direction and -1 for left direction. + */ +inline void shiftColumnsByOnePlace(Eigen::Ref e, int direction) +{ + int size = e.size(); + if(size == 1) {return;} + if(abs(direction) != 1) { + throw std::logic_error("Invalid direction, only 1 and -1 are valid values."); + } + + if((e.cols() == 1 || e.rows() == 1) && size > 1) { + auto start_ptr = direction == 1 ? e.data() + size - 2 : e.data() + 1; + auto end_ptr = direction == 1 ? e.data() : e.data() + size - 1; + while(start_ptr != end_ptr) { + *(start_ptr + direction) = *start_ptr; + start_ptr -= direction; + } + *(start_ptr + direction) = *start_ptr; + } else { + auto start_ptr = direction == 1 ? e.data() + size - 2 * e.rows() : e.data() + e.rows(); + auto end_ptr = direction == 1 ? e.data() : e.data() + size - e.rows(); + auto span = e.rows(); + while(start_ptr != end_ptr) { + std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); + start_ptr -= (direction * span); + } + std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); + } +} + +/** + * @brief Normalize the yaws between points on the basis of final yaw angle + * of the trajectory. + * @param last_yaws Final yaw angles of the trajectories. + * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories. + * @return Normalized yaw between points. + */ +inline auto normalize_yaws_between_points( + const Eigen::Ref & last_yaws, + const Eigen::Ref & yaw_between_points) +{ + Eigen::ArrayXf yaws = utils::shortest_angular_distance( + last_yaws, yaw_between_points).abs(); + int size = yaws.size(); + Eigen::ArrayXf yaws_between_points_corrected(size); + for(int i = 0; i != size; i++) { + const float & yaw_between_point = yaw_between_points[i]; + yaws_between_points_corrected[i] = yaws[i] < M_PIF_2 ? + yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF); + } + return yaws_between_points_corrected; +} + +/** + * @brief Normalize the yaws between points on the basis of goal angle. + * @param goal_yaw Goal yaw angle. + * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories. + * @return Normalized yaw between points + */ +inline auto normalize_yaws_between_points( + const float goal_yaw, const Eigen::Ref & yaw_between_points) +{ + int size = yaw_between_points.size(); + Eigen::ArrayXf yaws_between_points_corrected(size); + for(int i = 0; i != size; i++) { + const float & yaw_between_point = yaw_between_points[i]; + yaws_between_points_corrected[i] = fabs( + angles::normalize_angle(yaw_between_point - goal_yaw)) < M_PIF_2 ? + yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF); + } + return yaws_between_points_corrected; +} + +/** + * @brief Clamps the input between the given lower and upper bounds. + * @param lower_bound Lower bound. + * @param upper_bound Upper bound. + * @return Clamped output. + */ +inline float clamp( + const float lower_bound, const float upper_bound, const float input) +{ + return std::min(upper_bound, std::max(input, lower_bound)); +} + +} // namespace mppi::utils + +#endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy.hpp new file mode 100644 index 00000000000..cb6c000f2db --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy.hpp @@ -0,0 +1,824 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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. + +#ifndef NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ +#define NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include "angles/angles.h" + +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/trajectory.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "nav2_ros_common/node_utils.hpp" +#include "nav2_core/goal_checker.hpp" + +#include "nav2_mppi_controller/models/optimizer_settings.hpp" +#include "nav2_mppi_controller/models/control_sequence.hpp" +#include "nav2_mppi_controller/models/path.hpp" +#include "builtin_interfaces/msg/time.hpp" +#include "nav2_mppi_controller/critic_data.hpp" + +#define M_PIF 3.141592653589793238462643383279502884e+00F +#define M_PIF_2 1.5707963267948966e+00F + +namespace mppi::utils +{ +/** + * @brief Convert data into pose + * @param x X position + * @param y Y position + * @param z Z position + * @return Pose object + */ +inline geometry_msgs::msg::Pose createPose(double x, double y, double z) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.position.z = z; + pose.orientation.w = 1; + pose.orientation.x = 0; + pose.orientation.y = 0; + pose.orientation.z = 0; + return pose; +} + +/** + * @brief Convert data into scale + * @param x X scale + * @param y Y scale + * @param z Z scale + * @return Scale object + */ +inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z) +{ + geometry_msgs::msg::Vector3 scale; + scale.x = x; + scale.y = y; + scale.z = z; + return scale; +} + +/** + * @brief Convert data into color + * @param r Red component + * @param g Green component + * @param b Blue component + * @param a Alpha component (transparency) + * @return Color object + */ +inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a) +{ + std_msgs::msg::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +/** + * @brief Convert data into a Maarker + * @param id Marker ID + * @param pose Marker pose + * @param scale Marker scale + * @param color Marker color + * @param frame Reference frame to use + * @return Visualization Marker + */ +inline visualization_msgs::msg::Marker createMarker( + int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale, + const std_msgs::msg::ColorRGBA & color, const std::string & frame_id, const std::string & ns) +{ + using visualization_msgs::msg::Marker; + Marker marker; + marker.header.frame_id = frame_id; + marker.header.stamp = rclcpp::Time(0, 0); + marker.ns = ns; + marker.id = id; + marker.type = Marker::SPHERE; + marker.action = Marker::ADD; + + marker.pose = pose; + marker.scale = scale; + marker.color = color; + return marker; +} + +/** + * @brief Convert data into TwistStamped + * @param vx X velocity + * @param wz Angular velocity + * @param stamp Timestamp + * @param frame Reference frame to use + */ +inline geometry_msgs::msg::TwistStamped toTwistStamped( + float vx, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame) +{ + geometry_msgs::msg::TwistStamped twist; + twist.header.frame_id = frame; + twist.header.stamp = stamp; + twist.twist.linear.x = vx; + twist.twist.angular.z = wz; + + return twist; +} + +/** + * @brief Convert data into TwistStamped + * @param vx X velocity + * @param vy Y velocity + * @param wz Angular velocity + * @param stamp Timestamp + * @param frame Reference frame to use + */ +inline geometry_msgs::msg::TwistStamped toTwistStamped( + float vx, float vy, float wz, const builtin_interfaces::msg::Time & stamp, + const std::string & frame) +{ + auto twist = toTwistStamped(vx, wz, stamp, frame); + twist.twist.linear.y = vy; + + return twist; +} + +inline std::unique_ptr toTrajectoryMsg( + const Eigen::ArrayXXf & trajectory, + const models::ControlSequence & control_sequence, + const double & model_dt, + const std_msgs::msg::Header & header) +{ + auto trajectory_msg = std::make_unique(); + trajectory_msg->header = header; + trajectory_msg->points.resize(trajectory.rows()); + + for (int i = 0; i < trajectory.rows(); ++i) { + auto & curr_pt = trajectory_msg->points[i]; + curr_pt.time_from_start = rclcpp::Duration::from_seconds(i * model_dt); + curr_pt.pose.position.x = trajectory(i, 0); + curr_pt.pose.position.y = trajectory(i, 1); + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, trajectory(i, 2)); + curr_pt.pose.orientation = tf2::toMsg(quat); + curr_pt.velocity.linear.x = control_sequence.vx(i); + curr_pt.velocity.angular.z = control_sequence.wz(i); + if (control_sequence.vy.size() > 0) { + curr_pt.velocity.linear.y = control_sequence.vy(i); + } + } + + return trajectory_msg; +} + +/** + * @brief Convert path to a tensor + * @param path Path to convert + * @return Path tensor + */ +inline models::Path toTensor(const nav_msgs::msg::Path & path) +{ + auto result = models::Path{}; + result.reset(path.poses.size()); + + for (size_t i = 0; i < path.poses.size(); ++i) { + result.x(i) = path.poses[i].pose.position.x; + result.y(i) = path.poses[i].pose.position.y; + result.yaws(i) = tf2::getYaw(path.poses[i].pose.orientation); + } + + return result; +} + +/** + * @brief Get the last pose from a path + * @param path Reference to the path + * @return geometry_msgs::msg::Pose Last pose in the path + */ +inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path) +{ + const unsigned int path_last_idx = path.x.size() - 1; + + auto last_orientation = path.yaws(path_last_idx); + + tf2::Quaternion pose_orientation; + pose_orientation.setRPY(0.0, 0.0, last_orientation); + + geometry_msgs::msg::Pose pathPose; + pathPose.position.x = path.x(path_last_idx); + pathPose.position.y = path.y(path_last_idx); + pathPose.orientation.x = pose_orientation.x(); + pathPose.orientation.y = pose_orientation.y(); + pathPose.orientation.z = pose_orientation.z(); + pathPose.orientation.w = pose_orientation.w(); + + return pathPose; +} + +/** + * @brief normalize + * Normalizes the angle to be -M_PIF circle to +M_PIF circle + * It takes and returns radians. + * @param angles Angles to normalize + * @return normalized angles + */ +template +auto normalize_angles(const T & angles) +{ + return (angles + M_PIF).unaryExpr([&](const float x) { + float remainder = std::fmod(x, 2.0f * M_PIF); + return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF; + }); +} + +/** + * @brief shortest_angular_distance + * + * Given 2 angles, this returns the shortest angular + * difference. The inputs and outputs are of course radians. + * + * The result + * would always be -pi <= result <= pi. Adding the result + * to "from" will always get you an equivalent angle to "to". + * @param from Start angle + * @param to End angle + * @return Shortest distance between angles + */ +template +auto shortest_angular_distance( + const F & from, + const T & to) +{ + return normalize_angles(to - from); +} + +/** + * @brief Evaluate furthest point idx of data.path which is + * nearest to some trajectory in data.trajectories + * @param data Data to use + * @return Idx of furthest path point reached by a set of trajectories + */ +inline size_t findPathFurthestReachedPoint(const CriticData & data) +{ + int traj_cols = data.trajectories.x.cols(); + const auto traj_x = data.trajectories.x.col(traj_cols - 1); + const auto traj_y = data.trajectories.y.col(traj_cols - 1); + + const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1).colwise() - traj_x; + const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1).colwise() - traj_y; + + const auto dists = dx * dx + dy * dy; + + int max_id_by_trajectories = 0, min_id_by_path = 0; + float min_distance_by_path = std::numeric_limits::max(); + size_t n_rows = dists.rows(); + size_t n_cols = dists.cols(); + for (size_t i = 0; i != n_rows; i++) { + min_id_by_path = 0; + min_distance_by_path = std::numeric_limits::max(); + for (size_t j = max_id_by_trajectories; j != n_cols; j++) { + const float cur_dist = dists(i, j); + if (cur_dist < min_distance_by_path) { + min_distance_by_path = cur_dist; + min_id_by_path = j; + } + } + max_id_by_trajectories = std::max(max_id_by_trajectories, min_id_by_path); + } + return max_id_by_trajectories; +} + +/** + * @brief evaluate path furthest point if it is not set + * @param data Data to use + */ +inline void setPathFurthestPointIfNotSet(CriticData & data) +{ + if (!data.furthest_reached_path_point) { + data.furthest_reached_path_point = findPathFurthestReachedPoint(data); + } +} + +/** + * @brief evaluate path costs + * @param data Data to use + */ +inline void findPathCosts( + CriticData & data, + std::shared_ptr costmap_ros) +{ + auto * costmap = costmap_ros->getCostmap(); + unsigned int map_x, map_y; + const size_t path_segments_count = data.path.x.size() - 1; + data.path_pts_valid = std::vector(path_segments_count, false); + const bool tracking_unknown = costmap_ros->getLayeredCostmap()->isTrackingUnknown(); + for (unsigned int idx = 0; idx < path_segments_count; idx++) { + if (!costmap->worldToMap(data.path.x(idx), data.path.y(idx), map_x, map_y)) { + (*data.path_pts_valid)[idx] = false; + continue; + } + + switch (costmap->getCost(map_x, map_y)) { + case (nav2_costmap_2d::LETHAL_OBSTACLE): + (*data.path_pts_valid)[idx] = false; + continue; + case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): + (*data.path_pts_valid)[idx] = false; + continue; + case (nav2_costmap_2d::NO_INFORMATION): + (*data.path_pts_valid)[idx] = tracking_unknown ? true : false; + continue; + } + + (*data.path_pts_valid)[idx] = true; + } +} + +/** + * @brief evaluate path costs if it is not set + * @param data Data to use + */ +inline void setPathCostsIfNotSet( + CriticData & data, + std::shared_ptr costmap_ros) +{ + if (!data.path_pts_valid) { + findPathCosts(data, costmap_ros); + } +} + +/** + * @brief evaluate angle from pose (have angle) to point (no angle) + * @param pose pose + * @param point_x Point to find angle relative to X axis + * @param point_y Point to find angle relative to Y axis + * @param forward_preference If reversing direction is valid + * @return Angle between two points + */ +inline float posePointAngle( + const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) +{ + float pose_x = pose.position.x; + float pose_y = pose.position.y; + float pose_yaw = tf2::getYaw(pose.orientation); + + float yaw = atan2f(point_y - pose_y, point_x - pose_x); + + // If no preference for forward, return smallest angle either in heading or 180 of heading + if (!forward_preference) { + return std::min( + fabs(angles::shortest_angular_distance(yaw, pose_yaw)), + fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PIF)))); + } + + return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); +} + +/** + * @brief evaluate angle from pose (have angle) to point (no angle) + * @param pose pose + * @param point_x Point to find angle relative to X axis + * @param point_y Point to find angle relative to Y axis + * @param point_yaw Yaw of the point to consider along Z axis + * @return Angle between two points + */ +inline float posePointAngle( + const geometry_msgs::msg::Pose & pose, + double point_x, double point_y, double point_yaw) +{ + float pose_x = static_cast(pose.position.x); + float pose_y = static_cast(pose.position.y); + float pose_yaw = static_cast(tf2::getYaw(pose.orientation)); + + float yaw = atan2f(static_cast(point_y) - pose_y, static_cast(point_x) - pose_x); + + if (fabs(angles::shortest_angular_distance(yaw, static_cast(point_yaw))) > M_PIF_2) { + yaw = angles::normalize_angle(yaw + M_PIF); + } + + return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); +} + +/** + * @brief Apply Savisky-Golay filter to optimal trajectory + * @param control_sequence Sequence to apply filter to + * @param control_history Recent set of controls for edge-case handling + * @param Settings Settings to use + */ +inline void savitskyGolayFilter( + models::ControlSequence & control_sequence, + std::array & control_history, + const models::OptimizerSettings & settings) +{ + // Savitzky-Golay Quadratic, 9-point Coefficients + Eigen::Array filter = {-21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f, + -21.0f}; + filter /= 231.0f; + + // Too short to smooth meaningfully + const unsigned int num_sequences = control_sequence.vx.size() - 1; + if (num_sequences < 20) { + return; + } + + auto applyFilter = [&](const Eigen::Array & data) -> float { + return (data * filter).eval().sum(); + }; + + auto applyFilterOverAxis = + [&](Eigen::ArrayXf & sequence, const Eigen::ArrayXf & initial_sequence, + const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void + { + float pt_m4 = hist_0; + float pt_m3 = hist_1; + float pt_m2 = hist_2; + float pt_m1 = hist_3; + float pt = initial_sequence(0); + float pt_p1 = initial_sequence(1); + float pt_p2 = initial_sequence(2); + float pt_p3 = initial_sequence(3); + float pt_p4 = initial_sequence(4); + + for (unsigned int idx = 0; idx != num_sequences; idx++) { + sequence(idx) = applyFilter({pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4}); + pt_m4 = pt_m3; + pt_m3 = pt_m2; + pt_m2 = pt_m1; + pt_m1 = pt; + pt = pt_p1; + pt_p1 = pt_p2; + pt_p2 = pt_p3; + pt_p3 = pt_p4; + + if (idx + 5 < num_sequences) { + pt_p4 = initial_sequence(idx + 5); + } else { + // Return the last point + pt_p4 = initial_sequence(num_sequences); + } + } + }; + + // Filter trajectories + const models::ControlSequence initial_control_sequence = control_sequence; + applyFilterOverAxis( + control_sequence.vx, initial_control_sequence.vx, control_history[0].vx, + control_history[1].vx, control_history[2].vx, control_history[3].vx); + applyFilterOverAxis( + control_sequence.vy, initial_control_sequence.vy, control_history[0].vy, + control_history[1].vy, control_history[2].vy, control_history[3].vy); + applyFilterOverAxis( + control_sequence.wz, initial_control_sequence.wz, control_history[0].wz, + control_history[1].wz, control_history[2].wz, control_history[3].wz); + + // Update control history + unsigned int offset = settings.shift_control_sequence ? 1 : 0; + control_history[0] = control_history[1]; + control_history[1] = control_history[2]; + control_history[2] = control_history[3]; + control_history[3] = { + control_sequence.vx(offset), + control_sequence.vy(offset), + control_sequence.wz(offset)}; +} + +/** + * @brief Find the iterator of the first pose at which there is an inversion or in-place rotation in the path + * @param path to check for inversion or rotation + * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check) + * @return the first point after the inversion or rotation found in the path + */ +inline unsigned int findFirstPathInversion( + nav_msgs::msg::Path & path, + float rotation_threshold = 0.0f) +{ + if (path.poses.size() < 2) { + RCLCPP_INFO( + rclcpp::get_logger("mppi_utils"), + "[MPPI PathUtils] Path too short for constraints (size=%zu)", + path.poses.size()); + return path.poses.size(); + } + + unsigned int first_after_constraint = path.poses.size(); + + // TYLKO DO LOGÓW: maksymalna rotacja poniżej progu (żeby wiedzieć, czy było \"prawie\") + float max_rotation_below_threshold = 0.0f; + + // Check for in-place rotation first (if enabled) + if (rotation_threshold > 0.0f) { + for (unsigned int idx = 0; idx < path.poses.size() - 1; ++idx) { + float dx = path.poses[idx + 1].pose.position.x - path.poses[idx].pose.position.x; + float dy = path.poses[idx + 1].pose.position.y - path.poses[idx].pose.position.y; + float translation = sqrtf(dx * dx + dy * dy); + + // Check if poses are at roughly the same location + if (translation < 1e-3) { + float yaw1 = tf2::getYaw(path.poses[idx].pose.orientation); + float yaw2 = tf2::getYaw(path.poses[idx + 1].pose.orientation); + float rotation = fabs(angles::shortest_angular_distance(yaw1, yaw2)); + + // TYLKO DO LOGÓW: zapamiętaj największą rotację poniżej progu + if (rotation < rotation_threshold && + rotation > max_rotation_below_threshold) + { + max_rotation_below_threshold = rotation; + } + + // Check if this meets the minimum rotation threshold + if (rotation > rotation_threshold) { + // Found start of in-place rotation, now find where it ends + unsigned int end_idx = idx + 1; + + // Continue while we have minimal translation (still rotating in place) + while (end_idx < path.poses.size() - 1) { + float next_dx = path.poses[end_idx + 1].pose.position.x - + path.poses[end_idx].pose.position.x; + float next_dy = path.poses[end_idx + 1].pose.position.y - + path.poses[end_idx].pose.position.y; + float next_translation = sqrtf(next_dx * next_dx + next_dy * next_dy); + + if (next_translation >= 1e-3) { + break; // End of in-place rotation sequence + } + end_idx++; + } + + first_after_constraint = end_idx; + + // LOG: wykryto rotation constraint + RCLCPP_INFO( + rclcpp::get_logger("mppi_utils"), + "[MPPI PathUtils] Rotation constraint detected: rotation=%.3f rad, " + "threshold=%.3f rad, start_idx=%u, constraint_idx=%u", + rotation, rotation_threshold, idx, end_idx); + + break; + } + } + } + } + + // Check for inversion (at least 3 poses needed) + if (path.poses.size() >= 3) { + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + float oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + float oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + float ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + float ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + float dot_product = (oa_x * ab_x) + (oa_y * ab_y); + + float norm_oa = hypotf(oa_x, oa_y); + float norm_ab = hypotf(ab_x, ab_y); + + if (norm_oa < 1e-4f || norm_ab < 1e-4f) { + continue; + } + + float cos_ang = dot_product / (norm_oa * norm_ab); + cos_ang = std::max(-1.0f, std::min(1.0f, cos_ang)); + float angle = acosf(cos_ang); // [0, pi] + + if (rotation_threshold > 0.0f) { + if (angle > rotation_threshold) { + first_after_constraint = std::min(first_after_constraint, idx + 1); + + // LOG: constraint z powodu kąta (cusp/inversion po łuku) + RCLCPP_INFO( + rclcpp::get_logger("mppi_utils"), + "[MPPI PathUtils] Inversion/cusp constraint detected by angle: " + "angle=%.3f rad (threshold=%.3f rad), at idx=%u (constraint_idx=%u)", + angle, rotation_threshold, idx, first_after_constraint); + + break; + } + } else { + if (dot_product < 0.0f) { + first_after_constraint = std::min(first_after_constraint, idx + 1); + + // LOG: klasyczny cusp przez dot_product<0 + RCLCPP_INFO( + rclcpp::get_logger("mppi_utils"), + "[MPPI PathUtils] Inversion cusp constraint detected by dot_product<0 " + "at idx=%u (constraint_idx=%u, dot=%.6f)", + idx, first_after_constraint, dot_product); + + break; + } + } + } + } + + // Podsumowanie: nic nie znalazł -> zobacz, czy rotacja była \"prawie\" wystarczająca + if (first_after_constraint == path.poses.size()) { + if (rotation_threshold > 0.0f && max_rotation_below_threshold > 0.0f) { + float ratio = max_rotation_below_threshold / rotation_threshold; + + if (ratio >= 0.9f) { + // LOG: rotacja była blisko progu (>= 90%) + RCLCPP_INFO( + rclcpp::get_logger("mppi_utils"), + "[MPPI PathUtils] Rotation almost met threshold: rotation=%.3f rad, " + "threshold=%.3f rad (%.1f%% of threshold) – no constraint applied", + max_rotation_below_threshold, rotation_threshold, ratio * 100.0f); + } else { + RCLCPP_INFO( + rclcpp::get_logger("mppi_utils"), + "[MPPI PathUtils] No constraint found. Max rotation below threshold: " + "rotation=%.3f rad, threshold=%.3f rad (%.1f%% of threshold)", + max_rotation_below_threshold, rotation_threshold, ratio * 100.0f); + } + } else { + RCLCPP_INFO( + rclcpp::get_logger("mppi_utils"), + "[MPPI PathUtils] No inversion / rotation constraint found " + "(path_size=%zu, rotation_threshold=%.3f)", + path.poses.size(), rotation_threshold); + } + } + + return first_after_constraint; +} + + +/** + * @brief Find and remove poses after the first inversion or in-place rotation in the path + * @param path Path to check for inversion or rotation + * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check) + * @return The location of the inversion/rotation, return 0 if none exist + */ +inline unsigned int removePosesAfterFirstInversion( + nav_msgs::msg::Path & path, + float rotation_threshold = 0.0f) +{ + nav_msgs::msg::Path cropped_path = path; + + const unsigned int first_after_constraint = findFirstPathInversion(cropped_path, rotation_threshold); + + if (first_after_constraint == path.poses.size()) { + return 0u; // No constraint found + } + + cropped_path.poses.erase( + cropped_path.poses.begin() + first_after_constraint, cropped_path.poses.end()); + path = cropped_path; + return first_after_constraint; +} + +/** + * @brief Compare to trajectory points to find closest path point along integrated distances + * @param vec Vect to check + * @return dist Distance to look for + * @return init Starting index to indec from + */ +inline unsigned int findClosestPathPt( + const std::vector & vec, const float dist, const unsigned int init = 0u) +{ + float distim1 = init != 0u ? vec[init] : 0.0f; // First is 0, no accumulated distance yet + float disti = 0.0f; + const unsigned int size = vec.size(); + for (unsigned int i = init + 1; i != size; i++) { + disti = vec[i]; + if (disti > dist) { + if (i > 0 && dist - distim1 < disti - dist) { + return i - 1; + } + return i; + } + distim1 = disti; + } + return size - 1; +} + +// A struct to hold pose data in floating point resolution +struct Pose2D +{ + float x, y, theta; +}; + +/** + * @brief Shift the columns of a 2D Eigen Array or scalar values of + * 1D Eigen Array by 1 place. + * @param e Eigen Array + * @param direction direction in which Array will be shifted. + * 1 for shift in right direction and -1 for left direction. + */ +inline void shiftColumnsByOnePlace(Eigen::Ref e, int direction) +{ + int size = e.size(); + if(size == 1) {return;} + if(abs(direction) != 1) { + throw std::logic_error("Invalid direction, only 1 and -1 are valid values."); + } + + if((e.cols() == 1 || e.rows() == 1) && size > 1) { + auto start_ptr = direction == 1 ? e.data() + size - 2 : e.data() + 1; + auto end_ptr = direction == 1 ? e.data() : e.data() + size - 1; + while(start_ptr != end_ptr) { + *(start_ptr + direction) = *start_ptr; + start_ptr -= direction; + } + *(start_ptr + direction) = *start_ptr; + } else { + auto start_ptr = direction == 1 ? e.data() + size - 2 * e.rows() : e.data() + e.rows(); + auto end_ptr = direction == 1 ? e.data() : e.data() + size - e.rows(); + auto span = e.rows(); + while(start_ptr != end_ptr) { + std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); + start_ptr -= (direction * span); + } + std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); + } +} + +/** + * @brief Normalize the yaws between points on the basis of final yaw angle + * of the trajectory. + * @param last_yaws Final yaw angles of the trajectories. + * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories. + * @return Normalized yaw between points. + */ +inline auto normalize_yaws_between_points( + const Eigen::Ref & last_yaws, + const Eigen::Ref & yaw_between_points) +{ + Eigen::ArrayXf yaws = utils::shortest_angular_distance( + last_yaws, yaw_between_points).abs(); + int size = yaws.size(); + Eigen::ArrayXf yaws_between_points_corrected(size); + for(int i = 0; i != size; i++) { + const float & yaw_between_point = yaw_between_points[i]; + yaws_between_points_corrected[i] = yaws[i] < M_PIF_2 ? + yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF); + } + return yaws_between_points_corrected; +} + +/** + * @brief Normalize the yaws between points on the basis of goal angle. + * @param goal_yaw Goal yaw angle. + * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories. + * @return Normalized yaw between points + */ +inline auto normalize_yaws_between_points( + const float goal_yaw, const Eigen::Ref & yaw_between_points) +{ + int size = yaw_between_points.size(); + Eigen::ArrayXf yaws_between_points_corrected(size); + for(int i = 0; i != size; i++) { + const float & yaw_between_point = yaw_between_points[i]; + yaws_between_points_corrected[i] = fabs( + angles::normalize_angle(yaw_between_point - goal_yaw)) < M_PIF_2 ? + yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF); + } + return yaws_between_points_corrected; +} + +/** + * @brief Clamps the input between the given lower and upper bounds. + * @param lower_bound Lower bound. + * @param upper_bound Upper bound. + * @return Clamped output. + */ +inline float clamp( + const float lower_bound, const float upper_bound, const float input) +{ + return std::min(upper_bound, std::max(input, lower_bound)); +} + +} // namespace mppi::utils + +#endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy_mod.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy_mod.hpp new file mode 100644 index 00000000000..0ad511a351f --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy_mod.hpp @@ -0,0 +1,757 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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. + +#ifndef NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ +#define NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include "angles/angles.h" + +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/trajectory.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "nav2_ros_common/node_utils.hpp" +#include "nav2_core/goal_checker.hpp" + +#include "nav2_mppi_controller/models/optimizer_settings.hpp" +#include "nav2_mppi_controller/models/control_sequence.hpp" +#include "nav2_mppi_controller/models/path.hpp" +#include "builtin_interfaces/msg/time.hpp" +#include "nav2_mppi_controller/critic_data.hpp" + +#define M_PIF 3.141592653589793238462643383279502884e+00F +#define M_PIF_2 1.5707963267948966e+00F + +namespace mppi::utils +{ +/** + * @brief Convert data into pose + * @param x X position + * @param y Y position + * @param z Z position + * @return Pose object + */ +inline geometry_msgs::msg::Pose createPose(double x, double y, double z) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.position.z = z; + pose.orientation.w = 1; + pose.orientation.x = 0; + pose.orientation.y = 0; + pose.orientation.z = 0; + return pose; +} + +/** + * @brief Convert data into scale + * @param x X scale + * @param y Y scale + * @param z Z scale + * @return Scale object + */ +inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z) +{ + geometry_msgs::msg::Vector3 scale; + scale.x = x; + scale.y = y; + scale.z = z; + return scale; +} + +/** + * @brief Convert data into color + * @param r Red component + * @param g Green component + * @param b Blue component + * @param a Alpha component (transparency) + * @return Color object + */ +inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a) +{ + std_msgs::msg::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +/** + * @brief Convert data into a Maarker + * @param id Marker ID + * @param pose Marker pose + * @param scale Marker scale + * @param color Marker color + * @param frame Reference frame to use + * @return Visualization Marker + */ +inline visualization_msgs::msg::Marker createMarker( + int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale, + const std_msgs::msg::ColorRGBA & color, const std::string & frame_id, const std::string & ns) +{ + using visualization_msgs::msg::Marker; + Marker marker; + marker.header.frame_id = frame_id; + marker.header.stamp = rclcpp::Time(0, 0); + marker.ns = ns; + marker.id = id; + marker.type = Marker::SPHERE; + marker.action = Marker::ADD; + + marker.pose = pose; + marker.scale = scale; + marker.color = color; + return marker; +} + +/** + * @brief Convert data into TwistStamped + * @param vx X velocity + * @param wz Angular velocity + * @param stamp Timestamp + * @param frame Reference frame to use + */ +inline geometry_msgs::msg::TwistStamped toTwistStamped( + float vx, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame) +{ + geometry_msgs::msg::TwistStamped twist; + twist.header.frame_id = frame; + twist.header.stamp = stamp; + twist.twist.linear.x = vx; + twist.twist.angular.z = wz; + + return twist; +} + +/** + * @brief Convert data into TwistStamped + * @param vx X velocity + * @param vy Y velocity + * @param wz Angular velocity + * @param stamp Timestamp + * @param frame Reference frame to use + */ +inline geometry_msgs::msg::TwistStamped toTwistStamped( + float vx, float vy, float wz, const builtin_interfaces::msg::Time & stamp, + const std::string & frame) +{ + auto twist = toTwistStamped(vx, wz, stamp, frame); + twist.twist.linear.y = vy; + + return twist; +} + +inline std::unique_ptr toTrajectoryMsg( + const Eigen::ArrayXXf & trajectory, + const models::ControlSequence & control_sequence, + const double & model_dt, + const std_msgs::msg::Header & header) +{ + auto trajectory_msg = std::make_unique(); + trajectory_msg->header = header; + trajectory_msg->points.resize(trajectory.rows()); + + for (int i = 0; i < trajectory.rows(); ++i) { + auto & curr_pt = trajectory_msg->points[i]; + curr_pt.time_from_start = rclcpp::Duration::from_seconds(i * model_dt); + curr_pt.pose.position.x = trajectory(i, 0); + curr_pt.pose.position.y = trajectory(i, 1); + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, trajectory(i, 2)); + curr_pt.pose.orientation = tf2::toMsg(quat); + curr_pt.velocity.linear.x = control_sequence.vx(i); + curr_pt.velocity.angular.z = control_sequence.wz(i); + if (control_sequence.vy.size() > 0) { + curr_pt.velocity.linear.y = control_sequence.vy(i); + } + } + + return trajectory_msg; +} + +/** + * @brief Convert path to a tensor + * @param path Path to convert + * @return Path tensor + */ +inline models::Path toTensor(const nav_msgs::msg::Path & path) +{ + auto result = models::Path{}; + result.reset(path.poses.size()); + + for (size_t i = 0; i < path.poses.size(); ++i) { + result.x(i) = path.poses[i].pose.position.x; + result.y(i) = path.poses[i].pose.position.y; + result.yaws(i) = tf2::getYaw(path.poses[i].pose.orientation); + } + + return result; +} + +/** + * @brief Get the last pose from a path + * @param path Reference to the path + * @return geometry_msgs::msg::Pose Last pose in the path + */ +inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path) +{ + const unsigned int path_last_idx = path.x.size() - 1; + + auto last_orientation = path.yaws(path_last_idx); + + tf2::Quaternion pose_orientation; + pose_orientation.setRPY(0.0, 0.0, last_orientation); + + geometry_msgs::msg::Pose pathPose; + pathPose.position.x = path.x(path_last_idx); + pathPose.position.y = path.y(path_last_idx); + pathPose.orientation.x = pose_orientation.x(); + pathPose.orientation.y = pose_orientation.y(); + pathPose.orientation.z = pose_orientation.z(); + pathPose.orientation.w = pose_orientation.w(); + + return pathPose; +} + +/** + * @brief normalize + * Normalizes the angle to be -M_PIF circle to +M_PIF circle + * It takes and returns radians. + * @param angles Angles to normalize + * @return normalized angles + */ +template +auto normalize_angles(const T & angles) +{ + return (angles + M_PIF).unaryExpr([&](const float x) { + float remainder = std::fmod(x, 2.0f * M_PIF); + return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF; + }); +} + +/** + * @brief shortest_angular_distance + * + * Given 2 angles, this returns the shortest angular + * difference. The inputs and outputs are of course radians. + * + * The result + * would always be -pi <= result <= pi. Adding the result + * to "from" will always get you an equivalent angle to "to". + * @param from Start angle + * @param to End angle + * @return Shortest distance between angles + */ +template +auto shortest_angular_distance( + const F & from, + const T & to) +{ + return normalize_angles(to - from); +} + +/** + * @brief Evaluate furthest point idx of data.path which is + * nearest to some trajectory in data.trajectories + * @param data Data to use + * @return Idx of furthest path point reached by a set of trajectories + */ +inline size_t findPathFurthestReachedPoint(const CriticData & data) +{ + int traj_cols = data.trajectories.x.cols(); + const auto traj_x = data.trajectories.x.col(traj_cols - 1); + const auto traj_y = data.trajectories.y.col(traj_cols - 1); + + const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1).colwise() - traj_x; + const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1).colwise() - traj_y; + + const auto dists = dx * dx + dy * dy; + + int max_id_by_trajectories = 0, min_id_by_path = 0; + float min_distance_by_path = std::numeric_limits::max(); + size_t n_rows = dists.rows(); + size_t n_cols = dists.cols(); + for (size_t i = 0; i != n_rows; i++) { + min_id_by_path = 0; + min_distance_by_path = std::numeric_limits::max(); + for (size_t j = max_id_by_trajectories; j != n_cols; j++) { + const float cur_dist = dists(i, j); + if (cur_dist < min_distance_by_path) { + min_distance_by_path = cur_dist; + min_id_by_path = j; + } + } + max_id_by_trajectories = std::max(max_id_by_trajectories, min_id_by_path); + } + return max_id_by_trajectories; +} + +/** + * @brief evaluate path furthest point if it is not set + * @param data Data to use + */ +inline void setPathFurthestPointIfNotSet(CriticData & data) +{ + if (!data.furthest_reached_path_point) { + data.furthest_reached_path_point = findPathFurthestReachedPoint(data); + } +} + +/** + * @brief evaluate path costs + * @param data Data to use + */ +inline void findPathCosts( + CriticData & data, + std::shared_ptr costmap_ros) +{ + auto * costmap = costmap_ros->getCostmap(); + unsigned int map_x, map_y; + const size_t path_segments_count = data.path.x.size() - 1; + data.path_pts_valid = std::vector(path_segments_count, false); + const bool tracking_unknown = costmap_ros->getLayeredCostmap()->isTrackingUnknown(); + for (unsigned int idx = 0; idx < path_segments_count; idx++) { + if (!costmap->worldToMap(data.path.x(idx), data.path.y(idx), map_x, map_y)) { + (*data.path_pts_valid)[idx] = false; + continue; + } + + switch (costmap->getCost(map_x, map_y)) { + case (nav2_costmap_2d::LETHAL_OBSTACLE): + (*data.path_pts_valid)[idx] = false; + continue; + case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): + (*data.path_pts_valid)[idx] = false; + continue; + case (nav2_costmap_2d::NO_INFORMATION): + (*data.path_pts_valid)[idx] = tracking_unknown ? true : false; + continue; + } + + (*data.path_pts_valid)[idx] = true; + } +} + +/** + * @brief evaluate path costs if it is not set + * @param data Data to use + */ +inline void setPathCostsIfNotSet( + CriticData & data, + std::shared_ptr costmap_ros) +{ + if (!data.path_pts_valid) { + findPathCosts(data, costmap_ros); + } +} + +/** + * @brief evaluate angle from pose (have angle) to point (no angle) + * @param pose pose + * @param point_x Point to find angle relative to X axis + * @param point_y Point to find angle relative to Y axis + * @param forward_preference If reversing direction is valid + * @return Angle between two points + */ +inline float posePointAngle( + const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) +{ + float pose_x = pose.position.x; + float pose_y = pose.position.y; + float pose_yaw = tf2::getYaw(pose.orientation); + + float yaw = atan2f(point_y - pose_y, point_x - pose_x); + + // If no preference for forward, return smallest angle either in heading or 180 of heading + if (!forward_preference) { + return std::min( + fabs(angles::shortest_angular_distance(yaw, pose_yaw)), + fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PIF)))); + } + + return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); +} + +/** + * @brief evaluate angle from pose (have angle) to point (no angle) + * @param pose pose + * @param point_x Point to find angle relative to X axis + * @param point_y Point to find angle relative to Y axis + * @param point_yaw Yaw of the point to consider along Z axis + * @return Angle between two points + */ +inline float posePointAngle( + const geometry_msgs::msg::Pose & pose, + double point_x, double point_y, double point_yaw) +{ + float pose_x = static_cast(pose.position.x); + float pose_y = static_cast(pose.position.y); + float pose_yaw = static_cast(tf2::getYaw(pose.orientation)); + + float yaw = atan2f(static_cast(point_y) - pose_y, static_cast(point_x) - pose_x); + + if (fabs(angles::shortest_angular_distance(yaw, static_cast(point_yaw))) > M_PIF_2) { + yaw = angles::normalize_angle(yaw + M_PIF); + } + + return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); +} + +/** + * @brief Apply Savisky-Golay filter to optimal trajectory + * @param control_sequence Sequence to apply filter to + * @param control_history Recent set of controls for edge-case handling + * @param Settings Settings to use + */ +inline void savitskyGolayFilter( + models::ControlSequence & control_sequence, + std::array & control_history, + const models::OptimizerSettings & settings) +{ + // Savitzky-Golay Quadratic, 9-point Coefficients + Eigen::Array filter = {-21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f, + -21.0f}; + filter /= 231.0f; + + // Too short to smooth meaningfully + const unsigned int num_sequences = control_sequence.vx.size() - 1; + if (num_sequences < 20) { + return; + } + + auto applyFilter = [&](const Eigen::Array & data) -> float { + return (data * filter).eval().sum(); + }; + + auto applyFilterOverAxis = + [&](Eigen::ArrayXf & sequence, const Eigen::ArrayXf & initial_sequence, + const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void + { + float pt_m4 = hist_0; + float pt_m3 = hist_1; + float pt_m2 = hist_2; + float pt_m1 = hist_3; + float pt = initial_sequence(0); + float pt_p1 = initial_sequence(1); + float pt_p2 = initial_sequence(2); + float pt_p3 = initial_sequence(3); + float pt_p4 = initial_sequence(4); + + for (unsigned int idx = 0; idx != num_sequences; idx++) { + sequence(idx) = applyFilter({pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4}); + pt_m4 = pt_m3; + pt_m3 = pt_m2; + pt_m2 = pt_m1; + pt_m1 = pt; + pt = pt_p1; + pt_p1 = pt_p2; + pt_p2 = pt_p3; + pt_p3 = pt_p4; + + if (idx + 5 < num_sequences) { + pt_p4 = initial_sequence(idx + 5); + } else { + // Return the last point + pt_p4 = initial_sequence(num_sequences); + } + } + }; + + // Filter trajectories + const models::ControlSequence initial_control_sequence = control_sequence; + applyFilterOverAxis( + control_sequence.vx, initial_control_sequence.vx, control_history[0].vx, + control_history[1].vx, control_history[2].vx, control_history[3].vx); + applyFilterOverAxis( + control_sequence.vy, initial_control_sequence.vy, control_history[0].vy, + control_history[1].vy, control_history[2].vy, control_history[3].vy); + applyFilterOverAxis( + control_sequence.wz, initial_control_sequence.wz, control_history[0].wz, + control_history[1].wz, control_history[2].wz, control_history[3].wz); + + // Update control history + unsigned int offset = settings.shift_control_sequence ? 1 : 0; + control_history[0] = control_history[1]; + control_history[1] = control_history[2]; + control_history[2] = control_history[3]; + control_history[3] = { + control_sequence.vx(offset), + control_sequence.vy(offset), + control_sequence.wz(offset)}; +} + +/** + * @brief Find the iterator of the first pose at which there is an inversion or in-place rotation in the path + * @param path to check for inversion or rotation + * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check) + * @return the first point after the inversion or rotation found in the path + */ +inline unsigned int findFirstPathInversion( + nav_msgs::msg::Path & path, + float rotation_threshold = 0.0f) +{ + if (path.poses.size() < 2) { + return path.poses.size(); + } + + unsigned int first_after_constraint = path.poses.size(); + + // Check for in-place rotation first (if enabled) + if (rotation_threshold > 0.0f) { + for (unsigned int idx = 0; idx < path.poses.size() - 1; ++idx) { + float dx = path.poses[idx + 1].pose.position.x - path.poses[idx].pose.position.x; + float dy = path.poses[idx + 1].pose.position.y - path.poses[idx].pose.position.y; + float translation = sqrtf(dx * dx + dy * dy); + + // Check if poses are at roughly the same location + if (translation < 1e-3) { + float yaw1 = tf2::getYaw(path.poses[idx].pose.orientation); + float yaw2 = tf2::getYaw(path.poses[idx + 1].pose.orientation); + float rotation = fabs(angles::shortest_angular_distance(yaw1, yaw2)); + + // Check if this meets the minimum rotation threshold + if (rotation > rotation_threshold) { + // Found start of in-place rotation, now find where it ends + unsigned int end_idx = idx + 1; + + // Continue while we have minimal translation (still rotating in place) + while (end_idx < path.poses.size() - 1) { + float next_dx = path.poses[end_idx + 1].pose.position.x - + path.poses[end_idx].pose.position.x; + float next_dy = path.poses[end_idx + 1].pose.position.y - + path.poses[end_idx].pose.position.y; + float next_translation = sqrtf(next_dx * next_dx + next_dy * next_dy); + + if (next_translation >= 1e-3) { + break; // End of in-place rotation sequence + } + end_idx++; + } + + first_after_constraint = end_idx; + break; + } + } + } + } + + // Check for inversion (at least 3 poses needed) + if (path.poses.size() >= 3) { + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + float oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + float oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + float ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + float ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + float dot_product = (oa_x * ab_x) + (oa_y * ab_y); + + float norm_oa = hypotf(oa_x, oa_y); + float norm_ab = hypotf(ab_x, ab_y); + + if (norm_oa < 1e-4f || norm_ab < 1e-4f) { + continue; + } + + float cos_ang = dot_product / (norm_oa * norm_ab); + cos_ang = std::max(-1.0f, std::min(1.0f, cos_ang)); + float angle = acosf(cos_ang); // [0, pi] + + if (rotation_threshold > 0.0f) { + if (angle > rotation_threshold) { + first_after_constraint = std::min(first_after_constraint, idx + 1); + break; + } + } else { + if (dot_product < 0.0f) { + first_after_constraint = std::min(first_after_constraint, idx + 1); + break; + } + } + } + } + + return first_after_constraint; +} + +/** + * @brief Find and remove poses after the first inversion or in-place rotation in the path + * @param path Path to check for inversion or rotation + * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check) + * @return The location of the inversion/rotation, return 0 if none exist + */ +inline unsigned int removePosesAfterFirstInversion( + nav_msgs::msg::Path & path, + float rotation_threshold = 0.0f) +{ + nav_msgs::msg::Path cropped_path = path; + + const unsigned int first_after_constraint = findFirstPathInversion(cropped_path, rotation_threshold); + + if (first_after_constraint == path.poses.size()) { + return 0u; // No constraint found + } + + cropped_path.poses.erase( + cropped_path.poses.begin() + first_after_constraint, cropped_path.poses.end()); + path = cropped_path; + return first_after_constraint; +} + +/** + * @brief Compare to trajectory points to find closest path point along integrated distances + * @param vec Vect to check + * @return dist Distance to look for + * @return init Starting index to indec from + */ +inline unsigned int findClosestPathPt( + const std::vector & vec, const float dist, const unsigned int init = 0u) +{ + float distim1 = init != 0u ? vec[init] : 0.0f; // First is 0, no accumulated distance yet + float disti = 0.0f; + const unsigned int size = vec.size(); + for (unsigned int i = init + 1; i != size; i++) { + disti = vec[i]; + if (disti > dist) { + if (i > 0 && dist - distim1 < disti - dist) { + return i - 1; + } + return i; + } + distim1 = disti; + } + return size - 1; +} + +// A struct to hold pose data in floating point resolution +struct Pose2D +{ + float x, y, theta; +}; + +/** + * @brief Shift the columns of a 2D Eigen Array or scalar values of + * 1D Eigen Array by 1 place. + * @param e Eigen Array + * @param direction direction in which Array will be shifted. + * 1 for shift in right direction and -1 for left direction. + */ +inline void shiftColumnsByOnePlace(Eigen::Ref e, int direction) +{ + int size = e.size(); + if(size == 1) {return;} + if(abs(direction) != 1) { + throw std::logic_error("Invalid direction, only 1 and -1 are valid values."); + } + + if((e.cols() == 1 || e.rows() == 1) && size > 1) { + auto start_ptr = direction == 1 ? e.data() + size - 2 : e.data() + 1; + auto end_ptr = direction == 1 ? e.data() : e.data() + size - 1; + while(start_ptr != end_ptr) { + *(start_ptr + direction) = *start_ptr; + start_ptr -= direction; + } + *(start_ptr + direction) = *start_ptr; + } else { + auto start_ptr = direction == 1 ? e.data() + size - 2 * e.rows() : e.data() + e.rows(); + auto end_ptr = direction == 1 ? e.data() : e.data() + size - e.rows(); + auto span = e.rows(); + while(start_ptr != end_ptr) { + std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); + start_ptr -= (direction * span); + } + std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); + } +} + +/** + * @brief Normalize the yaws between points on the basis of final yaw angle + * of the trajectory. + * @param last_yaws Final yaw angles of the trajectories. + * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories. + * @return Normalized yaw between points. + */ +inline auto normalize_yaws_between_points( + const Eigen::Ref & last_yaws, + const Eigen::Ref & yaw_between_points) +{ + Eigen::ArrayXf yaws = utils::shortest_angular_distance( + last_yaws, yaw_between_points).abs(); + int size = yaws.size(); + Eigen::ArrayXf yaws_between_points_corrected(size); + for(int i = 0; i != size; i++) { + const float & yaw_between_point = yaw_between_points[i]; + yaws_between_points_corrected[i] = yaws[i] < M_PIF_2 ? + yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF); + } + return yaws_between_points_corrected; +} + +/** + * @brief Normalize the yaws between points on the basis of goal angle. + * @param goal_yaw Goal yaw angle. + * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories. + * @return Normalized yaw between points + */ +inline auto normalize_yaws_between_points( + const float goal_yaw, const Eigen::Ref & yaw_between_points) +{ + int size = yaw_between_points.size(); + Eigen::ArrayXf yaws_between_points_corrected(size); + for(int i = 0; i != size; i++) { + const float & yaw_between_point = yaw_between_points[i]; + yaws_between_points_corrected[i] = fabs( + angles::normalize_angle(yaw_between_point - goal_yaw)) < M_PIF_2 ? + yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF); + } + return yaws_between_points_corrected; +} + +/** + * @brief Clamps the input between the given lower and upper bounds. + * @param lower_bound Lower bound. + * @param upper_bound Upper bound. + * @return Clamped output. + */ +inline float clamp( + const float lower_bound, const float upper_bound, const float input) +{ + return std::min(upper_bound, std::max(input, lower_bound)); +} + +} // namespace mppi::utils + +#endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy_original.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy_original.hpp new file mode 100644 index 00000000000..e2f6a925d36 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy_original.hpp @@ -0,0 +1,705 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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. + +#ifndef NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ +#define NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include "angles/angles.h" + +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/trajectory.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "nav2_ros_common/node_utils.hpp" +#include "nav2_core/goal_checker.hpp" + +#include "nav2_mppi_controller/models/optimizer_settings.hpp" +#include "nav2_mppi_controller/models/control_sequence.hpp" +#include "nav2_mppi_controller/models/path.hpp" +#include "builtin_interfaces/msg/time.hpp" +#include "nav2_mppi_controller/critic_data.hpp" + +#define M_PIF 3.141592653589793238462643383279502884e+00F +#define M_PIF_2 1.5707963267948966e+00F + +namespace mppi::utils +{ +/** + * @brief Convert data into pose + * @param x X position + * @param y Y position + * @param z Z position + * @return Pose object + */ +inline geometry_msgs::msg::Pose createPose(double x, double y, double z) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.position.z = z; + pose.orientation.w = 1; + pose.orientation.x = 0; + pose.orientation.y = 0; + pose.orientation.z = 0; + return pose; +} + +/** + * @brief Convert data into scale + * @param x X scale + * @param y Y scale + * @param z Z scale + * @return Scale object + */ +inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z) +{ + geometry_msgs::msg::Vector3 scale; + scale.x = x; + scale.y = y; + scale.z = z; + return scale; +} + +/** + * @brief Convert data into color + * @param r Red component + * @param g Green component + * @param b Blue component + * @param a Alpha component (transparency) + * @return Color object + */ +inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a) +{ + std_msgs::msg::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +/** + * @brief Convert data into a Maarker + * @param id Marker ID + * @param pose Marker pose + * @param scale Marker scale + * @param color Marker color + * @param frame Reference frame to use + * @return Visualization Marker + */ +inline visualization_msgs::msg::Marker createMarker( + int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale, + const std_msgs::msg::ColorRGBA & color, const std::string & frame_id, const std::string & ns) +{ + using visualization_msgs::msg::Marker; + Marker marker; + marker.header.frame_id = frame_id; + marker.header.stamp = rclcpp::Time(0, 0); + marker.ns = ns; + marker.id = id; + marker.type = Marker::SPHERE; + marker.action = Marker::ADD; + + marker.pose = pose; + marker.scale = scale; + marker.color = color; + return marker; +} + +/** + * @brief Convert data into TwistStamped + * @param vx X velocity + * @param wz Angular velocity + * @param stamp Timestamp + * @param frame Reference frame to use + */ +inline geometry_msgs::msg::TwistStamped toTwistStamped( + float vx, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame) +{ + geometry_msgs::msg::TwistStamped twist; + twist.header.frame_id = frame; + twist.header.stamp = stamp; + twist.twist.linear.x = vx; + twist.twist.angular.z = wz; + + return twist; +} + +/** + * @brief Convert data into TwistStamped + * @param vx X velocity + * @param vy Y velocity + * @param wz Angular velocity + * @param stamp Timestamp + * @param frame Reference frame to use + */ +inline geometry_msgs::msg::TwistStamped toTwistStamped( + float vx, float vy, float wz, const builtin_interfaces::msg::Time & stamp, + const std::string & frame) +{ + auto twist = toTwistStamped(vx, wz, stamp, frame); + twist.twist.linear.y = vy; + + return twist; +} + +inline std::unique_ptr toTrajectoryMsg( + const Eigen::ArrayXXf & trajectory, + const models::ControlSequence & control_sequence, + const double & model_dt, + const std_msgs::msg::Header & header) +{ + auto trajectory_msg = std::make_unique(); + trajectory_msg->header = header; + trajectory_msg->points.resize(trajectory.rows()); + + for (int i = 0; i < trajectory.rows(); ++i) { + auto & curr_pt = trajectory_msg->points[i]; + curr_pt.time_from_start = rclcpp::Duration::from_seconds(i * model_dt); + curr_pt.pose.position.x = trajectory(i, 0); + curr_pt.pose.position.y = trajectory(i, 1); + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, trajectory(i, 2)); + curr_pt.pose.orientation = tf2::toMsg(quat); + curr_pt.velocity.linear.x = control_sequence.vx(i); + curr_pt.velocity.angular.z = control_sequence.wz(i); + if (control_sequence.vy.size() > 0) { + curr_pt.velocity.linear.y = control_sequence.vy(i); + } + } + + return trajectory_msg; +} + +/** + * @brief Convert path to a tensor + * @param path Path to convert + * @return Path tensor + */ +inline models::Path toTensor(const nav_msgs::msg::Path & path) +{ + auto result = models::Path{}; + result.reset(path.poses.size()); + + for (size_t i = 0; i < path.poses.size(); ++i) { + result.x(i) = path.poses[i].pose.position.x; + result.y(i) = path.poses[i].pose.position.y; + result.yaws(i) = tf2::getYaw(path.poses[i].pose.orientation); + } + + return result; +} + +/** + * @brief Get the last pose from a path + * @param path Reference to the path + * @return geometry_msgs::msg::Pose Last pose in the path + */ +inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path) +{ + const unsigned int path_last_idx = path.x.size() - 1; + + auto last_orientation = path.yaws(path_last_idx); + + tf2::Quaternion pose_orientation; + pose_orientation.setRPY(0.0, 0.0, last_orientation); + + geometry_msgs::msg::Pose pathPose; + pathPose.position.x = path.x(path_last_idx); + pathPose.position.y = path.y(path_last_idx); + pathPose.orientation.x = pose_orientation.x(); + pathPose.orientation.y = pose_orientation.y(); + pathPose.orientation.z = pose_orientation.z(); + pathPose.orientation.w = pose_orientation.w(); + + return pathPose; +} + +/** + * @brief normalize + * Normalizes the angle to be -M_PIF circle to +M_PIF circle + * It takes and returns radians. + * @param angles Angles to normalize + * @return normalized angles + */ +template +auto normalize_angles(const T & angles) +{ + return (angles + M_PIF).unaryExpr([&](const float x) { + float remainder = std::fmod(x, 2.0f * M_PIF); + return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF; + }); +} + +/** + * @brief shortest_angular_distance + * + * Given 2 angles, this returns the shortest angular + * difference. The inputs and outputs are of course radians. + * + * The result + * would always be -pi <= result <= pi. Adding the result + * to "from" will always get you an equivalent angle to "to". + * @param from Start angle + * @param to End angle + * @return Shortest distance between angles + */ +template +auto shortest_angular_distance( + const F & from, + const T & to) +{ + return normalize_angles(to - from); +} + +/** + * @brief Evaluate furthest point idx of data.path which is + * nearest to some trajectory in data.trajectories + * @param data Data to use + * @return Idx of furthest path point reached by a set of trajectories + */ +inline size_t findPathFurthestReachedPoint(const CriticData & data) +{ + int traj_cols = data.trajectories.x.cols(); + const auto traj_x = data.trajectories.x.col(traj_cols - 1); + const auto traj_y = data.trajectories.y.col(traj_cols - 1); + + const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1).colwise() - traj_x; + const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1).colwise() - traj_y; + + const auto dists = dx * dx + dy * dy; + + int max_id_by_trajectories = 0, min_id_by_path = 0; + float min_distance_by_path = std::numeric_limits::max(); + size_t n_rows = dists.rows(); + size_t n_cols = dists.cols(); + for (size_t i = 0; i != n_rows; i++) { + // Skip trajectories in collision + bool in_collision = data.trajectories_in_collision && + i < data.trajectories_in_collision->size() && + (*data.trajectories_in_collision)[i]; + if (in_collision) { + continue; + } + min_id_by_path = 0; + min_distance_by_path = std::numeric_limits::max(); + // Search all path points to find the true closest point + for (size_t j = 0; j != n_cols; j++) { + const float cur_dist = dists(i, j); + if (cur_dist < min_distance_by_path) { + min_distance_by_path = cur_dist; + min_id_by_path = j; + // If we've found the last path point as closest, can't do better for this trajectory + if (min_id_by_path >= static_cast(n_cols - 1)) { + break; + } + } + } + max_id_by_trajectories = std::max(max_id_by_trajectories, min_id_by_path); + // Early exit: if we've reached the last path point, we can't do better + if (max_id_by_trajectories >= static_cast(n_cols - 1)) { + break; + } + } + return max_id_by_trajectories; +} + +/** + * @brief evaluate path furthest point if it is not set + * @param data Data to use + */ +inline void setPathFurthestPointIfNotSet(CriticData & data) +{ + if (!data.furthest_reached_path_point) { + data.furthest_reached_path_point = findPathFurthestReachedPoint(data); + } +} + +/** + * @brief evaluate path costs + * @param data Data to use + */ +inline void findPathCosts( + CriticData & data, + std::shared_ptr costmap_ros) +{ + auto * costmap = costmap_ros->getCostmap(); + unsigned int map_x, map_y; + const size_t path_segments_count = data.path.x.size() - 1; + data.path_pts_valid = std::vector(path_segments_count, false); + const bool tracking_unknown = costmap_ros->getLayeredCostmap()->isTrackingUnknown(); + for (unsigned int idx = 0; idx < path_segments_count; idx++) { + if (!costmap->worldToMap(data.path.x(idx), data.path.y(idx), map_x, map_y)) { + (*data.path_pts_valid)[idx] = false; + continue; + } + + switch (costmap->getCost(map_x, map_y)) { + case (nav2_costmap_2d::LETHAL_OBSTACLE): + (*data.path_pts_valid)[idx] = false; + continue; + case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): + (*data.path_pts_valid)[idx] = false; + continue; + case (nav2_costmap_2d::NO_INFORMATION): + (*data.path_pts_valid)[idx] = tracking_unknown ? true : false; + continue; + } + + (*data.path_pts_valid)[idx] = true; + } +} + +/** + * @brief evaluate path costs if it is not set + * @param data Data to use + */ +inline void setPathCostsIfNotSet( + CriticData & data, + std::shared_ptr costmap_ros) +{ + if (!data.path_pts_valid) { + findPathCosts(data, costmap_ros); + } +} + +/** + * @brief evaluate angle from pose (have angle) to point (no angle) + * @param pose pose + * @param point_x Point to find angle relative to X axis + * @param point_y Point to find angle relative to Y axis + * @param forward_preference If reversing direction is valid + * @return Angle between two points + */ +inline float posePointAngle( + const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) +{ + float pose_x = pose.position.x; + float pose_y = pose.position.y; + float pose_yaw = tf2::getYaw(pose.orientation); + + float yaw = atan2f(point_y - pose_y, point_x - pose_x); + + // If no preference for forward, return smallest angle either in heading or 180 of heading + if (!forward_preference) { + return std::min( + fabs(angles::shortest_angular_distance(yaw, pose_yaw)), + fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PIF)))); + } + + return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); +} + +/** + * @brief evaluate angle from pose (have angle) to point (no angle) + * @param pose pose + * @param point_x Point to find angle relative to X axis + * @param point_y Point to find angle relative to Y axis + * @param point_yaw Yaw of the point to consider along Z axis + * @return Angle between two points + */ +inline float posePointAngle( + const geometry_msgs::msg::Pose & pose, + double point_x, double point_y, double point_yaw) +{ + float pose_x = static_cast(pose.position.x); + float pose_y = static_cast(pose.position.y); + float pose_yaw = static_cast(tf2::getYaw(pose.orientation)); + + float yaw = atan2f(static_cast(point_y) - pose_y, static_cast(point_x) - pose_x); + + if (fabs(angles::shortest_angular_distance(yaw, static_cast(point_yaw))) > M_PIF_2) { + yaw = angles::normalize_angle(yaw + M_PIF); + } + + return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); +} + +/** + * @brief Apply Savisky-Golay filter to optimal trajectory + * @param control_sequence Sequence to apply filter to + * @param control_history Recent set of controls for edge-case handling + * @param Settings Settings to use + */ +inline void savitskyGolayFilter( + models::ControlSequence & control_sequence, + std::array & control_history, + const models::OptimizerSettings & settings) +{ + // Savitzky-Golay Quadratic, 9-point Coefficients + Eigen::Array filter = {-21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f, + -21.0f}; + filter /= 231.0f; + + // Too short to smooth meaningfully + const unsigned int num_sequences = control_sequence.vx.size() - 1; + if (num_sequences < 20) { + return; + } + + auto applyFilter = [&](const Eigen::Array & data) -> float { + return (data * filter).eval().sum(); + }; + + auto applyFilterOverAxis = + [&](Eigen::ArrayXf & sequence, const Eigen::ArrayXf & initial_sequence, + const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void + { + float pt_m4 = hist_0; + float pt_m3 = hist_1; + float pt_m2 = hist_2; + float pt_m1 = hist_3; + float pt = initial_sequence(0); + float pt_p1 = initial_sequence(1); + float pt_p2 = initial_sequence(2); + float pt_p3 = initial_sequence(3); + float pt_p4 = initial_sequence(4); + + for (unsigned int idx = 0; idx != num_sequences; idx++) { + sequence(idx) = applyFilter({pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4}); + pt_m4 = pt_m3; + pt_m3 = pt_m2; + pt_m2 = pt_m1; + pt_m1 = pt; + pt = pt_p1; + pt_p1 = pt_p2; + pt_p2 = pt_p3; + pt_p3 = pt_p4; + + if (idx + 5 < num_sequences) { + pt_p4 = initial_sequence(idx + 5); + } else { + // Return the last point + pt_p4 = initial_sequence(num_sequences); + } + } + }; + + // Filter trajectories + const models::ControlSequence initial_control_sequence = control_sequence; + applyFilterOverAxis( + control_sequence.vx, initial_control_sequence.vx, control_history[0].vx, + control_history[1].vx, control_history[2].vx, control_history[3].vx); + applyFilterOverAxis( + control_sequence.vy, initial_control_sequence.vy, control_history[0].vy, + control_history[1].vy, control_history[2].vy, control_history[3].vy); + applyFilterOverAxis( + control_sequence.wz, initial_control_sequence.wz, control_history[0].wz, + control_history[1].wz, control_history[2].wz, control_history[3].wz); + + // Update control history + unsigned int offset = settings.shift_control_sequence ? 1 : 0; + control_history[0] = control_history[1]; + control_history[1] = control_history[2]; + control_history[2] = control_history[3]; + control_history[3] = { + control_sequence.vx(offset), + control_sequence.vy(offset), + control_sequence.wz(offset)}; +} + +/** + * @brief Find the iterator of the first pose at which there is an inversion on the path, + * @param path to check for inversion + * @return the first point after the inversion found in the path + */ +inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) +{ + // At least 3 poses for a possible inversion + if (path.poses.size() < 3) { + return path.poses.size(); + } + + // Iterating through the path to determine the position of the path inversion + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + float oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + float oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + float ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + float ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + // Checking for the existence of cusp, in the path, using the dot product. + float dot_product = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_product < 0.0f) { + return idx + 1; + } + } + + return path.poses.size(); +} + +/** + * @brief Find and remove poses after the first inversion in the path + * @param path to check for inversion + * @return The location of the inversion, return 0 if none exist + */ +inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) +{ + nav_msgs::msg::Path cropped_path = path; + const unsigned int first_after_inversion = findFirstPathInversion(cropped_path); + if (first_after_inversion == path.poses.size()) { + return 0u; + } + + cropped_path.poses.erase( + cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end()); + path = cropped_path; + return first_after_inversion; +} + +/** + * @brief Compare to trajectory points to find closest path point along integrated distances + * @param vec Vect to check + * @return dist Distance to look for + * @return init Starting index to indec from + */ +inline unsigned int findClosestPathPt( + const std::vector & vec, const float dist, const unsigned int init = 0u) +{ + float distim1 = init != 0u ? vec[init] : 0.0f; // First is 0, no accumulated distance yet + float disti = 0.0f; + const unsigned int size = vec.size(); + for (unsigned int i = init + 1; i != size; i++) { + disti = vec[i]; + if (disti > dist) { + if (i > 0 && dist - distim1 < disti - dist) { + return i - 1; + } + return i; + } + distim1 = disti; + } + return size - 1; +} + +// A struct to hold pose data in floating point resolution +struct Pose2D +{ + float x, y, theta; +}; + +/** + * @brief Shift the columns of a 2D Eigen Array or scalar values of + * 1D Eigen Array by 1 place. + * @param e Eigen Array + * @param direction direction in which Array will be shifted. + * 1 for shift in right direction and -1 for left direction. + */ +inline void shiftColumnsByOnePlace(Eigen::Ref e, int direction) +{ + int size = e.size(); + if(size == 1) {return;} + if(abs(direction) != 1) { + throw std::logic_error("Invalid direction, only 1 and -1 are valid values."); + } + + if((e.cols() == 1 || e.rows() == 1) && size > 1) { + auto start_ptr = direction == 1 ? e.data() + size - 2 : e.data() + 1; + auto end_ptr = direction == 1 ? e.data() : e.data() + size - 1; + while(start_ptr != end_ptr) { + *(start_ptr + direction) = *start_ptr; + start_ptr -= direction; + } + *(start_ptr + direction) = *start_ptr; + } else { + auto start_ptr = direction == 1 ? e.data() + size - 2 * e.rows() : e.data() + e.rows(); + auto end_ptr = direction == 1 ? e.data() : e.data() + size - e.rows(); + auto span = e.rows(); + while(start_ptr != end_ptr) { + std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); + start_ptr -= (direction * span); + } + std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); + } +} + +/** + * @brief Normalize the yaws between points on the basis of final yaw angle + * of the trajectory. + * @param last_yaws Final yaw angles of the trajectories. + * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories. + * @return Normalized yaw between points. + */ +inline auto normalize_yaws_between_points( + const Eigen::Ref & last_yaws, + const Eigen::Ref & yaw_between_points) +{ + Eigen::ArrayXf yaws = utils::shortest_angular_distance( + last_yaws, yaw_between_points).abs(); + int size = yaws.size(); + Eigen::ArrayXf yaws_between_points_corrected(size); + for(int i = 0; i != size; i++) { + const float & yaw_between_point = yaw_between_points[i]; + yaws_between_points_corrected[i] = yaws[i] < M_PIF_2 ? + yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF); + } + return yaws_between_points_corrected; +} + +/** + * @brief Normalize the yaws between points on the basis of goal angle. + * @param goal_yaw Goal yaw angle. + * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories. + * @return Normalized yaw between points + */ +inline auto normalize_yaws_between_points( + const float goal_yaw, const Eigen::Ref & yaw_between_points) +{ + int size = yaw_between_points.size(); + Eigen::ArrayXf yaws_between_points_corrected(size); + for(int i = 0; i != size; i++) { + const float & yaw_between_point = yaw_between_points[i]; + yaws_between_points_corrected[i] = fabs( + angles::normalize_angle(yaw_between_point - goal_yaw)) < M_PIF_2 ? + yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF); + } + return yaws_between_points_corrected; +} + +/** + * @brief Clamps the input between the given lower and upper bounds. + * @param lower_bound Lower bound. + * @param upper_bound Upper bound. + * @return Clamped output. + */ +inline float clamp( + const float lower_bound, const float upper_bound, const float input) +{ + return std::min(upper_bound, std::max(input, lower_bound)); +} + +} // namespace mppi::utils + +#endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index f518f16e169..0ad511a351f 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -243,81 +243,6 @@ inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path) return pathPose; } -/** - * @brief Get the target pose to be evaluated by the critic - * @param data Data to use - * @param enforce_path_inversion True to return the cusp point (last pose of the path) - * instead of the original goal - * @return geometry_msgs::msg::Pose Target pose for the critic - */ -inline geometry_msgs::msg::Pose getCriticGoal( - const CriticData & data, - bool enforce_path_inversion) -{ - if (enforce_path_inversion) { - return getLastPathPose(data.path); - } else { - return data.goal; - } -} - -/** - * @brief Check if the robot pose is within the Goal Checker's tolerances to goal - * @param global_checker Pointer to the goal checker - * @param robot Pose of robot - * @param goal Goal pose - * @return bool If robot is within goal checker tolerances to the goal - */ -inline bool withinPositionGoalTolerance( - nav2_core::GoalChecker * goal_checker, - const geometry_msgs::msg::Pose & robot, - const geometry_msgs::msg::Pose & goal) -{ - if (goal_checker) { - geometry_msgs::msg::Pose pose_tolerance; - geometry_msgs::msg::Twist velocity_tolerance; - goal_checker->getTolerances(pose_tolerance, velocity_tolerance); - - const auto pose_tolerance_sq = pose_tolerance.position.x * pose_tolerance.position.x; - - auto dx = robot.position.x - goal.position.x; - auto dy = robot.position.y - goal.position.y; - - auto dist_sq = dx * dx + dy * dy; - - if (dist_sq < pose_tolerance_sq) { - return true; - } - } - - return false; -} - -/** - * @brief Check if the robot pose is within tolerance to the goal - * @param pose_tolerance Pose tolerance to use - * @param robot Pose of robot - * @param goal Goal pose - * @return bool If robot is within tolerance to the goal - */ -inline bool withinPositionGoalTolerance( - float pose_tolerance, - const geometry_msgs::msg::Pose & robot, - const geometry_msgs::msg::Pose & goal) -{ - const double & dist_sq = - std::pow(goal.position.x - robot.position.x, 2) + - std::pow(goal.position.y - robot.position.y, 2); - - const float pose_tolerance_sq = pose_tolerance * pose_tolerance; - - if (dist_sq < pose_tolerance_sq) { - return true; - } - - return false; -} - /** * @brief normalize * Normalizes the angle to be -M_PIF circle to +M_PIF circle @@ -586,56 +511,124 @@ inline void savitskyGolayFilter( } /** - * @brief Find the iterator of the first pose at which there is an inversion on the path, - * @param path to check for inversion - * @return the first point after the inversion found in the path + * @brief Find the iterator of the first pose at which there is an inversion or in-place rotation in the path + * @param path to check for inversion or rotation + * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check) + * @return the first point after the inversion or rotation found in the path */ -inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) +inline unsigned int findFirstPathInversion( + nav_msgs::msg::Path & path, + float rotation_threshold = 0.0f) { - // At least 3 poses for a possible inversion - if (path.poses.size() < 3) { + if (path.poses.size() < 2) { return path.poses.size(); } - // Iterating through the path to determine the position of the path inversion - for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { - // We have two vectors for the dot product OA and AB. Determining the vectors. - float oa_x = path.poses[idx].pose.position.x - - path.poses[idx - 1].pose.position.x; - float oa_y = path.poses[idx].pose.position.y - - path.poses[idx - 1].pose.position.y; - float ab_x = path.poses[idx + 1].pose.position.x - - path.poses[idx].pose.position.x; - float ab_y = path.poses[idx + 1].pose.position.y - - path.poses[idx].pose.position.y; - - // Checking for the existence of cusp, in the path, using the dot product. - float dot_product = (oa_x * ab_x) + (oa_y * ab_y); - if (dot_product < 0.0f) { - return idx + 1; + unsigned int first_after_constraint = path.poses.size(); + + // Check for in-place rotation first (if enabled) + if (rotation_threshold > 0.0f) { + for (unsigned int idx = 0; idx < path.poses.size() - 1; ++idx) { + float dx = path.poses[idx + 1].pose.position.x - path.poses[idx].pose.position.x; + float dy = path.poses[idx + 1].pose.position.y - path.poses[idx].pose.position.y; + float translation = sqrtf(dx * dx + dy * dy); + + // Check if poses are at roughly the same location + if (translation < 1e-3) { + float yaw1 = tf2::getYaw(path.poses[idx].pose.orientation); + float yaw2 = tf2::getYaw(path.poses[idx + 1].pose.orientation); + float rotation = fabs(angles::shortest_angular_distance(yaw1, yaw2)); + + // Check if this meets the minimum rotation threshold + if (rotation > rotation_threshold) { + // Found start of in-place rotation, now find where it ends + unsigned int end_idx = idx + 1; + + // Continue while we have minimal translation (still rotating in place) + while (end_idx < path.poses.size() - 1) { + float next_dx = path.poses[end_idx + 1].pose.position.x - + path.poses[end_idx].pose.position.x; + float next_dy = path.poses[end_idx + 1].pose.position.y - + path.poses[end_idx].pose.position.y; + float next_translation = sqrtf(next_dx * next_dx + next_dy * next_dy); + + if (next_translation >= 1e-3) { + break; // End of in-place rotation sequence + } + end_idx++; + } + + first_after_constraint = end_idx; + break; + } + } } } - return path.poses.size(); + // Check for inversion (at least 3 poses needed) + if (path.poses.size() >= 3) { + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + float oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + float oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + float ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + float ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + float dot_product = (oa_x * ab_x) + (oa_y * ab_y); + + float norm_oa = hypotf(oa_x, oa_y); + float norm_ab = hypotf(ab_x, ab_y); + + if (norm_oa < 1e-4f || norm_ab < 1e-4f) { + continue; + } + + float cos_ang = dot_product / (norm_oa * norm_ab); + cos_ang = std::max(-1.0f, std::min(1.0f, cos_ang)); + float angle = acosf(cos_ang); // [0, pi] + + if (rotation_threshold > 0.0f) { + if (angle > rotation_threshold) { + first_after_constraint = std::min(first_after_constraint, idx + 1); + break; + } + } else { + if (dot_product < 0.0f) { + first_after_constraint = std::min(first_after_constraint, idx + 1); + break; + } + } + } + } + + return first_after_constraint; } /** - * @brief Find and remove poses after the first inversion in the path - * @param path to check for inversion - * @return The location of the inversion, return 0 if none exist + * @brief Find and remove poses after the first inversion or in-place rotation in the path + * @param path Path to check for inversion or rotation + * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check) + * @return The location of the inversion/rotation, return 0 if none exist */ -inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) +inline unsigned int removePosesAfterFirstInversion( + nav_msgs::msg::Path & path, + float rotation_threshold = 0.0f) { nav_msgs::msg::Path cropped_path = path; - const unsigned int first_after_inversion = findFirstPathInversion(cropped_path); - if (first_after_inversion == path.poses.size()) { - return 0u; + + const unsigned int first_after_constraint = findFirstPathInversion(cropped_path, rotation_threshold); + + if (first_after_constraint == path.poses.size()) { + return 0u; // No constraint found } cropped_path.poses.erase( - cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end()); + cropped_path.poses.begin() + first_after_constraint, cropped_path.poses.end()); path = cropped_path; - return first_after_inversion; + return first_after_constraint; } /** diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils_mod_final.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils_mod_final.hpp new file mode 100644 index 00000000000..0ad511a351f --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils_mod_final.hpp @@ -0,0 +1,757 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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. + +#ifndef NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ +#define NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include "angles/angles.h" + +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/trajectory.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "nav2_ros_common/node_utils.hpp" +#include "nav2_core/goal_checker.hpp" + +#include "nav2_mppi_controller/models/optimizer_settings.hpp" +#include "nav2_mppi_controller/models/control_sequence.hpp" +#include "nav2_mppi_controller/models/path.hpp" +#include "builtin_interfaces/msg/time.hpp" +#include "nav2_mppi_controller/critic_data.hpp" + +#define M_PIF 3.141592653589793238462643383279502884e+00F +#define M_PIF_2 1.5707963267948966e+00F + +namespace mppi::utils +{ +/** + * @brief Convert data into pose + * @param x X position + * @param y Y position + * @param z Z position + * @return Pose object + */ +inline geometry_msgs::msg::Pose createPose(double x, double y, double z) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.position.z = z; + pose.orientation.w = 1; + pose.orientation.x = 0; + pose.orientation.y = 0; + pose.orientation.z = 0; + return pose; +} + +/** + * @brief Convert data into scale + * @param x X scale + * @param y Y scale + * @param z Z scale + * @return Scale object + */ +inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z) +{ + geometry_msgs::msg::Vector3 scale; + scale.x = x; + scale.y = y; + scale.z = z; + return scale; +} + +/** + * @brief Convert data into color + * @param r Red component + * @param g Green component + * @param b Blue component + * @param a Alpha component (transparency) + * @return Color object + */ +inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a) +{ + std_msgs::msg::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +/** + * @brief Convert data into a Maarker + * @param id Marker ID + * @param pose Marker pose + * @param scale Marker scale + * @param color Marker color + * @param frame Reference frame to use + * @return Visualization Marker + */ +inline visualization_msgs::msg::Marker createMarker( + int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale, + const std_msgs::msg::ColorRGBA & color, const std::string & frame_id, const std::string & ns) +{ + using visualization_msgs::msg::Marker; + Marker marker; + marker.header.frame_id = frame_id; + marker.header.stamp = rclcpp::Time(0, 0); + marker.ns = ns; + marker.id = id; + marker.type = Marker::SPHERE; + marker.action = Marker::ADD; + + marker.pose = pose; + marker.scale = scale; + marker.color = color; + return marker; +} + +/** + * @brief Convert data into TwistStamped + * @param vx X velocity + * @param wz Angular velocity + * @param stamp Timestamp + * @param frame Reference frame to use + */ +inline geometry_msgs::msg::TwistStamped toTwistStamped( + float vx, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame) +{ + geometry_msgs::msg::TwistStamped twist; + twist.header.frame_id = frame; + twist.header.stamp = stamp; + twist.twist.linear.x = vx; + twist.twist.angular.z = wz; + + return twist; +} + +/** + * @brief Convert data into TwistStamped + * @param vx X velocity + * @param vy Y velocity + * @param wz Angular velocity + * @param stamp Timestamp + * @param frame Reference frame to use + */ +inline geometry_msgs::msg::TwistStamped toTwistStamped( + float vx, float vy, float wz, const builtin_interfaces::msg::Time & stamp, + const std::string & frame) +{ + auto twist = toTwistStamped(vx, wz, stamp, frame); + twist.twist.linear.y = vy; + + return twist; +} + +inline std::unique_ptr toTrajectoryMsg( + const Eigen::ArrayXXf & trajectory, + const models::ControlSequence & control_sequence, + const double & model_dt, + const std_msgs::msg::Header & header) +{ + auto trajectory_msg = std::make_unique(); + trajectory_msg->header = header; + trajectory_msg->points.resize(trajectory.rows()); + + for (int i = 0; i < trajectory.rows(); ++i) { + auto & curr_pt = trajectory_msg->points[i]; + curr_pt.time_from_start = rclcpp::Duration::from_seconds(i * model_dt); + curr_pt.pose.position.x = trajectory(i, 0); + curr_pt.pose.position.y = trajectory(i, 1); + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, trajectory(i, 2)); + curr_pt.pose.orientation = tf2::toMsg(quat); + curr_pt.velocity.linear.x = control_sequence.vx(i); + curr_pt.velocity.angular.z = control_sequence.wz(i); + if (control_sequence.vy.size() > 0) { + curr_pt.velocity.linear.y = control_sequence.vy(i); + } + } + + return trajectory_msg; +} + +/** + * @brief Convert path to a tensor + * @param path Path to convert + * @return Path tensor + */ +inline models::Path toTensor(const nav_msgs::msg::Path & path) +{ + auto result = models::Path{}; + result.reset(path.poses.size()); + + for (size_t i = 0; i < path.poses.size(); ++i) { + result.x(i) = path.poses[i].pose.position.x; + result.y(i) = path.poses[i].pose.position.y; + result.yaws(i) = tf2::getYaw(path.poses[i].pose.orientation); + } + + return result; +} + +/** + * @brief Get the last pose from a path + * @param path Reference to the path + * @return geometry_msgs::msg::Pose Last pose in the path + */ +inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path) +{ + const unsigned int path_last_idx = path.x.size() - 1; + + auto last_orientation = path.yaws(path_last_idx); + + tf2::Quaternion pose_orientation; + pose_orientation.setRPY(0.0, 0.0, last_orientation); + + geometry_msgs::msg::Pose pathPose; + pathPose.position.x = path.x(path_last_idx); + pathPose.position.y = path.y(path_last_idx); + pathPose.orientation.x = pose_orientation.x(); + pathPose.orientation.y = pose_orientation.y(); + pathPose.orientation.z = pose_orientation.z(); + pathPose.orientation.w = pose_orientation.w(); + + return pathPose; +} + +/** + * @brief normalize + * Normalizes the angle to be -M_PIF circle to +M_PIF circle + * It takes and returns radians. + * @param angles Angles to normalize + * @return normalized angles + */ +template +auto normalize_angles(const T & angles) +{ + return (angles + M_PIF).unaryExpr([&](const float x) { + float remainder = std::fmod(x, 2.0f * M_PIF); + return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF; + }); +} + +/** + * @brief shortest_angular_distance + * + * Given 2 angles, this returns the shortest angular + * difference. The inputs and outputs are of course radians. + * + * The result + * would always be -pi <= result <= pi. Adding the result + * to "from" will always get you an equivalent angle to "to". + * @param from Start angle + * @param to End angle + * @return Shortest distance between angles + */ +template +auto shortest_angular_distance( + const F & from, + const T & to) +{ + return normalize_angles(to - from); +} + +/** + * @brief Evaluate furthest point idx of data.path which is + * nearest to some trajectory in data.trajectories + * @param data Data to use + * @return Idx of furthest path point reached by a set of trajectories + */ +inline size_t findPathFurthestReachedPoint(const CriticData & data) +{ + int traj_cols = data.trajectories.x.cols(); + const auto traj_x = data.trajectories.x.col(traj_cols - 1); + const auto traj_y = data.trajectories.y.col(traj_cols - 1); + + const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1).colwise() - traj_x; + const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1).colwise() - traj_y; + + const auto dists = dx * dx + dy * dy; + + int max_id_by_trajectories = 0, min_id_by_path = 0; + float min_distance_by_path = std::numeric_limits::max(); + size_t n_rows = dists.rows(); + size_t n_cols = dists.cols(); + for (size_t i = 0; i != n_rows; i++) { + min_id_by_path = 0; + min_distance_by_path = std::numeric_limits::max(); + for (size_t j = max_id_by_trajectories; j != n_cols; j++) { + const float cur_dist = dists(i, j); + if (cur_dist < min_distance_by_path) { + min_distance_by_path = cur_dist; + min_id_by_path = j; + } + } + max_id_by_trajectories = std::max(max_id_by_trajectories, min_id_by_path); + } + return max_id_by_trajectories; +} + +/** + * @brief evaluate path furthest point if it is not set + * @param data Data to use + */ +inline void setPathFurthestPointIfNotSet(CriticData & data) +{ + if (!data.furthest_reached_path_point) { + data.furthest_reached_path_point = findPathFurthestReachedPoint(data); + } +} + +/** + * @brief evaluate path costs + * @param data Data to use + */ +inline void findPathCosts( + CriticData & data, + std::shared_ptr costmap_ros) +{ + auto * costmap = costmap_ros->getCostmap(); + unsigned int map_x, map_y; + const size_t path_segments_count = data.path.x.size() - 1; + data.path_pts_valid = std::vector(path_segments_count, false); + const bool tracking_unknown = costmap_ros->getLayeredCostmap()->isTrackingUnknown(); + for (unsigned int idx = 0; idx < path_segments_count; idx++) { + if (!costmap->worldToMap(data.path.x(idx), data.path.y(idx), map_x, map_y)) { + (*data.path_pts_valid)[idx] = false; + continue; + } + + switch (costmap->getCost(map_x, map_y)) { + case (nav2_costmap_2d::LETHAL_OBSTACLE): + (*data.path_pts_valid)[idx] = false; + continue; + case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): + (*data.path_pts_valid)[idx] = false; + continue; + case (nav2_costmap_2d::NO_INFORMATION): + (*data.path_pts_valid)[idx] = tracking_unknown ? true : false; + continue; + } + + (*data.path_pts_valid)[idx] = true; + } +} + +/** + * @brief evaluate path costs if it is not set + * @param data Data to use + */ +inline void setPathCostsIfNotSet( + CriticData & data, + std::shared_ptr costmap_ros) +{ + if (!data.path_pts_valid) { + findPathCosts(data, costmap_ros); + } +} + +/** + * @brief evaluate angle from pose (have angle) to point (no angle) + * @param pose pose + * @param point_x Point to find angle relative to X axis + * @param point_y Point to find angle relative to Y axis + * @param forward_preference If reversing direction is valid + * @return Angle between two points + */ +inline float posePointAngle( + const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) +{ + float pose_x = pose.position.x; + float pose_y = pose.position.y; + float pose_yaw = tf2::getYaw(pose.orientation); + + float yaw = atan2f(point_y - pose_y, point_x - pose_x); + + // If no preference for forward, return smallest angle either in heading or 180 of heading + if (!forward_preference) { + return std::min( + fabs(angles::shortest_angular_distance(yaw, pose_yaw)), + fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PIF)))); + } + + return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); +} + +/** + * @brief evaluate angle from pose (have angle) to point (no angle) + * @param pose pose + * @param point_x Point to find angle relative to X axis + * @param point_y Point to find angle relative to Y axis + * @param point_yaw Yaw of the point to consider along Z axis + * @return Angle between two points + */ +inline float posePointAngle( + const geometry_msgs::msg::Pose & pose, + double point_x, double point_y, double point_yaw) +{ + float pose_x = static_cast(pose.position.x); + float pose_y = static_cast(pose.position.y); + float pose_yaw = static_cast(tf2::getYaw(pose.orientation)); + + float yaw = atan2f(static_cast(point_y) - pose_y, static_cast(point_x) - pose_x); + + if (fabs(angles::shortest_angular_distance(yaw, static_cast(point_yaw))) > M_PIF_2) { + yaw = angles::normalize_angle(yaw + M_PIF); + } + + return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); +} + +/** + * @brief Apply Savisky-Golay filter to optimal trajectory + * @param control_sequence Sequence to apply filter to + * @param control_history Recent set of controls for edge-case handling + * @param Settings Settings to use + */ +inline void savitskyGolayFilter( + models::ControlSequence & control_sequence, + std::array & control_history, + const models::OptimizerSettings & settings) +{ + // Savitzky-Golay Quadratic, 9-point Coefficients + Eigen::Array filter = {-21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f, + -21.0f}; + filter /= 231.0f; + + // Too short to smooth meaningfully + const unsigned int num_sequences = control_sequence.vx.size() - 1; + if (num_sequences < 20) { + return; + } + + auto applyFilter = [&](const Eigen::Array & data) -> float { + return (data * filter).eval().sum(); + }; + + auto applyFilterOverAxis = + [&](Eigen::ArrayXf & sequence, const Eigen::ArrayXf & initial_sequence, + const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void + { + float pt_m4 = hist_0; + float pt_m3 = hist_1; + float pt_m2 = hist_2; + float pt_m1 = hist_3; + float pt = initial_sequence(0); + float pt_p1 = initial_sequence(1); + float pt_p2 = initial_sequence(2); + float pt_p3 = initial_sequence(3); + float pt_p4 = initial_sequence(4); + + for (unsigned int idx = 0; idx != num_sequences; idx++) { + sequence(idx) = applyFilter({pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4}); + pt_m4 = pt_m3; + pt_m3 = pt_m2; + pt_m2 = pt_m1; + pt_m1 = pt; + pt = pt_p1; + pt_p1 = pt_p2; + pt_p2 = pt_p3; + pt_p3 = pt_p4; + + if (idx + 5 < num_sequences) { + pt_p4 = initial_sequence(idx + 5); + } else { + // Return the last point + pt_p4 = initial_sequence(num_sequences); + } + } + }; + + // Filter trajectories + const models::ControlSequence initial_control_sequence = control_sequence; + applyFilterOverAxis( + control_sequence.vx, initial_control_sequence.vx, control_history[0].vx, + control_history[1].vx, control_history[2].vx, control_history[3].vx); + applyFilterOverAxis( + control_sequence.vy, initial_control_sequence.vy, control_history[0].vy, + control_history[1].vy, control_history[2].vy, control_history[3].vy); + applyFilterOverAxis( + control_sequence.wz, initial_control_sequence.wz, control_history[0].wz, + control_history[1].wz, control_history[2].wz, control_history[3].wz); + + // Update control history + unsigned int offset = settings.shift_control_sequence ? 1 : 0; + control_history[0] = control_history[1]; + control_history[1] = control_history[2]; + control_history[2] = control_history[3]; + control_history[3] = { + control_sequence.vx(offset), + control_sequence.vy(offset), + control_sequence.wz(offset)}; +} + +/** + * @brief Find the iterator of the first pose at which there is an inversion or in-place rotation in the path + * @param path to check for inversion or rotation + * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check) + * @return the first point after the inversion or rotation found in the path + */ +inline unsigned int findFirstPathInversion( + nav_msgs::msg::Path & path, + float rotation_threshold = 0.0f) +{ + if (path.poses.size() < 2) { + return path.poses.size(); + } + + unsigned int first_after_constraint = path.poses.size(); + + // Check for in-place rotation first (if enabled) + if (rotation_threshold > 0.0f) { + for (unsigned int idx = 0; idx < path.poses.size() - 1; ++idx) { + float dx = path.poses[idx + 1].pose.position.x - path.poses[idx].pose.position.x; + float dy = path.poses[idx + 1].pose.position.y - path.poses[idx].pose.position.y; + float translation = sqrtf(dx * dx + dy * dy); + + // Check if poses are at roughly the same location + if (translation < 1e-3) { + float yaw1 = tf2::getYaw(path.poses[idx].pose.orientation); + float yaw2 = tf2::getYaw(path.poses[idx + 1].pose.orientation); + float rotation = fabs(angles::shortest_angular_distance(yaw1, yaw2)); + + // Check if this meets the minimum rotation threshold + if (rotation > rotation_threshold) { + // Found start of in-place rotation, now find where it ends + unsigned int end_idx = idx + 1; + + // Continue while we have minimal translation (still rotating in place) + while (end_idx < path.poses.size() - 1) { + float next_dx = path.poses[end_idx + 1].pose.position.x - + path.poses[end_idx].pose.position.x; + float next_dy = path.poses[end_idx + 1].pose.position.y - + path.poses[end_idx].pose.position.y; + float next_translation = sqrtf(next_dx * next_dx + next_dy * next_dy); + + if (next_translation >= 1e-3) { + break; // End of in-place rotation sequence + } + end_idx++; + } + + first_after_constraint = end_idx; + break; + } + } + } + } + + // Check for inversion (at least 3 poses needed) + if (path.poses.size() >= 3) { + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + float oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + float oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + float ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + float ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + float dot_product = (oa_x * ab_x) + (oa_y * ab_y); + + float norm_oa = hypotf(oa_x, oa_y); + float norm_ab = hypotf(ab_x, ab_y); + + if (norm_oa < 1e-4f || norm_ab < 1e-4f) { + continue; + } + + float cos_ang = dot_product / (norm_oa * norm_ab); + cos_ang = std::max(-1.0f, std::min(1.0f, cos_ang)); + float angle = acosf(cos_ang); // [0, pi] + + if (rotation_threshold > 0.0f) { + if (angle > rotation_threshold) { + first_after_constraint = std::min(first_after_constraint, idx + 1); + break; + } + } else { + if (dot_product < 0.0f) { + first_after_constraint = std::min(first_after_constraint, idx + 1); + break; + } + } + } + } + + return first_after_constraint; +} + +/** + * @brief Find and remove poses after the first inversion or in-place rotation in the path + * @param path Path to check for inversion or rotation + * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check) + * @return The location of the inversion/rotation, return 0 if none exist + */ +inline unsigned int removePosesAfterFirstInversion( + nav_msgs::msg::Path & path, + float rotation_threshold = 0.0f) +{ + nav_msgs::msg::Path cropped_path = path; + + const unsigned int first_after_constraint = findFirstPathInversion(cropped_path, rotation_threshold); + + if (first_after_constraint == path.poses.size()) { + return 0u; // No constraint found + } + + cropped_path.poses.erase( + cropped_path.poses.begin() + first_after_constraint, cropped_path.poses.end()); + path = cropped_path; + return first_after_constraint; +} + +/** + * @brief Compare to trajectory points to find closest path point along integrated distances + * @param vec Vect to check + * @return dist Distance to look for + * @return init Starting index to indec from + */ +inline unsigned int findClosestPathPt( + const std::vector & vec, const float dist, const unsigned int init = 0u) +{ + float distim1 = init != 0u ? vec[init] : 0.0f; // First is 0, no accumulated distance yet + float disti = 0.0f; + const unsigned int size = vec.size(); + for (unsigned int i = init + 1; i != size; i++) { + disti = vec[i]; + if (disti > dist) { + if (i > 0 && dist - distim1 < disti - dist) { + return i - 1; + } + return i; + } + distim1 = disti; + } + return size - 1; +} + +// A struct to hold pose data in floating point resolution +struct Pose2D +{ + float x, y, theta; +}; + +/** + * @brief Shift the columns of a 2D Eigen Array or scalar values of + * 1D Eigen Array by 1 place. + * @param e Eigen Array + * @param direction direction in which Array will be shifted. + * 1 for shift in right direction and -1 for left direction. + */ +inline void shiftColumnsByOnePlace(Eigen::Ref e, int direction) +{ + int size = e.size(); + if(size == 1) {return;} + if(abs(direction) != 1) { + throw std::logic_error("Invalid direction, only 1 and -1 are valid values."); + } + + if((e.cols() == 1 || e.rows() == 1) && size > 1) { + auto start_ptr = direction == 1 ? e.data() + size - 2 : e.data() + 1; + auto end_ptr = direction == 1 ? e.data() : e.data() + size - 1; + while(start_ptr != end_ptr) { + *(start_ptr + direction) = *start_ptr; + start_ptr -= direction; + } + *(start_ptr + direction) = *start_ptr; + } else { + auto start_ptr = direction == 1 ? e.data() + size - 2 * e.rows() : e.data() + e.rows(); + auto end_ptr = direction == 1 ? e.data() : e.data() + size - e.rows(); + auto span = e.rows(); + while(start_ptr != end_ptr) { + std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); + start_ptr -= (direction * span); + } + std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); + } +} + +/** + * @brief Normalize the yaws between points on the basis of final yaw angle + * of the trajectory. + * @param last_yaws Final yaw angles of the trajectories. + * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories. + * @return Normalized yaw between points. + */ +inline auto normalize_yaws_between_points( + const Eigen::Ref & last_yaws, + const Eigen::Ref & yaw_between_points) +{ + Eigen::ArrayXf yaws = utils::shortest_angular_distance( + last_yaws, yaw_between_points).abs(); + int size = yaws.size(); + Eigen::ArrayXf yaws_between_points_corrected(size); + for(int i = 0; i != size; i++) { + const float & yaw_between_point = yaw_between_points[i]; + yaws_between_points_corrected[i] = yaws[i] < M_PIF_2 ? + yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF); + } + return yaws_between_points_corrected; +} + +/** + * @brief Normalize the yaws between points on the basis of goal angle. + * @param goal_yaw Goal yaw angle. + * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories. + * @return Normalized yaw between points + */ +inline auto normalize_yaws_between_points( + const float goal_yaw, const Eigen::Ref & yaw_between_points) +{ + int size = yaw_between_points.size(); + Eigen::ArrayXf yaws_between_points_corrected(size); + for(int i = 0; i != size; i++) { + const float & yaw_between_point = yaw_between_points[i]; + yaws_between_points_corrected[i] = fabs( + angles::normalize_angle(yaw_between_point - goal_yaw)) < M_PIF_2 ? + yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF); + } + return yaws_between_points_corrected; +} + +/** + * @brief Clamps the input between the given lower and upper bounds. + * @param lower_bound Lower bound. + * @param upper_bound Upper bound. + * @return Clamped output. + */ +inline float clamp( + const float lower_bound, const float upper_bound, const float input) +{ + return std::min(upper_bound, std::max(input, lower_bound)); +} + +} // namespace mppi::utils + +#endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_mppi_controller/media/critics_stats.png b/nav2_mppi_controller/media/critics_stats.png new file mode 100644 index 00000000000..c373f8e0dcc Binary files /dev/null and b/nav2_mppi_controller/media/critics_stats.png differ diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index adde8cf93dc..67067ca4ced 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -12,11 +12,12 @@ ament_cmake_ros angles + backward_ros geometry_msgs - libomp-dev nav2_common nav2_core nav2_costmap_2d + nav2_msgs nav2_util nav_msgs pluginlib diff --git a/nav2_mppi_controller/src/collision_checker.cpp b/nav2_mppi_controller/src/collision_checker.cpp new file mode 100644 index 00000000000..351bc06a575 --- /dev/null +++ b/nav2_mppi_controller/src/collision_checker.cpp @@ -0,0 +1,394 @@ +// Copyright (c) 2025, Angsa Robotics GmbH +// +// 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. Reserved. + +#include +#include + +#include "nav2_mppi_controller/collision_checker.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" + + +namespace nav2_mppi_controller +{ + +MPPICollisionChecker::MPPICollisionChecker( + std::shared_ptr costmap_ros, + rclcpp_lifecycle::LifecycleNode::SharedPtr node) +: FootprintCollisionChecker(costmap_ros ? costmap_ros->getCostmap() : nullptr) +{ + if (node) { + clock_ = node->get_clock(); + logger_ = node->get_logger(); + } + + if (costmap_ros) { + costmap_ros_ = costmap_ros; + } +} + +void MPPICollisionChecker::setFootprint( + const nav2_costmap_2d::Footprint & footprint, + const bool & radius, + const std::string & inflation_layer_name) +{ + // Calculate the circumscribed cost automatically + possible_collision_cost_ = findCircumscribedCost(inflation_layer_name); + + if (possible_collision_cost_ <= 0.0f) { + RCLCPP_ERROR_THROTTLE( + logger_, *clock_, 1000, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_mppi_controller#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + + footprint_is_radius_ = radius; + + // Use radius, no caching required + if (radius) { + return; + } + + // Store the unoriented footprint for on-the-fly transformation + unoriented_footprint_ = footprint; +} + +CollisionResult MPPICollisionChecker::inCollision( + const std::vector & x, + const std::vector & y, + const std::vector & yaw, + const bool & traverse_unknown) +{ + CollisionResult result; + result.in_collision = false; + + // Check if all vectors have the same size + if (x.size() != y.size() || x.size() != yaw.size()) { + RCLCPP_ERROR(logger_, "Unexpected size of input vectors for collision checking"); + result.in_collision = true; + return result; + } + + // Initialize result vectors + result.center_cost.resize(x.size(), -1.0f); + result.footprint_cost.resize(x.size(), -1.0f); + + // Step 1: Check all poses for bounds and get center costs + std::vector needs_footprint_check(x.size(), false); + + for (size_t i = 0; i < x.size(); ++i) { + // Check to make sure cell is inside the map + if (outsideRange(costmap_->getSizeInCellsX(), x[i]) || + outsideRange(costmap_->getSizeInCellsY(), y[i])) + { + result.in_collision = true; + return result; + } + + // Get center cost for this pose + float current_center_cost = static_cast(costmap_->getCost( + static_cast(x[i] + 0.5f), static_cast(y[i] + 0.5f))); + + result.center_cost[i] = current_center_cost; + + if (current_center_cost == nav2_costmap_2d::NO_INFORMATION && !traverse_unknown) { + result.in_collision = true; + return result; + } + + if (current_center_cost >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { + result.in_collision = true; + return result; + } + + // For footprint-based collision checking, mark poses that need further checking + if (!footprint_is_radius_) { + // Skip if center cost is below collision threshold + if (current_center_cost >= possible_collision_cost_ || possible_collision_cost_ <= 0.0f) { + needs_footprint_check[i] = true; + } + } + } + + if (footprint_is_radius_) { + return result; // No further checks needed for radius-based checking + } + + // Step 2: If using footprint, check footprint costs for poses that need it + std::vector needs_swept_area_check(x.size(), false); + // Cache computed footprints to avoid recomputation in Step 4 + std::vector cached_footprints(x.size()); + + for (size_t i = 0; i < x.size(); ++i) { + // Skip poses that don't need footprint checking + if (!needs_footprint_check[i]) { + continue; + } + + // Transform footprint to current orientation and world position + double wx, wy; + costmap_->mapToWorld(static_cast(x[i]), static_cast(y[i]), wx, wy); + + // Transform the footprint to the given orientation + nav2_costmap_2d::Footprint oriented_footprint = transformFootprint(unoriented_footprint_, + yaw[i]); + + // Translate to world position + nav2_costmap_2d::Footprint current_footprint; + current_footprint.reserve(oriented_footprint.size()); + geometry_msgs::msg::Point new_pt; + for (unsigned int j = 0; j < oriented_footprint.size(); ++j) { + new_pt.x = wx + oriented_footprint[j].x; + new_pt.y = wy + oriented_footprint[j].y; + current_footprint.push_back(new_pt); + } + + // Cache the computed footprint for potential reuse in Step 4 + cached_footprints[i] = current_footprint; + + // Check footprint perimeter + float footprint_cost = static_cast(footprintCost(current_footprint, false)); + + // Store footprint cost in result + result.footprint_cost[i] = footprint_cost; + + if (footprint_cost == nav2_costmap_2d::NO_INFORMATION && !traverse_unknown) { + result.in_collision = true; + return result; + } + + if (footprint_cost >= nav2_costmap_2d::LETHAL_OBSTACLE) { + result.in_collision = true; + return result; + } + + // Mark for swept area checking if footprint cost is INSCRIBED_INFLATED_OBSTACLE + if (footprint_cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { + needs_swept_area_check[i] = true; + } + } + + // Step 3: Check swept area for consecutive poses with footprint cost INSCRIBED_INFLATED_OBSTACLE + // Find consecutive sequences of poses that need swept area checking + std::vector> consecutive_sequences; + std::vector current_sequence; + + for (size_t i = 0; i < x.size(); ++i) { + if (needs_swept_area_check[i]) { + current_sequence.push_back(i); + // Limit sequence to maximum of 5 poses to avoid an unprecise swept area check + if (current_sequence.size() >= 5) { + consecutive_sequences.push_back(current_sequence); + current_sequence.clear(); + } + } else { + if (!current_sequence.empty()) { + // Only add sequences with more than one pose + if (current_sequence.size() > 1) { + consecutive_sequences.push_back(current_sequence); + } + current_sequence.clear(); + } + } + } + + // Don't forget the last sequence if it ends at the last pose + if (!current_sequence.empty()) { + // Only add sequences with more than one pose + if (current_sequence.size() > 1) { + consecutive_sequences.push_back(current_sequence); + } + } + + // Step 4: Check swept area using convex hull for each consecutive sequence + for (const auto & sequence : consecutive_sequences) { + // Collect all footprint points from consecutive poses using cached footprints + std::vector all_points; + + for (size_t seq_idx : sequence) { + // Use cached footprint instead of recomputing + const nav2_costmap_2d::Footprint & current_footprint = cached_footprints[seq_idx]; + + for (const auto & footprint_pt : current_footprint) { + all_points.push_back(footprint_pt); + } + } + + // Create convex hull from all collected points + nav2_costmap_2d::Footprint convex_hull = createConvexHull(all_points); + + // Check swept area cost using full area checking + float swept_area_cost = static_cast(footprintCost(convex_hull, true)); + + if (swept_area_cost == nav2_costmap_2d::NO_INFORMATION && !traverse_unknown) { + result.in_collision = true; + return result; + } + + if (swept_area_cost >= nav2_costmap_2d::LETHAL_OBSTACLE) { + result.in_collision = true; + return result; + } + } + + return result; +} + +nav2_costmap_2d::Footprint MPPICollisionChecker::createConvexHull( + const std::vector & points) +{ + if (points.size() <= 1) { + return points; + } + + // Create a copy of points for sorting + std::vector sorted_points = points; + + // Sort points lexicographically (first by x, then by y) + std::sort(sorted_points.begin(), sorted_points.end(), + [](const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) { + return a.x < b.x || (a.x == b.x && a.y < b.y); + }); + + // Remove duplicate points + sorted_points.erase( + std::unique(sorted_points.begin(), sorted_points.end(), + [](const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) { + return std::abs(a.x - b.x) < 1e-9 && std::abs(a.y - b.y) < 1e-9; + }), + sorted_points.end()); + + if (sorted_points.size() <= 1) { + return sorted_points; + } + + // Andrew's monotone chain algorithm + std::vector hull; + + // Helper function to compute cross product + auto cross = [](const geometry_msgs::msg::Point & O, + const geometry_msgs::msg::Point & A, + const geometry_msgs::msg::Point & B) { + return (A.x - O.x) * (B.y - O.y) - (A.y - O.y) * (B.x - O.x); + }; + + // Build lower hull + for (size_t i = 0; i < sorted_points.size(); ++i) { + while (hull.size() >= 2 && cross(hull[hull.size() - 2], hull[hull.size() - 1], + sorted_points[i]) <= 0) + { + hull.pop_back(); + } + hull.push_back(sorted_points[i]); + } + + // Build upper hull + size_t t = hull.size() + 1; + for (int i = static_cast(sorted_points.size()) - 2; i >= 0; --i) { + while (hull.size() >= t && cross(hull[hull.size() - 2], hull[hull.size() - 1], + sorted_points[i]) <= 0) + { + hull.pop_back(); + } + hull.push_back(sorted_points[i]); + } + + // Remove last point because it's the same as the first one + if (hull.size() > 1) { + hull.pop_back(); + } + + return hull; +} + +bool MPPICollisionChecker::outsideRange(const unsigned int & max, const float & value) const +{ + return value < 0.0f || value > max; +} + +nav2_costmap_2d::Footprint MPPICollisionChecker::transformFootprint( + const nav2_costmap_2d::Footprint & footprint, + float yaw) const +{ + double sin_th = sin(yaw); + double cos_th = cos(yaw); + nav2_costmap_2d::Footprint oriented_footprint; + oriented_footprint.reserve(footprint.size()); + + geometry_msgs::msg::Point new_pt; + for (unsigned int i = 0; i < footprint.size(); ++i) { + new_pt.x = footprint[i].x * cos_th - footprint[i].y * sin_th; + new_pt.y = footprint[i].x * sin_th + footprint[i].y * cos_th; + oriented_footprint.push_back(new_pt); + } + + return oriented_footprint; +} + +float MPPICollisionChecker::findCircumscribedCost(const std::string & inflation_layer_name) +{ + if (!costmap_ros_) { + RCLCPP_ERROR(logger_, "Costmap ROS is not available for circumscribed cost calculation"); + return -1.0f; + } + + double result = -1.0; + const double circum_radius = costmap_ros_->getLayeredCostmap()->getCircumscribedRadius(); + + if (static_cast(circum_radius) == circumscribed_radius_) { + // early return if footprint size is unchanged + return circumscribed_cost_; + } + + // check if the costmap has an inflation layer + const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer( + costmap_ros_, + inflation_layer_name); + + if (inflation_layer != nullptr) { + const double resolution = costmap_ros_->getCostmap()->getResolution(); + double inflation_radius = inflation_layer->getInflationRadius(); + + if (inflation_radius < circum_radius) { + RCLCPP_ERROR( + logger_, + "The inflation radius (%f) is smaller than the circumscribed radius (%f) " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times!", + inflation_radius, circum_radius); + result = 0.0; + } else { + result = inflation_layer->computeCost(circum_radius / resolution); + } + } else { + RCLCPP_WARN( + logger_, + "No inflation layer found in costmap configuration. " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times and not avoid anything but absolute collisions!"); + } + + circumscribed_radius_ = static_cast(circum_radius); + circumscribed_cost_ = static_cast(result); + + return circumscribed_cost_; +} + +} // namespace nav2_mppi_controller \ No newline at end of file diff --git a/nav2_mppi_controller/src/controller copy 2.cpp b/nav2_mppi_controller/src/controller copy 2.cpp new file mode 100644 index 00000000000..a9fab10c85e --- /dev/null +++ b/nav2_mppi_controller/src/controller copy 2.cpp @@ -0,0 +1,157 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 "nav2_mppi_controller/controller.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" + +// #define BENCHMARK_TESTING + +namespace nav2_mppi_controller +{ + +void MPPIController::configure( + const nav2::LifecycleNode::WeakPtr & parent, + std::string name, const std::shared_ptr tf, + const std::shared_ptr costmap_ros) +{ + parent_ = parent; + costmap_ros_ = costmap_ros; + tf_buffer_ = tf; + name_ = name; + parameters_handler_ = std::make_unique(parent, name_); + + auto node = parent_.lock(); + // Get high-level controller parameters + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(visualize_, "visualize", false); + + getParam(publish_optimal_trajectory_, "publish_optimal_trajectory", false); + + // Configure composed objects + optimizer_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get()); + path_handler_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get()); + trajectory_visualizer_.on_configure( + parent_, name_, + costmap_ros_->getGlobalFrameID(), parameters_handler_.get()); + + if (publish_optimal_trajectory_) { + opt_traj_pub_ = node->create_publisher( + "~/optimal_trajectory"); + } + + RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str()); +} + +void MPPIController::cleanup() +{ + optimizer_.shutdown(); + trajectory_visualizer_.on_cleanup(); + parameters_handler_.reset(); + opt_traj_pub_.reset(); + RCLCPP_INFO(logger_, "Cleaned up MPPI Controller: %s", name_.c_str()); +} + +void MPPIController::activate() +{ + auto node = parent_.lock(); + trajectory_visualizer_.on_activate(); + parameters_handler_->start(); + if (opt_traj_pub_) { + opt_traj_pub_->on_activate(); + } + RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str()); +} + +void MPPIController::deactivate() +{ + trajectory_visualizer_.on_deactivate(); + if (opt_traj_pub_) { + opt_traj_pub_->on_deactivate(); + } + RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str()); +} + +void MPPIController::reset() +{ + optimizer_.reset(false /*Don't reset zone-based speed limits between requests*/); +} + +geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::Twist & robot_speed, + nav2_core::GoalChecker * goal_checker) +{ +#ifdef BENCHMARK_TESTING + auto start = std::chrono::system_clock::now(); +#endif + + std::lock_guard param_lock(*parameters_handler_->getLock()); + geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal(robot_pose.header.stamp).pose; + + nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose); + + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + std::unique_lock costmap_lock(*(costmap->getMutex())); + + auto [cmd, optimal_trajectory] = + optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker); + +#ifdef BENCHMARK_TESTING + auto end = std::chrono::system_clock::now(); + auto duration = std::chrono::duration_cast(end - start).count(); + RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration); +#endif + + if (publish_optimal_trajectory_ && opt_traj_pub_->get_subscription_count() > 0) { + auto trajectory_msg = utils::toTrajectoryMsg( + optimal_trajectory, + optimizer_.getOptimalControlSequence(), + optimizer_.getSettings().model_dt, + cmd.header); + opt_traj_pub_->publish(std::move(trajectory_msg)); + } + + if (visualize_) { + visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory); + } + + return cmd; +} + +void MPPIController::visualize( + nav_msgs::msg::Path transformed_plan, + const builtin_interfaces::msg::Time & cmd_stamp, + const Eigen::ArrayXXf & optimal_trajectory) +{ + trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories"); + trajectory_visualizer_.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); + trajectory_visualizer_.visualize(std::move(transformed_plan)); +} + +void MPPIController::setPlan(const nav_msgs::msg::Path & path) +{ + path_handler_.setPath(path); +} + +void MPPIController::setSpeedLimit(const double & speed_limit, const bool & percentage) +{ + optimizer_.setSpeedLimit(speed_limit, percentage); +} + +} // namespace nav2_mppi_controller + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_mppi_controller::MPPIController, nav2_core::Controller) diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index a9fab10c85e..1988b65d6c1 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -16,8 +16,9 @@ #include #include "nav2_mppi_controller/controller.hpp" #include "nav2_mppi_controller/tools/utils.hpp" - -// #define BENCHMARK_TESTING +#include "nav2_costmap_2d/footprint.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "std_msgs/msg/header.hpp" namespace nav2_mppi_controller { @@ -36,9 +37,6 @@ void MPPIController::configure( auto node = parent_.lock(); // Get high-level controller parameters auto getParam = parameters_handler_->getParamGetter(name_); - getParam(visualize_, "visualize", false); - - getParam(publish_optimal_trajectory_, "publish_optimal_trajectory", false); // Configure composed objects optimizer_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get()); @@ -47,11 +45,6 @@ void MPPIController::configure( parent_, name_, costmap_ros_->getGlobalFrameID(), parameters_handler_.get()); - if (publish_optimal_trajectory_) { - opt_traj_pub_ = node->create_publisher( - "~/optimal_trajectory"); - } - RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str()); } @@ -60,7 +53,6 @@ void MPPIController::cleanup() optimizer_.shutdown(); trajectory_visualizer_.on_cleanup(); parameters_handler_.reset(); - opt_traj_pub_.reset(); RCLCPP_INFO(logger_, "Cleaned up MPPI Controller: %s", name_.c_str()); } @@ -69,18 +61,12 @@ void MPPIController::activate() auto node = parent_.lock(); trajectory_visualizer_.on_activate(); parameters_handler_->start(); - if (opt_traj_pub_) { - opt_traj_pub_->on_activate(); - } RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str()); } void MPPIController::deactivate() { trajectory_visualizer_.on_deactivate(); - if (opt_traj_pub_) { - opt_traj_pub_->on_deactivate(); - } RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str()); } @@ -94,9 +80,9 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( const geometry_msgs::msg::Twist & robot_speed, nav2_core::GoalChecker * goal_checker) { -#ifdef BENCHMARK_TESTING + #ifdef BENCHMARK_TESTING auto start = std::chrono::system_clock::now(); -#endif + #endif std::lock_guard param_lock(*parameters_handler_->getLock()); geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal(robot_pose.header.stamp).pose; @@ -109,38 +95,28 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( auto [cmd, optimal_trajectory] = optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker); -#ifdef BENCHMARK_TESTING + + #ifdef BENCHMARK_TESTING auto end = std::chrono::system_clock::now(); auto duration = std::chrono::duration_cast(end - start).count(); RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration); -#endif - - if (publish_optimal_trajectory_ && opt_traj_pub_->get_subscription_count() > 0) { - auto trajectory_msg = utils::toTrajectoryMsg( - optimal_trajectory, - optimizer_.getOptimalControlSequence(), - optimizer_.getSettings().model_dt, - cmd.header); - opt_traj_pub_->publish(std::move(trajectory_msg)); - } - - if (visualize_) { - visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory); - } + #endif + + trajectory_visualizer_.visualize( + std::move(transformed_plan), + optimal_trajectory, + optimizer_.getOptimalControlSequence(), + optimizer_.getSettings().model_dt, + cmd.header.stamp, + costmap_ros_, + optimizer_.getGeneratedTrajectories(), + optimizer_.getCosts(), + optimizer_.getCriticCosts(), + optimizer_.getTrajectoriesInCollision()); return cmd; } -void MPPIController::visualize( - nav_msgs::msg::Path transformed_plan, - const builtin_interfaces::msg::Time & cmd_stamp, - const Eigen::ArrayXXf & optimal_trajectory) -{ - trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories"); - trajectory_visualizer_.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); - trajectory_visualizer_.visualize(std::move(transformed_plan)); -} - void MPPIController::setPlan(const nav_msgs::msg::Path & path) { path_handler_.setPath(path); @@ -154,4 +130,4 @@ void MPPIController::setSpeedLimit(const double & speed_limit, const bool & perc } // namespace nav2_mppi_controller #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(nav2_mppi_controller::MPPIController, nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(nav2_mppi_controller::MPPIController, nav2_core::Controller) \ No newline at end of file diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp index d6acd320b05..aaa5790548c 100644 --- a/nav2_mppi_controller/src/critic_manager.cpp +++ b/nav2_mppi_controller/src/critic_manager.cpp @@ -37,6 +37,11 @@ void CriticManager::getParams() auto node = parent_.lock(); auto getParam = parameters_handler_->getParamGetter(name_); getParam(critic_names_, "critics", std::vector{}, ParameterType::Static); + // Read visualization parameters from Visualization namespace + auto getVisualizerParam = parameters_handler_->getParamGetter(name_ + ".Visualization"); + getVisualizerParam(publish_critics_stats_, "publish_critics_stats", false, ParameterType::Static); + getVisualizerParam(visualize_per_critic_costs_, "publish_trajectories_with_individual_cost", + false, ParameterType::Static); } void CriticManager::loadCritics() @@ -46,6 +51,13 @@ void CriticManager::loadCritics() "nav2_mppi_controller", "mppi::critics::CriticFunction"); } + auto node = parent_.lock(); + if (publish_critics_stats_) { + critics_effect_pub_ = node->create_publisher( + "~/critics_stats"); + critics_effect_pub_->on_activate(); + } + critics_.clear(); for (auto name : critic_names_) { std::string fullname = getFullName(name); @@ -67,11 +79,62 @@ std::string CriticManager::getFullName(const std::string & name) void CriticManager::evalTrajectoriesScores( CriticData & data) const { - for (const auto & critic : critics_) { + std::unique_ptr stats_msg; + if (publish_critics_stats_) { + stats_msg = std::make_unique(); + stats_msg->critics.reserve(critics_.size()); + stats_msg->changed.reserve(critics_.size()); + stats_msg->costs_sum.reserve(critics_.size()); + } + + // Initialize per-critic costs tracking only if requested + if (visualize_per_critic_costs_) { + if (!data.individual_critics_cost) { + data.individual_critics_cost = std::vector>(); + } + data.individual_critics_cost->clear(); + data.individual_critics_cost->reserve(critics_.size()); + } + + for (size_t i = 0; i < critics_.size(); ++i) { if (data.fail_flag) { break; } - critic->score(data); + + // Store costs before critic evaluation + Eigen::ArrayXf costs_before; + if (visualize_per_critic_costs_ || publish_critics_stats_) { + costs_before = data.costs; + } + + critics_[i]->score(data); + + // Calculate cost contribution from this critic + if (visualize_per_critic_costs_ || publish_critics_stats_) { + + // Calculate sum of costs added by this individual critic + Eigen::ArrayXf cost_diff = data.costs - costs_before; + if (visualize_per_critic_costs_) { + data.individual_critics_cost->emplace_back(critic_names_[i], cost_diff); + } + + // Calculate statistics if publishing is enabled + if (publish_critics_stats_) { + stats_msg->critics.push_back(critic_names_[i]); + + // Calculate sum of costs added by this individual critic + float costs_sum = cost_diff.sum(); + stats_msg->costs_sum.push_back(costs_sum); + stats_msg->changed.push_back(costs_sum != 0.0f); + } + } + } + + // Publish statistics if enabled + if (critics_effect_pub_) { + auto node = parent_.lock(); + stats_msg->stamp = node->get_clock()->now(); + critics_effect_pub_->publish(std::move(stats_msg)); } } diff --git a/nav2_mppi_controller/src/critics/cost_critic copy 2.cpp b/nav2_mppi_controller/src/critics/cost_critic copy 2.cpp new file mode 100644 index 00000000000..2ab49793515 --- /dev/null +++ b/nav2_mppi_controller/src/critics/cost_critic copy 2.cpp @@ -0,0 +1,284 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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. + +#include +#include "nav2_mppi_controller/critics/cost_critic.hpp" +#include "nav2_core/controller_exceptions.hpp" +#include +#include + +namespace mppi::critics +{ + +void CostCritic::initialize() +{ + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(consider_footprint_, "consider_footprint", false); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 3.81f); + getParam(critical_cost_, "critical_cost", 300.0f); + getParam(near_collision_cost_, "near_collision_cost", 253); + getParam(collision_cost_, "collision_cost", 1000000.0f); + getParam(near_goal_distance_, "near_goal_distance", 0.5f); + getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); + getParam(trajectory_point_step_, "trajectory_point_step", 2); + getParam(precollision_lookahead_time_, "precollision_lookahead_time", 1.0f); + getParam(precollision_penalty_, "precollision_penalty", 100.0f); + + // Normalized by cost value to put in same regime as other weights + weight_ /= 254.0f; + + // Normalize weight when parameter is changed dynamically as well + auto weightDynamicCb = [&]( + const rclcpp::Parameter & weight, rcl_interfaces::msg::SetParametersResult & /*result*/) { + weight_ = weight.as_double() / 254.0f; + }; + parameters_handler_->addParamCallback(name_ + ".cost_weight", weightDynamicCb); + + collision_checker_.setCostmap(costmap_); + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + + if (possible_collision_cost_ < 1.0f) { + RCLCPP_ERROR( + logger_, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + + if (costmap_ros_->getUseRadius() == consider_footprint_) { + RCLCPP_WARN( + logger_, + "Inconsistent configuration in collision checking. Please verify the robot's shape settings " + "in both the costmap and the cost critic."); + if (costmap_ros_->getUseRadius()) { + throw nav2_core::ControllerException( + "Considering footprint in collision checking but no robot footprint provided in the " + "costmap."); + } + } + + if(near_collision_cost_ > 253) { + RCLCPP_WARN(logger_, "Near collision cost is set higher than INSCRIBED_INFLATED_OBSTACLE"); + } + + RCLCPP_INFO( + logger_, + "InflationCostCritic instantiated with %d power and %f / %f weights. " + "Critic will collision check based on %s cost.", + power_, critical_cost_, weight_, consider_footprint_ ? + "footprint" : "circular"); +} + +float CostCritic::findCircumscribedCost( + std::shared_ptr costmap) +{ + double result = -1.0; + const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + if (static_cast(circum_radius) == circumscribed_radius_) { + // early return if footprint size is unchanged + return circumscribed_cost_; + } + + // check if the costmap has an inflation layer + const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer( + costmap, + inflation_layer_name_); + if (inflation_layer != nullptr) { + const double resolution = costmap->getCostmap()->getResolution(); + double inflation_radius = inflation_layer->getInflationRadius(); + if (inflation_radius < circum_radius) { + RCLCPP_ERROR( + rclcpp::get_logger("computeCircumscribedCost"), + "The inflation radius (%f) is smaller than the circumscribed radius (%f) " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times!", + inflation_radius, circum_radius); + result = 0.0; + return result; + } + result = inflation_layer->computeCost(circum_radius / resolution); + } else { + RCLCPP_WARN( + logger_, + "No inflation layer found in costmap configuration. " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times and not avoid anything but absolute collisions!"); + } + + circumscribed_radius_ = static_cast(circum_radius); + circumscribed_cost_ = static_cast(result); + + return circumscribed_cost_; +} + +void CostCritic::score(CriticData & data) +{ + if (!enabled_) { + return; + } + + // Setup cost information for various parts of the critic + is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + auto * costmap = collision_checker_.getCostmap(); + origin_x_ = static_cast(costmap->getOriginX()); + origin_y_ = static_cast(costmap->getOriginY()); + resolution_ = static_cast(costmap->getResolution()); + size_x_ = costmap->getSizeInCellsX(); + size_y_ = costmap->getSizeInCellsY(); + + if (consider_footprint_) { + // footprint may have changed since initialization if user has dynamic footprints + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + } + + // If near the goal, don't apply the preferential term since the goal is near obstacles + bool near_goal = false; + if (data.state.local_path_length < near_goal_distance_) { + near_goal = true; + } + + Eigen::ArrayXf repulsive_cost(data.costs.rows()); + repulsive_cost.setZero(); + bool all_trajectories_collide = true; + + int strided_traj_cols = floor((data.trajectories.x.cols() - 1) / trajectory_point_step_) + 1; + int strided_traj_rows = data.trajectories.x.rows(); + int outer_stride = strided_traj_rows * trajectory_point_step_; + + // Initialize trajectories collision tracking + if (!data.trajectories_in_collision) { + data.trajectories_in_collision = std::vector(strided_traj_rows, false); + } + + const auto traj_x = Eigen::Map>(data.trajectories.x.data(), strided_traj_rows, strided_traj_cols, + Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto traj_y = Eigen::Map>(data.trajectories.y.data(), strided_traj_rows, strided_traj_cols, + Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto traj_yaw = Eigen::Map>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols, + Eigen::Stride<-1, -1>(outer_stride, 1)); + + for (int i = 0; i < strided_traj_rows; ++i) { + bool trajectory_collide = false; + float pose_cost = 0.0f; + float & traj_cost = repulsive_cost(i); + + for (int j = 0; j < strided_traj_cols; j++) { + float Tx = traj_x(i, j); + float Ty = traj_y(i, j); + unsigned int x_i = 0u, y_i = 0u; + + // The getCost doesn't use orientation + // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it + // So the center point has more information than the footprint + if (!worldToMapFloat(Tx, Ty, x_i, y_i)) { + pose_cost = 255.0f; // NO_INFORMATION in float + } else { + pose_cost = static_cast(costmap->getCost(getIndex(x_i, y_i))); + if (pose_cost < 1.0f) { + continue; // In free space + } + } + + if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) { + traj_cost = collision_cost_; + trajectory_collide = true; + break; + } + + // Let near-collision trajectory points be punished severely + // Note that we collision check based on the footprint actual, + // but score based on the center-point cost regardless + if (pose_cost >= static_cast(near_collision_cost_)) { + traj_cost += critical_cost_; + } else if (!near_goal) { // Generally prefer trajectories further from obstacles + traj_cost += pose_cost; + } + } + + if (!trajectory_collide && + precollision_lookahead_time_ > 0.0f && + precollision_penalty_ > 0.0f) + { + // how many columns ahead correspond to precollision_lookahead_time_ + const unsigned int max_preview_cols = std::min( + (unsigned int)std::ceil( + precollision_lookahead_time_ / + (data.model_dt * static_cast(trajectory_point_step_))), + (unsigned int)strided_traj_cols); + + bool collide_within_window = false; + + for (unsigned int pj = 0; pj < max_preview_cols; ++pj) { + const float Px = traj_x(i, pj); + const float Py = traj_y(i, pj); + const float Pyaw = traj_yaw(i, pj); + + const double fp_cost = collision_checker_.footprintCostAtPose( + (double)Px, + (double)Py, + (double)Pyaw, + costmap_ros_->getRobotFootprint()); + + // Jeśli footprint w tej próbce jest już w strefie naprawdę niebezpiecznej, + // to ta trajektoria "uderzy" w przeszkodę w czasie krótszym niż + // precollision_lookahead_time_. + // Uznajemy ją za złą. + if (fp_cost == (double)nav2_costmap_2d::LETHAL_OBSTACLE || + fp_cost == (double)nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + { + collide_within_window = true; + break; + } + } + + if (collide_within_window) { + // kara stała za to, że kolizja jest < precollision_lookahead_time_ + repulsive_cost(i) += precollision_penalty_; + } + } + + + (*data.trajectories_in_collision)[i] = trajectory_collide; + all_trajectories_collide &= trajectory_collide; + } + + if (power_ > 1u) { + data.costs += (repulsive_cost * + (weight_ / static_cast(strided_traj_cols))).pow(power_); + } else { + data.costs += repulsive_cost * (weight_ / static_cast(strided_traj_cols)); + } + + data.fail_flag = all_trajectories_collide; +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::CostCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/cost_critic copy 3.cpp b/nav2_mppi_controller/src/critics/cost_critic copy 3.cpp new file mode 100644 index 00000000000..74ce0df31e0 --- /dev/null +++ b/nav2_mppi_controller/src/critics/cost_critic copy 3.cpp @@ -0,0 +1,297 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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. + +#include +#include "nav2_mppi_controller/critics/cost_critic.hpp" +#include "nav2_core/controller_exceptions.hpp" + +namespace mppi::critics +{ + +void CostCritic::initialize() +{ + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(consider_footprint_, "consider_footprint", false); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 3.81f); + getParam(critical_cost_, "critical_cost", 300.0f); + getParam(near_collision_cost_, "near_collision_cost", 253); + getParam(collision_cost_, "collision_cost", 1000000.0f); + getParam(near_goal_distance_, "near_goal_distance", 0.5f); + getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); + getParam(trajectory_point_step_, "trajectory_point_step", 2); + + // Normalized by cost value to put in same regime as other weights + weight_ /= 254.0f; + + // Normalize weight when parameter is changed dynamically as well + auto weightDynamicCb = [&]( + const rclcpp::Parameter & weight, rcl_interfaces::msg::SetParametersResult & /*result*/) { + weight_ = weight.as_double() / 254.0f; + }; + parameters_handler_->addParamCallback(name_ + ".cost_weight", weightDynamicCb); + + collision_checker_.setCostmap(costmap_); + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + + if (possible_collision_cost_ < 1.0f) { + RCLCPP_ERROR( + logger_, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + + if (costmap_ros_->getUseRadius() == consider_footprint_) { + RCLCPP_WARN( + logger_, + "Inconsistent configuration in collision checking. Please verify the robot's shape settings " + "in both the costmap and the cost critic."); + if (costmap_ros_->getUseRadius()) { + throw nav2_core::ControllerException( + "Considering footprint in collision checking but no robot footprint provided in the " + "costmap."); + } + } + + if(near_collision_cost_ > 253) { + RCLCPP_WARN(logger_, "Near collision cost is set higher than INSCRIBED_INFLATED_OBSTACLE"); + } + + RCLCPP_INFO( + logger_, + "InflationCostCritic instantiated with %d power and %f / %f weights. " + "Critic will collision check based on %s cost.", + power_, critical_cost_, weight_, consider_footprint_ ? + "footprint" : "circular"); +} + +float CostCritic::findCircumscribedCost( + std::shared_ptr costmap) +{ + double result = -1.0; + const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius() + 0.2; + if (static_cast(circum_radius) == circumscribed_radius_) { + // early return if footprint size is unchanged + return circumscribed_cost_; + } + + // check if the costmap has an inflation layer + const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer( + costmap, + inflation_layer_name_); + if (inflation_layer != nullptr) { + const double resolution = costmap->getCostmap()->getResolution(); + double inflation_radius = inflation_layer->getInflationRadius(); + if (inflation_radius < circum_radius) { + RCLCPP_ERROR( + rclcpp::get_logger("computeCircumscribedCost"), + "The inflation radius (%f) is smaller than the circumscribed radius (%f) " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times!", + inflation_radius, circum_radius); + result = 0.0; + return result; + } + result = inflation_layer->computeCost(circum_radius / resolution); + } else { + RCLCPP_WARN( + logger_, + "No inflation layer found in costmap configuration. " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times and not avoid anything but absolute collisions!"); + } + + circumscribed_radius_ = static_cast(circum_radius); + circumscribed_cost_ = static_cast(result); + + return circumscribed_cost_; +} + +void CostCritic::score(CriticData & data) +{ + if (!enabled_) { + return; + } + + // static bool s_footprint_active = false; + // bool any_use_footprint_this_tick = false; + + // Setup cost information for various parts of the critic + is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + auto * costmap = collision_checker_.getCostmap(); + origin_x_ = static_cast(costmap->getOriginX()); + origin_y_ = static_cast(costmap->getOriginY()); + resolution_ = static_cast(costmap->getResolution()); + size_x_ = costmap->getSizeInCellsX(); + size_y_ = costmap->getSizeInCellsY(); + + if (consider_footprint_) { + // footprint may have changed since initialization if user has dynamic footprints + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + } + + // If near the goal, don't apply the preferential term since the goal is near obstacles + bool near_goal = false; + if (data.state.local_path_length < near_goal_distance_) { + near_goal = true; + } + + Eigen::ArrayXf repulsive_cost(data.costs.rows()); + repulsive_cost.setZero(); + bool all_trajectories_collide = true; + + int strided_traj_cols = floor((data.trajectories.x.cols() - 1) / trajectory_point_step_) + 1; + int strided_traj_rows = data.trajectories.x.rows(); + int outer_stride = strided_traj_rows * trajectory_point_step_; + + // Initialize trajectories collision tracking + if (!data.trajectories_in_collision) { + data.trajectories_in_collision = std::vector(strided_traj_rows, false); + } + + const auto traj_x = Eigen::Map>(data.trajectories.x.data(), strided_traj_rows, strided_traj_cols, + Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto traj_y = Eigen::Map>(data.trajectories.y.data(), strided_traj_rows, strided_traj_cols, + Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto traj_yaw = Eigen::Map>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols, + Eigen::Stride<-1, -1>(outer_stride, 1)); + + for (int i = 0; i < strided_traj_rows; ++i) { + bool trajectory_collide = false; + float pose_cost = 0.0f; + float & traj_cost = repulsive_cost(i); + + for (int j = 0; j < strided_traj_cols; j++) { + float Tx = traj_x(i, j); + float Ty = traj_y(i, j); + unsigned int x_i = 0u, y_i = 0u; + + // The getCost doesn't use orientation + // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it + // So the center point has more information than the footprint + if (!worldToMapFloat(Tx, Ty, x_i, y_i)) { + pose_cost = 0.0f; // NO_INFORMATION in float + } else { + pose_cost = static_cast(costmap->getCost(getIndex(x_i, y_i))); + if (pose_cost < 1.0f) { + continue; // In free space + } + } + int footprint_every_n_points_ = 2; + float cost_for_scoring = pose_cost; + // bool use_footprint_cost = consider_footprint_ && + // (pose_cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f); + bool use_footprint_cost = false; + if (consider_footprint_) { + const bool near_obstacle = (pose_cost >= possible_collision_cost_) || (possible_collision_cost_ < 1.0f); + bool sample_this_point = false; + if (footprint_every_n_points_ <= 0) { + sample_this_point = (j == strided_traj_cols - 1); // tylko końcowy punkt + } else { + sample_this_point = (j % footprint_every_n_points_ == 0) || (j == strided_traj_cols - 1); + } + use_footprint_cost = near_obstacle && sample_this_point; + } + + //any_use_footprint_this_tick = any_use_footprint_this_tick || use_footprint_cost; + + if (use_footprint_cost) { + cost_for_scoring = static_cast(collision_checker_.footprintCostAtPose( + static_cast(Tx), static_cast(Ty), static_cast(traj_yaw(i, j)), + costmap_ros_->getRobotFootprint())); + } + + // Check for collision based on the appropriate cost + bool in_collision = false; + + switch (static_cast(cost_for_scoring)) { + case (nav2_costmap_2d::LETHAL_OBSTACLE): + in_collision = true; + break; + case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): + in_collision = use_footprint_cost ? false : true; + break; + case (nav2_costmap_2d::NO_INFORMATION): + in_collision = is_tracking_unknown_ ? false : true; + break; + default: + in_collision = false; + } + + + if (in_collision) { + traj_cost = collision_cost_; + trajectory_collide = true; + break; + } + + // Let near-collision trajectory points be punished severely + // Note that we collision check based on the footprint actual, + // but score based on the center-point cost regardless + if (cost_for_scoring >= near_collision_cost_ && !near_goal) { + traj_cost += critical_cost_; + } else if (!near_goal) { // Generally prefer trajectories further from obstacles + traj_cost += cost_for_scoring; + } + } + + (*data.trajectories_in_collision)[i] = trajectory_collide; + all_trajectories_collide &= trajectory_collide; + } + + if (power_ > 1u) { + data.costs += (repulsive_cost * + (weight_ / static_cast(strided_traj_cols))).pow(power_); + } else { + data.costs += repulsive_cost * (weight_ / static_cast(strided_traj_cols)); + } + + // if (any_use_footprint_this_tick != s_footprint_active) { + // if (any_use_footprint_this_tick) { + // RCLCPP_INFO(logger_, "[CostCritic] WLACZONO FOOTPRINT COST"); + // RCLCPP_INFO(logger_, + // "[CostCritic] possible_collision_cost_ = %.1f", + // possible_collision_cost_); + // } else { + // RCLCPP_INFO(logger_, "[CostCritic] WYLACZONO FOOTPRINT COST"); + // RCLCPP_INFO(logger_, + // "[CostCritic] possible_collision_cost_ = %.1f", + // possible_collision_cost_); + // } + // s_footprint_active = any_use_footprint_this_tick; + // } + + + data.fail_flag = all_trajectories_collide; +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::CostCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/cost_critic copy.cpp b/nav2_mppi_controller/src/critics/cost_critic copy.cpp new file mode 100644 index 00000000000..5c5d803ca3a --- /dev/null +++ b/nav2_mppi_controller/src/critics/cost_critic copy.cpp @@ -0,0 +1,198 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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. + +#include +#include "nav2_mppi_controller/critics/cost_critic.hpp" +#include "nav2_core/controller_exceptions.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "geometry_msgs/msg/point.hpp" + +namespace mppi::critics +{ + +void CostCritic::initialize() +{ + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(consider_footprint_, "consider_footprint", false); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 3.81f); + getParam(critical_cost_, "critical_cost", 300.0f); + getParam(near_collision_cost_, "near_collision_cost", 253); + getParam(collision_cost_, "collision_cost", 1000000.0f); + getParam(near_goal_distance_, "near_goal_distance", 0.5f); + getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); + getParam(trajectory_point_step_, "trajectory_point_step", 2); + + // Normalized by cost value to put in same regime as other weights + weight_ /= 254.0f; + + // Normalize weight when parameter is changed dynamically as well + auto weightDynamicCb = [&]( + const rclcpp::Parameter & weight, rcl_interfaces::msg::SetParametersResult & /*result*/) { + weight_ = weight.as_double() / 254.0f; + }; + parameters_handler_->addParamCallback(name_ + ".cost_weight", weightDynamicCb); + + collision_checker_ = std::make_unique( + costmap_ros_, parent_.lock()); + + if (costmap_ros_->getUseRadius() == consider_footprint_) { + RCLCPP_WARN( + logger_, + "Inconsistent configuration in collision checking. Please verify the robot's shape settings " + "in both the costmap and the cost critic."); + if (costmap_ros_->getUseRadius()) { + throw nav2_core::ControllerException( + "Considering footprint in collision checking but no robot footprint provided in the " + "costmap."); + } + } + + if(near_collision_cost_ > 253) { + RCLCPP_WARN(logger_, "Near collision cost is set higher than INSCRIBED_INFLATED_OBSTACLE"); + } + + RCLCPP_INFO( + logger_, + "InflationCostCritic instantiated with %d power and %f / %f weights. " + "Critic will collision check based on %s cost.", + power_, critical_cost_, weight_, consider_footprint_ ? + "footprint" : "circular"); +} + +void CostCritic::score(CriticData & data) +{ + if (!enabled_) { + return; + } + + // Setup cost information for various parts of the critic + is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + auto * costmap = collision_checker_->getCostmap(); + origin_x_ = static_cast(costmap->getOriginX()); + origin_y_ = static_cast(costmap->getOriginY()); + resolution_ = static_cast(costmap->getResolution()); + size_x_ = costmap->getSizeInCellsX(); + size_y_ = costmap->getSizeInCellsY(); + + bool near_goal = false; + if (data.state.local_path_length < near_goal_distance_) { + near_goal = true; + } + + Eigen::ArrayXf repulsive_cost(data.costs.rows()); + repulsive_cost.setZero(); + bool all_trajectories_collide = true; + + int strided_traj_cols = floor((data.trajectories.x.cols() - 1) / trajectory_point_step_) + 1; + int strided_traj_rows = data.trajectories.x.rows(); + int outer_stride = strided_traj_rows * trajectory_point_step_; + + // Initialize trajectories collision tracking + if (!data.trajectories_in_collision) { + data.trajectories_in_collision = std::vector(strided_traj_rows, false); + } + + const auto traj_x = Eigen::Map>(data.trajectories.x.data(), strided_traj_rows, strided_traj_cols, + Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto traj_y = Eigen::Map>(data.trajectories.y.data(), strided_traj_rows, strided_traj_cols, + Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto traj_yaw = Eigen::Map>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols, + Eigen::Stride<-1, -1>(outer_stride, 1)); + + collision_checker_->setFootprint( + costmap_ros_->getRobotFootprint(), + costmap_ros_->getUseRadius(), + inflation_layer_name_); + + for (int i = 0; i < strided_traj_rows; ++i) { + bool trajectory_collide = false; + float & traj_cost = repulsive_cost(i); + // Prepare vectors for batch collision checking + std::vector x_coords, y_coords, yaw_angles; + x_coords.reserve(strided_traj_cols); + y_coords.reserve(strided_traj_cols); + yaw_angles.reserve(strided_traj_cols); + + // Convert world coordinates to map coordinates and collect yaw angles + for (int j = 0; j < strided_traj_cols; j++) { + float Tx = traj_x(i, j); + float Ty = traj_y(i, j); + unsigned int x_i = 0u, y_i = 0u; + + if (!worldToMapFloat(Tx, Ty, x_i, y_i)) { + // If any point is outside the map, mark trajectory as colliding + traj_cost = collision_cost_; + trajectory_collide = true; + break; + } + + x_coords.push_back(static_cast(x_i)); + y_coords.push_back(static_cast(y_i)); + yaw_angles.push_back(traj_yaw(i, j)); + } + // Skip batch collision checking if trajectory already marked as colliding + if (trajectory_collide) { + continue; + } + + // Perform batch collision checking + auto collision_result = collision_checker_->inCollision( + x_coords, y_coords, yaw_angles, is_tracking_unknown_); + + if (collision_result.in_collision) { + traj_cost = collision_cost_; + trajectory_collide = true; + } else { + for (size_t k = 0; k < collision_result.footprint_cost.size(); ++k) { + // For optimization purposes, we don't always have the footprint cost + // but we still use it when available to staw away from collision more conservatively + float cost = collision_result.footprint_cost[k] != -1.0f ? + collision_result.footprint_cost[k] : collision_result.center_cost[k]; + + if (cost >= static_cast(near_collision_cost_)) { + traj_cost += critical_cost_; + } else if (!near_goal) { // Generally prefer trajectories further from obstacles + traj_cost += cost; + } + } + } + + + (*data.trajectories_in_collision)[i] = trajectory_collide; + all_trajectories_collide &= trajectory_collide; + } + + if (power_ > 1u) { + data.costs += (repulsive_cost * + (weight_ / static_cast(strided_traj_cols))).pow(power_); + } else { + data.costs += repulsive_cost * (weight_ / static_cast(strided_traj_cols)); + } + + data.fail_flag = all_trajectories_collide; +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::CostCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index dedf910187e..6d2e869614e 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -23,8 +23,6 @@ namespace mppi::critics void CostCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); getParam(consider_footprint_, "consider_footprint", false); getParam(power_, "cost_power", 1); @@ -87,7 +85,7 @@ float CostCritic::findCircumscribedCost( std::shared_ptr costmap) { double result = -1.0; - const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius() /*+ 0.2*/; if (static_cast(circum_radius) == circumscribed_radius_) { // early return if footprint size is unchanged return circumscribed_cost_; @@ -135,7 +133,8 @@ void CostCritic::score(CriticData & data) return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + static bool s_footprint_active = false; + bool any_use_footprint_this_tick = false; // Setup cost information for various parts of the critic is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); @@ -153,7 +152,7 @@ void CostCritic::score(CriticData & data) // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; - if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, goal)) { + if (data.state.local_path_length < near_goal_distance_) { near_goal = true; } @@ -165,6 +164,11 @@ void CostCritic::score(CriticData & data) int strided_traj_rows = data.trajectories.x.rows(); int outer_stride = strided_traj_rows * trajectory_point_step_; + // Initialize trajectories collision tracking + if (!data.trajectories_in_collision) { + data.trajectories_in_collision = std::vector(strided_traj_rows, false); + } + const auto traj_x = Eigen::Map>(data.trajectories.x.data(), strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); @@ -189,7 +193,7 @@ void CostCritic::score(CriticData & data) // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it // So the center point has more information than the footprint if (!worldToMapFloat(Tx, Ty, x_i, y_i)) { - pose_cost = 255.0f; // NO_INFORMATION in float + pose_cost = 0.0f; // NO_INFORMATION in float } else { pose_cost = static_cast(costmap->getCost(getIndex(x_i, y_i))); if (pose_cost < 1.0f) { @@ -197,7 +201,36 @@ void CostCritic::score(CriticData & data) } } - if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) { + float cost_for_scoring = pose_cost; + bool use_footprint_cost = consider_footprint_ && + (pose_cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f); + any_use_footprint_this_tick = any_use_footprint_this_tick || use_footprint_cost; + + if (use_footprint_cost) { + cost_for_scoring = static_cast(collision_checker_.footprintCostAtPose( + static_cast(Tx), static_cast(Ty), static_cast(traj_yaw(i, j)), + costmap_ros_->getRobotFootprint())); + } + + // Check for collision based on the appropriate cost + bool in_collision = false; + + switch (static_cast(cost_for_scoring)) { + case (nav2_costmap_2d::LETHAL_OBSTACLE): + in_collision = true; + break; + case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): + in_collision = use_footprint_cost ? false : true; + break; + case (nav2_costmap_2d::NO_INFORMATION): + in_collision = false; + break; + default: + in_collision = false; + } + + + if (in_collision) { traj_cost = collision_cost_; trajectory_collide = true; break; @@ -206,13 +239,14 @@ void CostCritic::score(CriticData & data) // Let near-collision trajectory points be punished severely // Note that we collision check based on the footprint actual, // but score based on the center-point cost regardless - if (pose_cost >= static_cast(near_collision_cost_)) { + if (cost_for_scoring >= near_collision_cost_ && !near_goal) { traj_cost += critical_cost_; } else if (!near_goal) { // Generally prefer trajectories further from obstacles - traj_cost += pose_cost; + traj_cost += cost_for_scoring; } } + (*data.trajectories_in_collision)[i] = trajectory_collide; all_trajectories_collide &= trajectory_collide; } @@ -223,6 +257,18 @@ void CostCritic::score(CriticData & data) data.costs += repulsive_cost * (weight_ / static_cast(strided_traj_cols)); } + if (any_use_footprint_this_tick != s_footprint_active) { + static constexpr const char* RESET = "\033[0m"; + static constexpr const char* GREEN = "\033[32;1m"; + if (any_use_footprint_this_tick) { + RCLCPP_INFO(logger_, "%s[CostCritic] TURN ON FOOTPRINT COST%s",GREEN, RESET); + } else { + RCLCPP_INFO(logger_, "%s[CostCritic] TURN OFF FOOTPRINT COST%s",GREEN, RESET); + } + s_footprint_active = any_use_footprint_this_tick; + } + + data.fail_flag = all_trajectories_collide; } diff --git a/nav2_mppi_controller/src/critics/cost_critic_clear.cpp b/nav2_mppi_controller/src/critics/cost_critic_clear.cpp new file mode 100644 index 00000000000..9ab364dcd0b --- /dev/null +++ b/nav2_mppi_controller/src/critics/cost_critic_clear.cpp @@ -0,0 +1,281 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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. + +#include +#include "nav2_mppi_controller/critics/cost_critic.hpp" +#include "nav2_core/controller_exceptions.hpp" + +namespace mppi::critics +{ + +void CostCritic::initialize() +{ + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(consider_footprint_, "consider_footprint", false); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 3.81f); + getParam(critical_cost_, "critical_cost", 300.0f); + getParam(near_collision_cost_, "near_collision_cost", 253); + getParam(collision_cost_, "collision_cost", 1000000.0f); + getParam(near_goal_distance_, "near_goal_distance", 0.5f); + getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); + getParam(trajectory_point_step_, "trajectory_point_step", 2); + + // Normalized by cost value to put in same regime as other weights + weight_ /= 254.0f; + + // Normalize weight when parameter is changed dynamically as well + auto weightDynamicCb = [&]( + const rclcpp::Parameter & weight, rcl_interfaces::msg::SetParametersResult & /*result*/) { + weight_ = weight.as_double() / 254.0f; + }; + parameters_handler_->addParamCallback(name_ + ".cost_weight", weightDynamicCb); + + collision_checker_.setCostmap(costmap_); + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + + if (possible_collision_cost_ < 1.0f) { + RCLCPP_ERROR( + logger_, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + + if (costmap_ros_->getUseRadius() == consider_footprint_) { + RCLCPP_WARN( + logger_, + "Inconsistent configuration in collision checking. Please verify the robot's shape settings " + "in both the costmap and the cost critic."); + if (costmap_ros_->getUseRadius()) { + throw nav2_core::ControllerException( + "Considering footprint in collision checking but no robot footprint provided in the " + "costmap."); + } + } + + if(near_collision_cost_ > 253) { + RCLCPP_WARN(logger_, "Near collision cost is set higher than INSCRIBED_INFLATED_OBSTACLE"); + } + + RCLCPP_INFO( + logger_, + "InflationCostCritic instantiated with %d power and %f / %f weights. " + "Critic will collision check based on %s cost.", + power_, critical_cost_, weight_, consider_footprint_ ? + "footprint" : "circular"); +} + +float CostCritic::findCircumscribedCost( + std::shared_ptr costmap) +{ + double result = -1.0; + const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + if (static_cast(circum_radius) == circumscribed_radius_) { + // early return if footprint size is unchanged + return circumscribed_cost_; + } + + // check if the costmap has an inflation layer + const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer( + costmap, + inflation_layer_name_); + if (inflation_layer != nullptr) { + const double resolution = costmap->getCostmap()->getResolution(); + double inflation_radius = inflation_layer->getInflationRadius(); + if (inflation_radius < circum_radius) { + RCLCPP_ERROR( + rclcpp::get_logger("computeCircumscribedCost"), + "The inflation radius (%f) is smaller than the circumscribed radius (%f) " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times!", + inflation_radius, circum_radius); + result = 0.0; + return result; + } + result = inflation_layer->computeCost(circum_radius / resolution); + } else { + RCLCPP_WARN( + logger_, + "No inflation layer found in costmap configuration. " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times and not avoid anything but absolute collisions!"); + } + + circumscribed_radius_ = static_cast(circum_radius); + circumscribed_cost_ = static_cast(result); + + return circumscribed_cost_; +} + +void CostCritic::score(CriticData & data) +{ + if (!enabled_) { + return; + } + + static bool s_footprint_active = false; + bool any_use_footprint_this_tick = false; + + // Setup cost information for various parts of the critic + is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + auto * costmap = collision_checker_.getCostmap(); + origin_x_ = static_cast(costmap->getOriginX()); + origin_y_ = static_cast(costmap->getOriginY()); + resolution_ = static_cast(costmap->getResolution()); + size_x_ = costmap->getSizeInCellsX(); + size_y_ = costmap->getSizeInCellsY(); + + if (consider_footprint_) { + // footprint may have changed since initialization if user has dynamic footprints + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + } + + // If near the goal, don't apply the preferential term since the goal is near obstacles + bool near_goal = false; + if (data.state.local_path_length < near_goal_distance_) { + near_goal = true; + } + + Eigen::ArrayXf repulsive_cost(data.costs.rows()); + repulsive_cost.setZero(); + bool all_trajectories_collide = true; + + int strided_traj_cols = floor((data.trajectories.x.cols() - 1) / trajectory_point_step_) + 1; + int strided_traj_rows = data.trajectories.x.rows(); + int outer_stride = strided_traj_rows * trajectory_point_step_; + + // Initialize trajectories collision tracking + if (!data.trajectories_in_collision) { + data.trajectories_in_collision = std::vector(strided_traj_rows, false); + } + + const auto traj_x = Eigen::Map>(data.trajectories.x.data(), strided_traj_rows, strided_traj_cols, + Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto traj_y = Eigen::Map>(data.trajectories.y.data(), strided_traj_rows, strided_traj_cols, + Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto traj_yaw = Eigen::Map>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols, + Eigen::Stride<-1, -1>(outer_stride, 1)); + + for (int i = 0; i < strided_traj_rows; ++i) { + bool trajectory_collide = false; + float pose_cost = 0.0f; + float & traj_cost = repulsive_cost(i); + + for (int j = 0; j < strided_traj_cols; j++) { + float Tx = traj_x(i, j); + float Ty = traj_y(i, j); + unsigned int x_i = 0u, y_i = 0u; + + // The getCost doesn't use orientation + // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it + // So the center point has more information than the footprint + if (!worldToMapFloat(Tx, Ty, x_i, y_i)) { + pose_cost = 0.0f; // NO_INFORMATION in float + } else { + pose_cost = static_cast(costmap->getCost(getIndex(x_i, y_i))); + if (pose_cost < 1.0f) { + continue; // In free space + } + } + + float cost_for_scoring = pose_cost; + bool use_footprint_cost = consider_footprint_ && + (pose_cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f); + any_use_footprint_this_tick = any_use_footprint_this_tick || use_footprint_cost; + + if (use_footprint_cost) { + cost_for_scoring = static_cast(collision_checker_.footprintCostAtPose( + static_cast(Tx), static_cast(Ty), static_cast(traj_yaw(i, j)), + costmap_ros_->getRobotFootprint())); + } + + // Check for collision based on the appropriate cost + bool in_collision = false; + + switch (static_cast(cost_for_scoring)) { + case (nav2_costmap_2d::LETHAL_OBSTACLE): + in_collision = true; + break; + case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): + in_collision = use_footprint_cost ? false : true; + break; + case (nav2_costmap_2d::NO_INFORMATION): + in_collision = is_tracking_unknown_ ? false : true; + break; + default: + in_collision = false; + } + + + if (in_collision) { + traj_cost = collision_cost_; + trajectory_collide = true; + break; + } + + // Let near-collision trajectory points be punished severely + // Note that we collision check based on the footprint actual, + // but score based on the center-point cost regardless + if (cost_for_scoring >= near_collision_cost_ && !near_goal) { + traj_cost += critical_cost_; + } else if (!near_goal) { // Generally prefer trajectories further from obstacles + traj_cost += cost_for_scoring; + } + } + + (*data.trajectories_in_collision)[i] = trajectory_collide; + all_trajectories_collide &= trajectory_collide; + } + + if (power_ > 1u) { + data.costs += (repulsive_cost * + (weight_ / static_cast(strided_traj_cols))).pow(power_); + } else { + data.costs += repulsive_cost * (weight_ / static_cast(strided_traj_cols)); + } + + if (any_use_footprint_this_tick != s_footprint_active) { + static constexpr const char* RESET = "\033[0m"; + static constexpr const char* GREEN = "\033[32;1m"; + if (any_use_footprint_this_tick) { + RCLCPP_INFO(logger_, "%s[CostCritic] TURN ON FOOTPRINT COST%s",GREEN, RESET); + } else { + RCLCPP_INFO(logger_, "%s[CostCritic] TURN OFF FOOTPRINT COST%s",GREEN, RESET); + } + s_footprint_active = any_use_footprint_this_tick; + } + + + data.fail_flag = all_trajectories_collide; +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::CostCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 8ac899140f8..8792df2aaa2 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -20,8 +20,6 @@ namespace mppi::critics void GoalAngleCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 3.0f); @@ -36,17 +34,11 @@ void GoalAngleCritic::initialize() void GoalAngleCritic::score(CriticData & data) { - if (!enabled_) { + if (!enabled_ || data.state.local_path_length > threshold_to_consider_) { return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - - if (!utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) - { - return; - } + geometry_msgs::msg::Pose goal = utils::getLastPathPose(data.path); double goal_yaw = tf2::getYaw(goal.orientation); diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 927ef950de8..9889ef3f071 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -22,10 +22,7 @@ namespace mppi::critics void GoalCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); - getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0f); getParam(threshold_to_consider_, "threshold_to_consider", 1.4f); @@ -37,17 +34,11 @@ void GoalCritic::initialize() void GoalCritic::score(CriticData & data) { - if (!enabled_) { + if (!enabled_ || data.state.local_path_length > threshold_to_consider_) { return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - - if (!utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) - { - return; - } + geometry_msgs::msg::Pose goal = utils::getLastPathPose(data.path); auto goal_x = goal.position.x; auto goal_y = goal.position.y; diff --git a/nav2_mppi_controller/src/critics/obstacles_critic copy 3.cpp b/nav2_mppi_controller/src/critics/obstacles_critic copy 3.cpp new file mode 100644 index 00000000000..0577db0a76f --- /dev/null +++ b/nav2_mppi_controller/src/critics/obstacles_critic copy 3.cpp @@ -0,0 +1,249 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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. + +#include +#include "nav2_mppi_controller/critics/obstacles_critic.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" +#include "nav2_core/controller_exceptions.hpp" + +namespace mppi::critics +{ + +void ObstaclesCritic::initialize() +{ + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(consider_footprint_, "consider_footprint", false); + getParam(power_, "cost_power", 1); + getParam(repulsion_weight_, "repulsion_weight", 1.5f); + getParam(critical_weight_, "critical_weight", 20.0f); + getParam(collision_cost_, "collision_cost", 100000.0f); + getParam(collision_margin_distance_, "collision_margin_distance", 0.10f); + getParam(near_goal_distance_, "near_goal_distance", 0.5f); + getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); + + collision_checker_.setCostmap(costmap_); + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + + if (possible_collision_cost_ < 1.0f) { + RCLCPP_ERROR( + logger_, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + + if (costmap_ros_->getUseRadius() == consider_footprint_) { + RCLCPP_WARN( + logger_, + "Inconsistent configuration in collision checking. Please verify the robot's shape settings " + "in both the costmap and the obstacle critic."); + if (costmap_ros_->getUseRadius()) { + throw nav2_core::ControllerException( + "Considering footprint in collision checking but no robot footprint provided in the " + "costmap."); + } + } + + RCLCPP_INFO( + logger_, + "ObstaclesCritic instantiated with %d power and %f / %f weights. " + "Critic will collision check based on %s cost.", + power_, critical_weight_, repulsion_weight_, consider_footprint_ ? + "footprint" : "circular"); +} + +float ObstaclesCritic::findCircumscribedCost( + std::shared_ptr costmap) +{ + double result = -1.0; + const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + if (static_cast(circum_radius) == circumscribed_radius_) { + // early return if footprint size is unchanged + return circumscribed_cost_; + } + + // check if the costmap has an inflation layer + const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer( + costmap, + inflation_layer_name_); + if (inflation_layer != nullptr) { + const double resolution = costmap->getCostmap()->getResolution(); + result = inflation_layer->computeCost(circum_radius / resolution); + inflation_scale_factor_ = static_cast(inflation_layer->getCostScalingFactor()); + inflation_radius_ = static_cast(inflation_layer->getInflationRadius()); + } else { + RCLCPP_WARN( + logger_, + "No inflation layer found in costmap configuration. " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times and not avoid anything but absolute collisions!"); + } + + circumscribed_radius_ = static_cast(circum_radius); + circumscribed_cost_ = static_cast(result); + + return circumscribed_cost_; +} + +float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost) +{ + const float scale_factor = inflation_scale_factor_; + const float min_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius(); + float dist_to_obj = (scale_factor * min_radius - log(cost.cost) + log(253.0f)) / scale_factor; + + // If not footprint collision checking, the cost is using the center point cost and + // needs the radius subtracted to obtain the closest distance to the object + if (!cost.using_footprint) { + dist_to_obj -= min_radius; + } + + return dist_to_obj; +} + +void ObstaclesCritic::score(CriticData & data) +{ + if (!enabled_) { + return; + } + + if (consider_footprint_) { + // footprint may have changed since initialization if user has dynamic footprints + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + } + + // If near the goal, don't apply the preferential term since the goal is near obstacles + bool near_goal = false; + if (data.state.local_path_length < near_goal_distance_) { + near_goal = true; + } + + Eigen::ArrayXf raw_cost = Eigen::ArrayXf::Zero(data.costs.size()); + Eigen::ArrayXf repulsive_cost = Eigen::ArrayXf::Zero(data.costs.size()); + + const unsigned int traj_len = data.trajectories.x.cols(); + const unsigned int batch_size = data.trajectories.x.rows(); + bool all_trajectories_collide = true; + + for(unsigned int i = 0; i != batch_size; i++) { + bool trajectory_collide = false; + float traj_cost = 0.0f; + const auto & traj = data.trajectories; + CollisionCost pose_cost; + raw_cost(i) = 0.0f; + repulsive_cost(i) = 0.0f; + + for(unsigned int j = 0; j != traj_len; j++) { + pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j)); + if (pose_cost.cost < 1.0f) {continue;} // In free space + + if (inCollision(pose_cost.cost)) { + trajectory_collide = true; + break; + } + + // Cannot process repulsion if inflation layer does not exist + if (inflation_radius_ == 0.0f || inflation_scale_factor_ == 0.0f) { + continue; + } + + const float dist_to_obj = distanceToObstacle(pose_cost); + + // Let near-collision trajectory points be punished severely + if (dist_to_obj < collision_margin_distance_) { + traj_cost += (collision_margin_distance_ - dist_to_obj); + } + + // Generally prefer trajectories further from obstacles + if (!near_goal) { + repulsive_cost[i] += inflation_radius_ - dist_to_obj; + } + } + + if (!trajectory_collide) {all_trajectories_collide = false;} + raw_cost(i) = trajectory_collide ? collision_cost_ : traj_cost; + } + + // Normalize repulsive cost by trajectory length & lowest score to not overweight importance + // This is a preferential cost, not collision cost, to be tuned relative to desired behaviors + auto repulsive_cost_normalized = (repulsive_cost - repulsive_cost.minCoeff()) / traj_len; + + if (power_ > 1u) { + data.costs += + ((critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized)).pow(power_); + } else { + data.costs += (critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized); + } + + data.fail_flag = all_trajectories_collide; +} + +/** + * @brief Checks if cost represents a collision + * @param cost Costmap cost + * @return bool if in collision + */ +bool ObstaclesCritic::inCollision(float cost) const +{ + bool is_tracking_unknown = + costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + + using namespace nav2_costmap_2d; // NOLINT + switch (static_cast(cost)) { + case (LETHAL_OBSTACLE): + return true; + case (INSCRIBED_INFLATED_OBSTACLE): + return consider_footprint_ ? false : true; + case (NO_INFORMATION): + return is_tracking_unknown ? false : true; + } + + return false; +} + +CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) +{ + CollisionCost collision_cost; + float & cost = collision_cost.cost; + collision_cost.using_footprint = false; + unsigned int x_i, y_i; + if (!collision_checker_.worldToMap(x, y, x_i, y_i)) { + cost = nav2_costmap_2d::NO_INFORMATION; + return collision_cost; + } + cost = collision_checker_.pointCost(x_i, y_i); + + if (consider_footprint_ && + (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f)) + { + cost = static_cast(collision_checker_.footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint())); + collision_cost.using_footprint = true; + } + + return collision_cost; +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::ObstaclesCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/obstacles_critic copy.cpp b/nav2_mppi_controller/src/critics/obstacles_critic copy.cpp new file mode 100644 index 00000000000..387dfd64776 --- /dev/null +++ b/nav2_mppi_controller/src/critics/obstacles_critic copy.cpp @@ -0,0 +1,373 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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. + +#include +#include "nav2_mppi_controller/critics/obstacles_critic.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" +#include "nav2_core/controller_exceptions.hpp" + +namespace mppi::critics +{ + +void ObstaclesCritic::initialize() +{ + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(consider_footprint_, "consider_footprint", false); + getParam(power_, "cost_power", 1); + getParam(repulsion_weight_, "repulsion_weight", 1.5f); + getParam(critical_weight_, "critical_weight", 20.0f); + getParam(collision_cost_, "collision_cost", 100000.0f); + getParam(collision_margin_distance_, "collision_margin_distance", 0.10f); + getParam(near_goal_distance_, "near_goal_distance", 0.5f); + getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); + getParam(trajectory_point_step_, "trajectory_point_step", 2); + + //collision_checker_.setCostmap(costmap_); + collision_checker_ = std::make_unique( + costmap_ros_, parent_.lock()); + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + + if (possible_collision_cost_ < 1.0f) { + RCLCPP_ERROR( + logger_, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + + if (costmap_ros_->getUseRadius() == consider_footprint_) { + RCLCPP_WARN( + logger_, + "Inconsistent configuration in collision checking. Please verify the robot's shape settings " + "in both the costmap and the obstacle critic."); + if (costmap_ros_->getUseRadius()) { + throw nav2_core::ControllerException( + "Considering footprint in collision checking but no robot footprint provided in the " + "costmap."); + } + } + + RCLCPP_INFO( + logger_, + "ObstaclesCritic instantiated with %d power and %f / %f weights. " + "Critic will collision check based on %s cost.", + power_, critical_weight_, repulsion_weight_, consider_footprint_ ? + "footprint" : "circular"); + + RCLCPP_WARN(logger_, + "Inflation: radius=%.3f scale=%.3f, possible_collision_cost=%.1f, inscribed=%.3f", + inflation_radius_, inflation_scale_factor_, possible_collision_cost_, + costmap_ros_->getLayeredCostmap()->getInscribedRadius()); + +} + +float ObstaclesCritic::findCircumscribedCost( + std::shared_ptr costmap) +{ + double result = -1.0; + const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + if (static_cast(circum_radius) == circumscribed_radius_) { + // early return if footprint size is unchanged + return circumscribed_cost_; + } + + // check if the costmap has an inflation layer + const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer( + costmap, + inflation_layer_name_); + if (inflation_layer != nullptr) { + const double resolution = costmap->getCostmap()->getResolution(); + result = inflation_layer->computeCost(circum_radius / resolution); + inflation_scale_factor_ = static_cast(inflation_layer->getCostScalingFactor()); + inflation_radius_ = static_cast(inflation_layer->getInflationRadius()); + } else { + RCLCPP_WARN( + logger_, + "No inflation layer found in costmap configuration. " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times and not avoid anything but absolute collisions!"); + } + + circumscribed_radius_ = static_cast(circum_radius); + circumscribed_cost_ = static_cast(result); + + return circumscribed_cost_; +} + +float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost) +{ + const float scale_factor = inflation_scale_factor_; + const float min_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius(); + float dist_to_obj = (scale_factor * min_radius - log(cost.cost) + log(253.0f)) / scale_factor; + + // If not footprint collision checking, the cost is using the center point cost and + // needs the radius subtracted to obtain the closest distance to the object + if (!cost.using_footprint) { + dist_to_obj -= min_radius; + } + + return dist_to_obj; +} + +void ObstaclesCritic::score(CriticData & data) +{ + if (!enabled_) { + return; + } + + collision_checker_->setFootprint( + costmap_ros_->getRobotFootprint(), + costmap_ros_->getUseRadius(), + inflation_layer_name_); + + const auto & fp = costmap_ros_->getRobotFootprint(); + RCLCPP_WARN(logger_, + "[DBG] setFootprint: use_radius=%d, fp_pts=%zu, infl_radius=%.3f, infl_scale=%.3f, circ_cost=%.1f, inscribed=%.3f", + costmap_ros_->getUseRadius(), fp.size(), + inflation_radius_, inflation_scale_factor_, possible_collision_cost_, + costmap_ros_->getLayeredCostmap()->getInscribedRadius()); + if (!fp.empty()) { + float minx=fp[0].x, maxx=fp[0].x, miny=fp[0].y, maxy=fp[0].y; + for (auto &p: fp) { minx=std::min(minx,(float)p.x); maxx=std::max(maxx,(float)p.x); + miny=std::min(miny,(float)p.y); maxy=std::max(maxy,(float)p.y); } + RCLCPP_WARN(logger_, "[DBG] footprint bounds: x[%.3f, %.3f], y[%.3f, %.3f]", minx, maxx, miny, maxy); + } + + if (consider_footprint_) { + // footprint may have changed since initialization if user has dynamic footprints + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + } + + Eigen::ArrayXf raw_cost = Eigen::ArrayXf::Zero(data.costs.size()); + Eigen::ArrayXf repulsive_cost = Eigen::ArrayXf::Zero(data.costs.size()); + + //const unsigned int traj_len = data.trajectories.x.cols(); + //const unsigned int batch_size = data.trajectories.x.rows(); + const int traj_cols_full = data.trajectories.x.cols(); + const int traj_rows = data.trajectories.x.rows(); + const int strided_cols = std::floor((traj_cols_full - 1) / trajectory_point_step_) + 1; + + bool all_trajectories_collide = true; + + // Initialize trajectories collision tracking + // if (!data.trajectories_in_collision) { + // data.trajectories_in_collision = std::vector(batch_size, false); + // } + if (data.trajectories_in_collision && data.trajectories_in_collision->size() != (size_t)traj_rows) { + data.trajectories_in_collision->assign(traj_rows, false); + } + + + + // for(unsigned int i = 0; i != batch_size; i++) { + for (int i = 0; i < traj_rows; ++i) { + bool trajectory_collide = false; + // float traj_cost = 0.0f; + // const auto & traj = data.trajectories; + // CollisionCost pose_cost; + // raw_cost(i) = 0.0f; + // repulsive_cost(i) = 0.0f; + float& near_collision_acc = raw_cost(i); + float& repulse_acc = repulsive_cost(i); + near_collision_acc = 0.0f; + repulse_acc = 0.0f; + + + + // for(unsigned int j = 0; j != traj_len; j++) { + // pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j)); + // if (pose_cost.cost < 1.0f) {continue;} // In free space + + // if (inCollision(pose_cost.cost)) { + // trajectory_collide = true; + // break; + // } + + // // Cannot process repulsion if inflation layer does not exist + // if (inflation_radius_ == 0.0f || inflation_scale_factor_ == 0.0f) { + // continue; + // } + + // const float dist_to_obj = distanceToObstacle(pose_cost); + + // // Let near-collision trajectory points be punished severely + // if (dist_to_obj < collision_margin_distance_) { + // traj_cost += (collision_margin_distance_ - dist_to_obj); + // } + + // // Generally prefer trajectories further from obstacles + // if (!near_goal) { + // repulsive_cost[i] += inflation_radius_ - dist_to_obj; + // } + // } + + // 1) Zbierz próbki w stride (dokładnie jak w CostCritic) + std::vector xs; xs.reserve(strided_cols); + std::vector ys; ys.reserve(strided_cols); + std::vector yaws; yaws.reserve(strided_cols); + + for (int j = 0; j < strided_cols; ++j) { + const int col = j * trajectory_point_step_; + if (col >= traj_cols_full) break; + xs.push_back(data.trajectories.x(i, col)); + ys.push_back(data.trajectories.y(i, col)); + yaws.push_back(data.trajectories.yaws(i, col)); + } + + // 2) Jedno batched sprawdzenie + auto result = collision_checker_->inCollision( + xs, ys, yaws, + /* treat_unknown_as_obstacle = */ false); + + if (i == 0) { + auto vminmax = [](const std::vector& v){ + if (v.empty()) return std::make_pair(0.f, 0.f); + float mn=v[0], mx=v[0]; + for (auto& z: v) { mn=std::min(mn,z); mx=std::max(mx,z); } + return std::make_pair(mn,mx); + }; + auto cc = vminmax(result.center_cost); + auto fc = vminmax(result.footprint_cost); + RCLCPP_WARN(logger_, + "[DBG] inCollision: in=%d, N=%zu, center[min=%.1f max=%.1f], footprint[min=%.1f max=%.1f]", + (int)result.in_collision, result.center_cost.size(), + cc.first, cc.second, fc.first, fc.second); + + // pokaż też kilka próbek z trajektorii + for (size_t s = 0; s < std::min(5, xs.size()); ++s) { + RCLCPP_WARN(logger_, "[DBG] sample[%zu]: x=%.3f y=%.3f yaw=%.3f", s, xs[s], ys[s], yaws[s]); + } + } + + if (result.in_collision) { + trajectory_collide = true; + near_collision_acc = collision_cost_; + } else { + // 3) „Near-collision” preferencja dystansu – identycznie jak u Ciebie, + // ale karmione danymi z 'result' + for (size_t k = 0; k < result.center_cost.size(); ++k) { + const bool have_fp = (k < result.footprint_cost.size() && result.footprint_cost[k] >= 0.0f); + float cost = have_fp ? result.footprint_cost[k] : result.center_cost[k]; + if (cost < 1.0f) continue; // free + + float dist = distanceToObstacle(CollisionCost{cost, have_fp}); + + // „twardy” element: kara tuż przed kolizją + if (dist < collision_margin_distance_) { + near_collision_acc += (collision_margin_distance_ - dist); + } + // „miękki” element: delikatna preferencja dalej od przeszkód + if (inflation_radius_ > 0.f && inflation_scale_factor_ > 0.f && data.state.local_path_length >= near_goal_distance_) { + repulse_acc += std::max(0.f, inflation_radius_ - dist); + } + } + } + + // if (!trajectory_collide) {all_trajectories_collide = false;} + // (*data.trajectories_in_collision)[i] = trajectory_collide; + // raw_cost(i) = trajectory_collide ? collision_cost_ : traj_cost; + if (!trajectory_collide) { all_trajectories_collide = false; } + if (data.trajectories_in_collision) { + (*data.trajectories_in_collision)[i] = trajectory_collide; + } + } + + // Normalize repulsive cost by trajectory length & lowest score to not overweight importance + // This is a preferential cost, not collision cost, to be tuned relative to desired behaviors + //auto repulsive_cost_normalized = (repulsive_cost - repulsive_cost.minCoeff()) / traj_len; + auto repulsive_cost_normalized = (repulsive_cost - repulsive_cost.minCoeff()) / std::max(1, strided_cols); + + if (power_ > 1u) { + data.costs += + ((critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized)).pow(power_); + } else { + data.costs += (critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized); + } + + static int dbg_counter = 0; + if (++dbg_counter % 20 == 0) { + int coll_count = 0; + for (int i = 0; i < traj_rows; ++i) { + if (data.trajectories_in_collision && (*data.trajectories_in_collision)[i]) coll_count++; + } + RCLCPP_WARN(logger_, + "MPPI ObstaclesCritic: rows=%d, samples/row≈%d, collide_rows=%d, raw_cost_min=%.3f max=%.3f rep_min=%.3f max=%.3f", + traj_rows, strided_cols, + coll_count, + raw_cost.minCoeff(), raw_cost.maxCoeff(), + repulsive_cost.minCoeff(), repulsive_cost.maxCoeff()); + } + + data.fail_flag = all_trajectories_collide; +} + +/** + * @brief Checks if cost represents a collision + * @param cost Costmap cost + * @return bool if in collision + */ +bool ObstaclesCritic::inCollision(float cost) const +{ + bool is_tracking_unknown = + costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + + using namespace nav2_costmap_2d; // NOLINT + switch (static_cast(cost)) { + case (LETHAL_OBSTACLE): + return true; + case (INSCRIBED_INFLATED_OBSTACLE): + return consider_footprint_ ? false : true; + case (NO_INFORMATION): + return is_tracking_unknown ? false : true; + } + + return false; +} + +// CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) +// { +// CollisionCost collision_cost; +// float & cost = collision_cost.cost; +// collision_cost.using_footprint = false; +// unsigned int x_i, y_i; +// if (!collision_checker_.worldToMap(x, y, x_i, y_i)) { +// cost = nav2_costmap_2d::NO_INFORMATION; +// return collision_cost; +// } +// cost = collision_checker_.pointCost(x_i, y_i); + +// if (consider_footprint_ && +// (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f)) +// { +// cost = static_cast(collision_checker_.footprintCostAtPose( +// x, y, theta, costmap_ros_->getRobotFootprint())); +// collision_cost.using_footprint = true; +// } + +// return collision_cost; +// } + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::ObstaclesCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index db2972dfc9e..2a02150e9e1 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -24,8 +24,6 @@ namespace mppi::critics void ObstaclesCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); getParam(consider_footprint_, "consider_footprint", false); getParam(power_, "cost_power", 1); @@ -130,11 +128,9 @@ void ObstaclesCritic::score(CriticData & data) possible_collision_cost_ = findCircumscribedCost(costmap_ros_); } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; - if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, goal)) { + if (data.state.local_path_length < near_goal_distance_) { near_goal = true; } @@ -145,6 +141,12 @@ void ObstaclesCritic::score(CriticData & data) const unsigned int batch_size = data.trajectories.x.rows(); bool all_trajectories_collide = true; + // Initialize trajectories collision tracking + if (!data.trajectories_in_collision) { + data.trajectories_in_collision = std::vector(batch_size, false); + } + + for(unsigned int i = 0; i != batch_size; i++) { bool trajectory_collide = false; float traj_cost = 0.0f; @@ -181,6 +183,7 @@ void ObstaclesCritic::score(CriticData & data) } if (!trajectory_collide) {all_trajectories_collide = false;} + (*data.trajectories_in_collision)[i] = trajectory_collide; raw_cost(i) = trajectory_collide ? collision_cost_ : traj_cost; } diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 3ea13c641ab..028130de611 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -20,12 +20,9 @@ namespace mppi::critics void PathAlignCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 10.0f); - getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07f); getParam(offset_from_furthest_, "offset_from_furthest", 20); getParam(trajectory_point_step_, "trajectory_point_step", 4); @@ -33,6 +30,17 @@ void PathAlignCritic::initialize() threshold_to_consider_, "threshold_to_consider", 0.5f); getParam(use_path_orientations_, "use_path_orientations", false); + getParam(visualize_furthest_point_, "visualize_furthest_point", false); + + if (visualize_furthest_point_) { + auto node = parent_.lock(); + if (node) { + furthest_point_pub_ = node->create_publisher( + "PathAlignCritic/furthest_reached_path_point", 1); + furthest_point_pub_->on_activate(); + } + } + RCLCPP_INFO( logger_, @@ -42,16 +50,7 @@ void PathAlignCritic::initialize() void PathAlignCritic::score(CriticData & data) { - if (!enabled_) { - return; - } - - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - - // Don't apply close to goal, let the goal critics take over - if (utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) - { + if (!enabled_ || data.state.local_path_length < threshold_to_consider_) { return; } @@ -60,6 +59,23 @@ void PathAlignCritic::score(CriticData & data) // Up to furthest only, closest path point is always 0 from path handler const size_t path_segments_count = *data.furthest_reached_path_point; float path_segments_flt = static_cast(path_segments_count); + + // Visualize target pose if enabled + if (visualize_furthest_point_ && path_segments_count > 0 && + furthest_point_pub_->get_subscription_count() > 0) + { + auto furthest_point = std::make_unique(); + furthest_point->header.frame_id = costmap_ros_->getGlobalFrameID(); + furthest_point->header.stamp = clock_->now(); + furthest_point->pose.position.x = data.path.x(path_segments_count); + furthest_point->pose.position.y = data.path.y(path_segments_count); + furthest_point->pose.position.z = 0.0; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, data.path.yaws(path_segments_count)); + furthest_point->pose.orientation = tf2::toMsg(quat); + furthest_point_pub_->publish(std::move(furthest_point)); + } + if (path_segments_count < offset_from_furthest_) { return; } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index 96ceae1b93b..68901f1c2af 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -23,7 +23,6 @@ namespace mppi::critics void PathAngleCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); float vx_min; getParentParam(vx_min, "vx_min", -0.35); if (fabs(vx_min) < 1e-6f) { // zero @@ -42,6 +41,7 @@ void PathAngleCritic::initialize() getParam( max_angle_to_furthest_, "max_angle_to_furthest", 0.785398f); + getParam(visualize_furthest_point_, "visualize_furthest_point", false); int mode = 0; getParam(mode, "mode", mode); @@ -54,6 +54,15 @@ void PathAngleCritic::initialize() "don't allow for reversing! Setting mode to forward preference."); } + if (visualize_furthest_point_) { + auto node = parent_.lock(); + if (node) { + furthest_point_pub_ = node->create_publisher( + "PathAngleCritic/furthest_reached_path_point", 1); + furthest_point_pub_->on_activate(); + } + } + RCLCPP_INFO( logger_, "PathAngleCritic instantiated with %d power and %f weight. Mode set to: %s", @@ -62,15 +71,7 @@ void PathAngleCritic::initialize() void PathAngleCritic::score(CriticData & data) { - if (!enabled_) { - return; - } - - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - - if (utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) - { + if (!enabled_ || data.state.local_path_length < threshold_to_consider_) { return; } @@ -83,6 +84,21 @@ void PathAngleCritic::score(CriticData & data) const float goal_y = data.path.y(offsetted_idx); const float goal_yaw = data.path.yaws(offsetted_idx); const geometry_msgs::msg::Pose & pose = data.state.pose.pose; + + // Visualize target pose if enabled + if (visualize_furthest_point_ && furthest_point_pub_->get_subscription_count() > 0) { + auto furthest_point = std::make_unique(); + furthest_point->header.frame_id = costmap_ros_->getGlobalFrameID(); + furthest_point->header.stamp = clock_->now(); + furthest_point->pose.position.x = goal_x; + furthest_point->pose.position.y = goal_y; + furthest_point->pose.position.z = 0.0; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, goal_yaw); + furthest_point->pose.orientation = tf2::toMsg(quat); + furthest_point_pub_->publish(std::move(furthest_point)); + } + switch (mode_) { case PathAngleMode::FORWARD_PREFERENCE: diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index 4f8c1c70eec..e62a465a270 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -22,16 +22,24 @@ namespace mppi::critics void PathFollowCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); - getParam( threshold_to_consider_, "threshold_to_consider", 1.4f); getParam(offset_from_furthest_, "offset_from_furthest", 6); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0f); + getParam(visualize_furthest_point_, "visualize_furthest_point", false); + + if (visualize_furthest_point_) { + auto node = parent_.lock(); + if (node) { + furthest_point_pub_ = node->create_publisher( + "PathFollowCritic/furthest_reached_path_point", 1); + furthest_point_pub_->on_activate(); + } + } + } void PathFollowCritic::score(CriticData & data) @@ -40,12 +48,7 @@ void PathFollowCritic::score(CriticData & data) return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - - if (data.path.x.size() < 2 || - utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) - { + if (data.path.x.size() < 2 || data.state.local_path_length < threshold_to_consider_) { return; } @@ -68,6 +71,20 @@ void PathFollowCritic::score(CriticData & data) const auto path_x = data.path.x(offsetted_idx); const auto path_y = data.path.y(offsetted_idx); + // Visualize target pose if enabled + if (visualize_furthest_point_ && furthest_point_pub_->get_subscription_count() > 0) { + auto furthest_point = std::make_unique(); + furthest_point->header.frame_id = costmap_ros_->getGlobalFrameID(); + furthest_point->header.stamp = clock_->now(); + furthest_point->pose.position.x = path_x; + furthest_point->pose.position.y = path_y; + furthest_point->pose.position.z = 0.0; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, data.path.yaws(offsetted_idx)); + furthest_point->pose.orientation = tf2::toMsg(quat); + furthest_point_pub_->publish(std::move(furthest_point)); + } + const int && rightmost_idx = data.trajectories.x.cols() - 1; const auto last_x = data.trajectories.x.col(rightmost_idx); diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 47c84321559..20c58324365 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -22,8 +22,6 @@ namespace mppi::critics void PreferForwardCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0f); @@ -41,11 +39,7 @@ void PreferForwardCritic::score(CriticData & data) return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - - if (utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) - { + if (data.state.local_path_length < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index c9cfc79233c..2c6842c3a26 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -22,10 +22,7 @@ namespace mppi::critics void TwirlingCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); - getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 10.0f); @@ -39,12 +36,14 @@ void TwirlingCritic::score(CriticData & data) return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + if (data.goal_checker != nullptr) { + geometry_msgs::msg::Pose pose_tolerance; + geometry_msgs::msg::Twist velocity_tolerance; + data.goal_checker->getTolerances(pose_tolerance, velocity_tolerance); - if (utils::withinPositionGoalTolerance( - data.goal_checker, data.state.pose.pose, goal)) - { - return; + if (data.state.local_path_length < pose_tolerance.position.x) { + return; + } } if (power_ > 1u) { diff --git a/nav2_mppi_controller/src/critics/waypoint_critic.cpp b/nav2_mppi_controller/src/critics/waypoint_critic.cpp new file mode 100644 index 00000000000..ca8d5f1c3b6 --- /dev/null +++ b/nav2_mppi_controller/src/critics/waypoint_critic.cpp @@ -0,0 +1,207 @@ +// Copyright (c) 2025 +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_mppi_controller/critics/waypoint_critic.hpp" +#include +#include +#include +#include + +namespace mppi::critics +{ + +void WaypointCritic::initialize() +{ + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + auto getParam = parameters_handler_->getParamGetter(name_); + + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 1.0f); + getParam(position_tolerance_, "position_tolerance", 0.1f); + getParam(yaw_tolerance_, "yaw_tolerance", 0.1f); + getParam(waypoint_topic_, "waypoint_topic", std::string("truncated_path")); + getParam(use_orientation_, "use_orientation", true); + getParam(max_waypoint_distance_, "max_waypoint_distance", 10.0f); + + RCLCPP_INFO( + logger_, + "WaypointCritic instantiated with %d power, %f weight, " + "position tolerance: %f, yaw tolerance: %f, topic: %s", + power_, weight_, position_tolerance_, yaw_tolerance_, waypoint_topic_.c_str()); + + // Create subscription to waypoint topic + auto node = parent_.lock(); + if (node) { + waypoint_sub_ = node->create_subscription( + waypoint_topic_, + std::bind(&WaypointCritic::waypointCallback, this, std::placeholders::_1), + rclcpp::QoS(10).reliable()); + tf_buffer_ = std::make_shared(node->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_, node, false); + + RCLCPP_INFO(logger_, "WaypointCritic subscribed to topic: %s", waypoint_topic_.c_str()); + } else { + RCLCPP_ERROR(logger_, "Failed to get parent node for WaypointCritic subscription"); + } +} + +void WaypointCritic::waypointCallback(const nav_msgs::msg::Path::SharedPtr msg) +{ + std::lock_guard lock(waypoints_mutex_); + waypoints_ = msg->poses; + waypoints_frame_ = msg->header.frame_id; + RCLCPP_INFO( + logger_, + "WaypointCritic received %zu waypoints", + waypoints_.size()); +} + +void WaypointCritic::score(CriticData & data) +{ + if (!enabled_) { + return; + } + + std::lock_guard lock(waypoints_mutex_); + + // If no waypoints available, don't apply any cost + if (waypoints_.empty()) { + return; + } + + // Get current robot pose + const auto & robot_x = data.state.pose.pose.position.x; + const auto & robot_y = data.state.pose.pose.position.y; + + // Always target the first waypoint (assuming completed waypoints are removed by publisher) + int target_waypoint_idx = getTargetWaypoint(); + + if (target_waypoint_idx < 0) { + return; + } + + const std::string target_frame = data.state.pose.header.frame_id; + geometry_msgs::msg::PoseStamped wp_in = waypoints_[target_waypoint_idx]; + if (wp_in.header.frame_id.empty()) { + wp_in.header.frame_id = waypoints_frame_; + } + geometry_msgs::msg::PoseStamped wp_tf; + if (wp_in.header.frame_id == target_frame || !tf_buffer_) { + wp_tf = wp_in; + } else { + if (!tf_buffer_->canTransform( + target_frame, wp_in.header.frame_id, wp_in.header.stamp, + tf2::durationFromSec(0.0))) + { + RCLCPP_WARN(logger_, "TF %s->%s not available yet", + wp_in.header.frame_id.c_str(), target_frame.c_str()); + return; + } + tf_buffer_->transform(wp_in, wp_tf, target_frame, tf2::durationFromSec(0.0)); + } + const float target_x = static_cast(wp_tf.pose.position.x); + const float target_y = static_cast(wp_tf.pose.position.y); + const float target_yaw = static_cast(tf2::getYaw(wp_tf.pose.orientation)); + + +// const auto & plan = data.path; +// if (plan.x.size() == 0) { +// return; +// } +// RCLCPP_INFO( +// logger_, +// "data.path: size=%zu frame=%s", +// static_cast(plan.x.size()), +// data.state.pose.header.frame_id.c_str()); +// const float target_x = plan.x(0); +// const float target_y = plan.y(0); +// const float target_yaw = (plan.yaws.size() > 0) ? plan.yaws(0) : 0.0f; + + + // Check if waypoint is within reasonable distance + double waypoint_distance = calculateDistance(robot_x, robot_y, target_x, target_y); + if (waypoint_distance > max_waypoint_distance_) { + return; + } + + + // Calculate position costs for all trajectories + const auto delta_x = data.trajectories.x - target_x; + const auto delta_y = data.trajectories.y - target_y; + // RCLCPP_INFO( + // logger_, + // "Traj mean: x=%.3f y=%.3f | target=(%.3f, %.3f)", + // static_cast(data.trajectories.x.mean()), + // static_cast(data.trajectories.y.mean()), + // target_x, target_y); + + // Calculate euclidean distance cost + auto position_distances = (delta_x.square() + delta_y.square()).sqrt(); + + // Apply position tolerance - penalize trajectories that are farther than tolerance + auto position_costs = (position_distances - position_tolerance_).cwiseMax(0.0f); + + // If orientation is considered, add yaw cost + Eigen::ArrayXf total_costs = position_costs; + + if (use_orientation_) { + // Calculate yaw differences for all trajectories using utils function + auto yaw_differences = utils::shortest_angular_distance(data.trajectories.yaws, target_yaw); + + // Apply yaw tolerance + auto yaw_costs = (yaw_differences.abs() - yaw_tolerance_).cwiseMax(0.0f); + + // Combine position and yaw costs + total_costs = position_costs + yaw_costs; + } + + // Apply power and weight + if (power_ > 1u) { + data.costs += (total_costs.rowwise().mean() * weight_).pow(power_); + } else { + data.costs += (total_costs.rowwise().mean() * weight_).eval(); + } +} + +int WaypointCritic::getTargetWaypoint() +{ + if (waypoints_.empty()) { + return -1; + } + + // Always target the first waypoint in the list + // (assuming the publisher removes completed waypoints) + return 0; +} + +double WaypointCritic::calculateDistance(double x1, double y1, double x2, double y2) +{ + double dx = x2 - x1; + double dy = y2 - y1; + return sqrt(dx * dx + dy * dy); +} + +double WaypointCritic::calculateAngularDifference(double angle1, double angle2) +{ + double diff = angle2 - angle1; + while (diff > M_PI) diff -= 2.0 * M_PI; + while (diff < -M_PI) diff += 2.0 * M_PI; + return diff; +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS(mppi::critics::WaypointCritic, mppi::critics::CriticFunction) \ No newline at end of file diff --git a/nav2_mppi_controller/src/optimizer copy 2.cpp b/nav2_mppi_controller/src/optimizer copy 2.cpp new file mode 100644 index 00000000000..013b29de502 --- /dev/null +++ b/nav2_mppi_controller/src/optimizer copy 2.cpp @@ -0,0 +1,586 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2025 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. + +#include "nav2_mppi_controller/optimizer.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "nav2_core/controller_exceptions.hpp" +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" +#include "nav2_ros_common/node_utils.hpp" + +namespace mppi +{ + +void Optimizer::initialize( + nav2::LifecycleNode::WeakPtr parent, const std::string & name, + std::shared_ptr costmap_ros, + std::shared_ptr tf_buffer, + ParametersHandler * param_handler) +{ + parent_ = parent; + name_ = name; + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros_->getCostmap(); + parameters_handler_ = param_handler; + tf_buffer_ = tf_buffer; + + auto node = parent_.lock(); + logger_ = node->get_logger(); + + getParams(); + + critic_manager_.on_configure(parent_, name_, costmap_ros_, parameters_handler_); + noise_generator_.initialize(settings_, isHolonomic(), name_, parameters_handler_); + + // This may throw an exception if not valid and fail initialization + nav2::declare_parameter_if_not_declared( + node, name_ + ".TrajectoryValidator.plugin", + rclcpp::ParameterValue("mppi::DefaultOptimalTrajectoryValidator")); + std::string validator_plugin_type = nav2::get_plugin_type_param( + node, name_ + ".TrajectoryValidator"); + validator_loader_ = std::make_unique>( + "nav2_mppi_controller", "mppi::OptimalTrajectoryValidator"); + trajectory_validator_ = validator_loader_->createUniqueInstance(validator_plugin_type); + trajectory_validator_->initialize( + parent_, name_ + ".TrajectoryValidator", + costmap_ros_, parameters_handler_, tf_buffer, settings_); + RCLCPP_INFO(logger_, "Loaded trajectory validator plugin: %s", validator_plugin_type.c_str()); + + reset(); +} + +void Optimizer::shutdown() +{ + noise_generator_.shutdown(); +} + +void Optimizer::getParams() +{ + std::string motion_model_name; + + auto & s = settings_; + auto getParam = parameters_handler_->getParamGetter(name_); + auto getParentParam = parameters_handler_->getParamGetter(""); + getParam(s.model_dt, "model_dt", 0.05f); + getParam(s.time_steps, "time_steps", 56); + getParam(s.batch_size, "batch_size", 1000); + getParam(s.iteration_count, "iteration_count", 1); + getParam(s.temperature, "temperature", 0.3f); + getParam(s.gamma, "gamma", 0.015f); + getParam(s.base_constraints.vx_max, "vx_max", 0.5f); + getParam(s.base_constraints.vx_min, "vx_min", -0.35f); + getParam(s.base_constraints.vy, "vy_max", 0.5f); + getParam(s.base_constraints.wz, "wz_max", 1.9f); + getParam(s.base_constraints.ax_max, "ax_max", 3.0f); + getParam(s.base_constraints.ax_min, "ax_min", -3.0f); + getParam(s.base_constraints.ay_max, "ay_max", 3.0f); + getParam(s.base_constraints.ay_min, "ay_min", -3.0f); + getParam(s.base_constraints.az_max, "az_max", 3.5f); + getParam(s.sampling_std.vx, "vx_std", 0.2f); + getParam(s.sampling_std.vy, "vy_std", 0.2f); + getParam(s.sampling_std.wz, "wz_std", 0.4f); + getParam(s.retry_attempt_limit, "retry_attempt_limit", 1); + + s.base_constraints.ax_max = fabs(s.base_constraints.ax_max); + if (s.base_constraints.ax_min > 0.0) { + s.base_constraints.ax_min = -1.0 * s.base_constraints.ax_min; + RCLCPP_WARN( + logger_, + "Sign of the parameter ax_min is incorrect, consider setting it negative."); + } + + if (s.base_constraints.ay_min > 0.0) { + s.base_constraints.ay_min = -1.0 * s.base_constraints.ay_min; + RCLCPP_WARN( + logger_, + "Sign of the parameter ay_min is incorrect, consider setting it negative."); + } + + + getParam(motion_model_name, "motion_model", std::string("DiffDrive")); + + s.constraints = s.base_constraints; + + setMotionModel(motion_model_name); + parameters_handler_->addPostCallback([this]() {reset();}); + + double controller_frequency; + getParentParam(controller_frequency, "controller_frequency", 0.0, ParameterType::Static); + setOffset(controller_frequency); +} + +void Optimizer::setOffset(double controller_frequency) +{ + const double controller_period = 1.0 / controller_frequency; + constexpr double eps = 1e-6; + + if ((controller_period + eps) < settings_.model_dt) { + RCLCPP_WARN( + logger_, + "Controller period is less then model dt, consider setting it equal"); + } else if (abs(controller_period - settings_.model_dt) < eps) { + RCLCPP_INFO( + logger_, + "Controller period is equal to model dt. Control sequence " + "shifting is ON"); + settings_.shift_control_sequence = true; + } else { + throw nav2_core::ControllerException( + "Controller period more then model dt, set it equal to model dt"); + } +} + +void Optimizer::reset(bool reset_dynamic_speed_limits) +{ + state_.reset(settings_.batch_size, settings_.time_steps); + control_sequence_.reset(settings_.time_steps); + control_history_[0] = {0.0f, 0.0f, 0.0f}; + control_history_[1] = {0.0f, 0.0f, 0.0f}; + control_history_[2] = {0.0f, 0.0f, 0.0f}; + control_history_[3] = {0.0f, 0.0f, 0.0f}; + + if (reset_dynamic_speed_limits) { + settings_.constraints = settings_.base_constraints; + } + + costs_.setZero(settings_.batch_size); + generated_trajectories_.reset(settings_.batch_size, settings_.time_steps); + + noise_generator_.reset(settings_, isHolonomic()); + motion_model_->initialize(settings_.constraints, settings_.model_dt); + trajectory_validator_->initialize( + parent_, name_ + ".TrajectoryValidator", + costmap_ros_, parameters_handler_, tf_buffer_, settings_); + + RCLCPP_INFO(logger_, "Optimizer reset"); +} + +bool Optimizer::isHolonomic() const +{ + return motion_model_->isHolonomic(); +} + +std::tuple Optimizer::evalControl( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::Twist & robot_speed, + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal, + nav2_core::GoalChecker * goal_checker) +{ + prepare(robot_pose, robot_speed, plan, goal, goal_checker); + Eigen::ArrayXXf optimal_trajectory; + bool trajectory_valid = true; + + do { + optimize(); + optimal_trajectory = getOptimizedTrajectory(); + switch (trajectory_validator_->validateTrajectory( + optimal_trajectory, control_sequence_, robot_pose, robot_speed, plan, goal)) + { + case mppi::ValidationResult::SOFT_RESET: + trajectory_valid = false; + RCLCPP_WARN(logger_, "Soft reset triggered by trajectory validator"); + break; + case mppi::ValidationResult::FAILURE: + throw nav2_core::NoValidControl( + "Trajectory validator failed to validate trajectory, hard reset triggered."); + case mppi::ValidationResult::SUCCESS: + default: + trajectory_valid = true; + break; + } + } while (fallback(critics_data_.fail_flag || !trajectory_valid)); + + utils::savitskyGolayFilter(control_sequence_, control_history_, settings_); + auto control = getControlFromSequenceAsTwist(plan.header.stamp); + + if (settings_.shift_control_sequence) { + shiftControlSequence(); + } + + return std::make_tuple(control, optimal_trajectory); +} + +void Optimizer::optimize() +{ + for (size_t i = 0; i < settings_.iteration_count; ++i) { + generateNoisedTrajectories(); + critic_manager_.evalTrajectoriesScores(critics_data_); + updateControlSequence(); + } +} + +bool Optimizer::fallback(bool fail) +{ + static size_t counter = 0; + + if (!fail) { + counter = 0; + return false; + } + + reset(); + + if (++counter > settings_.retry_attempt_limit) { + counter = 0; + throw nav2_core::NoValidControl("Optimizer fail to compute path"); + } + + return true; +} + +void Optimizer::prepare( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::Twist & robot_speed, + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal, + nav2_core::GoalChecker * goal_checker) +{ + state_.pose = robot_pose; + state_.speed = robot_speed; + state_.local_path_length = nav2_util::geometry_utils::calculate_path_length(plan); + path_ = utils::toTensor(plan); + costs_.setZero(); + goal_ = goal; + + critics_data_.fail_flag = false; + critics_data_.goal_checker = goal_checker; + critics_data_.motion_model = motion_model_; + critics_data_.furthest_reached_path_point.reset(); + critics_data_.path_pts_valid.reset(); +} + +void Optimizer::shiftControlSequence() +{ + auto size = control_sequence_.vx.size(); + utils::shiftColumnsByOnePlace(control_sequence_.vx, -1); + utils::shiftColumnsByOnePlace(control_sequence_.wz, -1); + control_sequence_.vx(size - 1) = control_sequence_.vx(size - 2); + control_sequence_.wz(size - 1) = control_sequence_.wz(size - 2); + + if (isHolonomic()) { + utils::shiftColumnsByOnePlace(control_sequence_.vy, -1); + control_sequence_.vy(size - 1) = control_sequence_.vy(size - 2); + } +} + +void Optimizer::generateNoisedTrajectories() +{ + noise_generator_.setNoisedControls(state_, control_sequence_); + noise_generator_.generateNextNoises(); + updateStateVelocities(state_); + integrateStateVelocities(generated_trajectories_, state_); +} + +void Optimizer::applyControlSequenceConstraints() +{ + auto & s = settings_; + + float max_delta_vx = s.model_dt * s.constraints.ax_max; + float min_delta_vx = s.model_dt * s.constraints.ax_min; + float max_delta_vy = s.model_dt * s.constraints.ay_max; + float min_delta_vy = s.model_dt * s.constraints.ay_min; + float max_delta_wz = s.model_dt * s.constraints.az_max; + float vx_last = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, control_sequence_.vx(0)); + float wz_last = utils::clamp(-s.constraints.wz, s.constraints.wz, control_sequence_.wz(0)); + control_sequence_.vx(0) = vx_last; + control_sequence_.wz(0) = wz_last; + float vy_last = 0; + if (isHolonomic()) { + vy_last = utils::clamp(-s.constraints.vy, s.constraints.vy, control_sequence_.vy(0)); + control_sequence_.vy(0) = vy_last; + } + + for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) { + float & vx_curr = control_sequence_.vx(i); + vx_curr = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, vx_curr); + if(vx_last > 0) { + vx_curr = utils::clamp(vx_last + min_delta_vx, vx_last + max_delta_vx, vx_curr); + } else { + vx_curr = utils::clamp(vx_last - max_delta_vx, vx_last - min_delta_vx, vx_curr); + } + vx_last = vx_curr; + + float & wz_curr = control_sequence_.wz(i); + wz_curr = utils::clamp(-s.constraints.wz, s.constraints.wz, wz_curr); + wz_curr = utils::clamp(wz_last - max_delta_wz, wz_last + max_delta_wz, wz_curr); + wz_last = wz_curr; + + if (isHolonomic()) { + float & vy_curr = control_sequence_.vy(i); + vy_curr = utils::clamp(-s.constraints.vy, s.constraints.vy, vy_curr); + if(vy_last > 0) { + vy_curr = utils::clamp(vy_last + min_delta_vy, vy_last + max_delta_vy, vy_curr); + } else { + vy_curr = utils::clamp(vy_last - max_delta_vy, vy_last - min_delta_vy, vy_curr); + } + vy_last = vy_curr; + } + } + + motion_model_->applyConstraints(control_sequence_); +} + +void Optimizer::updateStateVelocities( + models::State & state) const +{ + updateInitialStateVelocities(state); + propagateStateVelocitiesFromInitials(state); +} + +void Optimizer::updateInitialStateVelocities( + models::State & state) const +{ + state.vx.col(0) = static_cast(state.speed.linear.x); + state.wz.col(0) = static_cast(state.speed.angular.z); + + if (isHolonomic()) { + state.vy.col(0) = static_cast(state.speed.linear.y); + } +} + +void Optimizer::propagateStateVelocitiesFromInitials( + models::State & state) const +{ + motion_model_->predict(state); +} + +void Optimizer::integrateStateVelocities( + Eigen::Array & trajectory, + const Eigen::ArrayXXf & sequence) const +{ + float initial_yaw = static_cast(tf2::getYaw(state_.pose.pose.orientation)); + + const auto vx = sequence.col(0); + const auto wz = sequence.col(1); + + auto traj_x = trajectory.col(0); + auto traj_y = trajectory.col(1); + auto traj_yaws = trajectory.col(2); + + const size_t n_size = traj_yaws.size(); + if (n_size == 0) { + return; + } + + float last_yaw = initial_yaw; + for(size_t i = 0; i != n_size; i++) { + last_yaw += wz(i) * settings_.model_dt; + traj_yaws(i) = last_yaw; + } + + Eigen::ArrayXf yaw_cos = traj_yaws.cos(); + Eigen::ArrayXf yaw_sin = traj_yaws.sin(); + utils::shiftColumnsByOnePlace(yaw_cos, 1); + utils::shiftColumnsByOnePlace(yaw_sin, 1); + yaw_cos(0) = cosf(initial_yaw); + yaw_sin(0) = sinf(initial_yaw); + + auto dx = (vx * yaw_cos).eval(); + auto dy = (vx * yaw_sin).eval(); + + if (isHolonomic()) { + auto vy = sequence.col(2); + dx = (dx - vy * yaw_sin).eval(); + dy = (dy + vy * yaw_cos).eval(); + } + + float last_x = state_.pose.pose.position.x; + float last_y = state_.pose.pose.position.y; + for(size_t i = 0; i != n_size; i++) { + last_x += dx(i) * settings_.model_dt; + last_y += dy(i) * settings_.model_dt; + traj_x(i) = last_x; + traj_y(i) = last_y; + } +} + +void Optimizer::integrateStateVelocities( + models::Trajectories & trajectories, + const models::State & state) const +{ + auto initial_yaw = static_cast(tf2::getYaw(state.pose.pose.orientation)); + const size_t n_cols = trajectories.yaws.cols(); + + Eigen::ArrayXf last_yaws = Eigen::ArrayXf::Constant(trajectories.yaws.rows(), initial_yaw); + for (size_t i = 0; i != n_cols; i++) { + last_yaws += state.wz.col(i) * settings_.model_dt; + trajectories.yaws.col(i) = last_yaws; + } + + Eigen::ArrayXXf yaw_cos = trajectories.yaws.cos(); + Eigen::ArrayXXf yaw_sin = trajectories.yaws.sin(); + utils::shiftColumnsByOnePlace(yaw_cos, 1); + utils::shiftColumnsByOnePlace(yaw_sin, 1); + yaw_cos.col(0) = cosf(initial_yaw); + yaw_sin.col(0) = sinf(initial_yaw); + + auto dx = (state.vx * yaw_cos).eval(); + auto dy = (state.vx * yaw_sin).eval(); + + if (isHolonomic()) { + dx -= state.vy * yaw_sin; + dy += state.vy * yaw_cos; + } + + Eigen::ArrayXf last_x = Eigen::ArrayXf::Constant(trajectories.x.rows(), + state.pose.pose.position.x); + Eigen::ArrayXf last_y = Eigen::ArrayXf::Constant(trajectories.y.rows(), + state.pose.pose.position.y); + + for (size_t i = 0; i != n_cols; i++) { + last_x += dx.col(i) * settings_.model_dt; + last_y += dy.col(i) * settings_.model_dt; + trajectories.x.col(i) = last_x; + trajectories.y.col(i) = last_y; + } +} + +Eigen::ArrayXXf Optimizer::getOptimizedTrajectory() +{ + const bool is_holo = isHolonomic(); + Eigen::ArrayXXf sequence = Eigen::ArrayXXf(settings_.time_steps, is_holo ? 3 : 2); + Eigen::Array trajectories = + Eigen::Array(settings_.time_steps, 3); + + sequence.col(0) = control_sequence_.vx; + sequence.col(1) = control_sequence_.wz; + + if (is_holo) { + sequence.col(2) = control_sequence_.vy; + } + + integrateStateVelocities(trajectories, sequence); + return trajectories; +} + +const models::ControlSequence & Optimizer::getOptimalControlSequence() +{ + return control_sequence_; +} + +void Optimizer::updateControlSequence() +{ + const bool is_holo = isHolonomic(); + auto & s = settings_; + + auto vx_T = control_sequence_.vx.transpose(); + auto bounded_noises_vx = state_.cvx.rowwise() - vx_T; + const float gamma_vx = s.gamma / (s.sampling_std.vx * s.sampling_std.vx); + costs_ += (gamma_vx * (bounded_noises_vx.rowwise() * vx_T).rowwise().sum()).eval(); + + if (s.sampling_std.wz > 0.0f) { + auto wz_T = control_sequence_.wz.transpose(); + auto bounded_noises_wz = state_.cwz.rowwise() - wz_T; + const float gamma_wz = s.gamma / (s.sampling_std.wz * s.sampling_std.wz); + costs_ += (gamma_wz * (bounded_noises_wz.rowwise() * wz_T).rowwise().sum()).eval(); + } + + if (is_holo) { + auto vy_T = control_sequence_.vy.transpose(); + auto bounded_noises_vy = state_.cvy.rowwise() - vy_T; + const float gamma_vy = s.gamma / (s.sampling_std.vy * s.sampling_std.vy); + costs_ += (gamma_vy * (bounded_noises_vy.rowwise() * vy_T).rowwise().sum()).eval(); + } + + auto costs_normalized = costs_ - costs_.minCoeff(); + const float inv_temp = 1.0f / s.temperature; + auto softmaxes = (-inv_temp * costs_normalized).exp().eval(); + softmaxes /= softmaxes.sum(); + + auto softmax_mat = softmaxes.matrix(); + control_sequence_.vx = state_.cvx.transpose().matrix() * softmax_mat; + control_sequence_.wz = state_.cwz.transpose().matrix() * softmax_mat; + + if (is_holo) { + control_sequence_.vy = state_.cvy.transpose().matrix() * softmax_mat; + } + + applyControlSequenceConstraints(); +} + +geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist( + const builtin_interfaces::msg::Time & stamp) +{ + unsigned int offset = settings_.shift_control_sequence ? 1 : 0; + + auto vx = control_sequence_.vx(offset); + auto wz = control_sequence_.wz(offset); + + if (isHolonomic()) { + auto vy = control_sequence_.vy(offset); + return utils::toTwistStamped(vx, vy, wz, stamp, costmap_ros_->getBaseFrameID()); + } + + return utils::toTwistStamped(vx, wz, stamp, costmap_ros_->getBaseFrameID()); +} + +void Optimizer::setMotionModel(const std::string & model) +{ + if (model == "DiffDrive") { + motion_model_ = std::make_shared(); + } else if (model == "Omni") { + motion_model_ = std::make_shared(); + } else if (model == "Ackermann") { + motion_model_ = std::make_shared(parameters_handler_, name_); + } else { + throw nav2_core::ControllerException( + std::string( + "Model " + model + " is not valid! Valid options are DiffDrive, Omni, " + "or Ackermann")); + } + motion_model_->initialize(settings_.constraints, settings_.model_dt); +} + +void Optimizer::setSpeedLimit(double speed_limit, bool percentage) +{ + auto & s = settings_; + if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) { + s.constraints.vx_max = s.base_constraints.vx_max; + s.constraints.vx_min = s.base_constraints.vx_min; + s.constraints.vy = s.base_constraints.vy; + s.constraints.wz = s.base_constraints.wz; + } else { + if (percentage) { + // Speed limit is expressed in % from maximum speed of robot + double ratio = speed_limit / 100.0; + s.constraints.vx_max = s.base_constraints.vx_max * ratio; + s.constraints.vx_min = s.base_constraints.vx_min * ratio; + s.constraints.vy = s.base_constraints.vy * ratio; + s.constraints.wz = s.base_constraints.wz * ratio; + } else { + // Speed limit is expressed in absolute value + double ratio = speed_limit / s.base_constraints.vx_max; + s.constraints.vx_max = s.base_constraints.vx_max * ratio; + s.constraints.vx_min = s.base_constraints.vx_min * ratio; + s.constraints.vy = s.base_constraints.vy * ratio; + s.constraints.wz = s.base_constraints.wz * ratio; + } + } +} + +models::Trajectories & Optimizer::getGeneratedTrajectories() +{ + return generated_trajectories_; +} + +} // namespace mppi diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 97a16249814..b12c7dd6925 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -257,6 +257,7 @@ void Optimizer::prepare( { state_.pose = robot_pose; state_.speed = robot_speed; + state_.local_path_length = nav2_util::geometry_utils::calculate_path_length(plan); path_ = utils::toTensor(plan); costs_.setZero(); goal_ = goal; @@ -266,6 +267,7 @@ void Optimizer::prepare( critics_data_.motion_model = motion_model_; critics_data_.furthest_reached_path_point.reset(); critics_data_.path_pts_valid.reset(); + critics_data_.trajectories_in_collision.reset(); } void Optimizer::shiftControlSequence() diff --git a/nav2_mppi_controller/src/path_handler copy 2.cpp b/nav2_mppi_controller/src/path_handler copy 2.cpp new file mode 100644 index 00000000000..b49e8991306 --- /dev/null +++ b/nav2_mppi_controller/src/path_handler copy 2.cpp @@ -0,0 +1,247 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// Copyright (c) 2023 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. + +#include "nav2_mppi_controller/tools/path_handler.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/robot_utils.hpp" + +namespace mppi +{ + +void PathHandler::initialize( + nav2::LifecycleNode::WeakPtr parent, const std::string & name, + std::shared_ptr costmap, + std::shared_ptr buffer, ParametersHandler * param_handler) +{ + name_ = name; + costmap_ = costmap; + tf_buffer_ = buffer; + auto node = parent.lock(); + logger_ = node->get_logger(); + parameters_handler_ = param_handler; + + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(max_robot_pose_search_dist_, "max_robot_pose_search_dist", getMaxCostmapDist()); + getParam(prune_distance_, "prune_distance", 1.5); + getParam(transform_tolerance_, "transform_tolerance", 0.1); + getParam(enforce_path_inversion_, "enforce_path_inversion", false); + getParam(enforce_path_rotation_, "enforce_path_rotation", false); + + if (enforce_path_inversion_ || enforce_path_rotation_) { + getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2); + getParam(inversion_yaw_tolerance_, "inversion_yaw_tolerance", 0.4); + getParam(minimum_rotation_angle_, "minimum_rotation_angle", 0.785); + inversion_locale_ = 0u; + } +} + +std::pair +PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( + const geometry_msgs::msg::PoseStamped & global_pose) +{ + using nav2_util::geometry_utils::euclidean_distance; + + auto begin = global_plan_up_to_inversion_.poses.begin(); + + // Limit the search for the closest pose up to max_robot_pose_search_dist on the path + auto closest_pose_upper_bound = + nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), + max_robot_pose_search_dist_); + + // Find closest point to the robot + auto closest_point = nav2_util::geometry_utils::min_by( + begin, closest_pose_upper_bound, + [&global_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(global_pose, ps); + }); + + nav_msgs::msg::Path transformed_plan; + transformed_plan.header.frame_id = costmap_->getGlobalFrameID(); + transformed_plan.header.stamp = global_pose.header.stamp; + + auto pruned_plan_end = + nav2_util::geometry_utils::first_after_integrated_distance( + closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); + + unsigned int mx, my; + // Find the furthest relevant pose on the path to consider within costmap + // bounds + // Transforming it to the costmap frame in the same loop + for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end; + ++global_plan_pose) + { + // Transform from global plan frame to costmap frame + geometry_msgs::msg::PoseStamped costmap_plan_pose; + global_plan_pose->header.stamp = global_pose.header.stamp; + global_plan_pose->header.frame_id = global_plan_.header.frame_id; + nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_buffer_, + costmap_->getGlobalFrameID(), transform_tolerance_); + + // Check if pose is inside the costmap + if (!costmap_->getCostmap()->worldToMap( + costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my)) + { + return {transformed_plan, closest_point}; + } + + // Filling the transformed plan to return with the transformed pose + transformed_plan.poses.push_back(costmap_plan_pose); + } + + return {transformed_plan, closest_point}; +} + +geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame( + const geometry_msgs::msg::PoseStamped & pose) +{ + if (global_plan_up_to_inversion_.poses.empty()) { + throw nav2_core::InvalidPath("Received plan with zero length"); + } + + geometry_msgs::msg::PoseStamped robot_pose; + if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_buffer_, + global_plan_up_to_inversion_.header.frame_id, transform_tolerance_)) + { + throw nav2_core::ControllerTFError( + "Unable to transform robot pose into global plan's frame"); + } + + return robot_pose; +} + +nav_msgs::msg::Path PathHandler::transformPath( + const geometry_msgs::msg::PoseStamped & robot_pose) +{ + // Find relevant bounds of path to use + geometry_msgs::msg::PoseStamped global_pose = + transformToGlobalPlanFrame(robot_pose); + auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose); + + prunePlan(global_plan_up_to_inversion_, lower_bound); + + if ((enforce_path_inversion_ || enforce_path_rotation_) && inversion_locale_ != 0u) { + if (isWithinInversionTolerances(global_pose)) { + // Robot has reached the inversion/rotation point, unlock the rest of the path + prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); + global_plan_up_to_inversion_ = global_plan_; + + double max_yaw_delta = 0.0; + + for (size_t i = 1; i < global_plan_.poses.size(); i++) { + double yaw_prev = tf2::getYaw(global_plan_.poses[i-1].pose.orientation); + double yaw_curr = tf2::getYaw(global_plan_.poses[i].pose.orientation); + double diff = fabs(angles::shortest_angular_distance(yaw_prev, yaw_curr)); + if (diff > max_yaw_delta) { + max_yaw_delta = diff; + } + } + + // Recompute locale on the updated path + float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f; + inversion_locale_ = utils::removePosesAfterFirstInversion( + global_plan_up_to_inversion_, rotation_threshold); + + RCLCPP_INFO( + logger_, + "[MPPI PathHandler] setPath: rotation_threshold=%.3f, inversion_locale=%u, path_size=%zu", + rotation_threshold, + inversion_locale_, + global_plan_up_to_inversion_.poses.size()); + + } + } + else{ + // RCLCPP_INFO( + // logger_, + // "[MPPI PathHandler] step: enforce_path_inversion=%s, enforce_path_rotation=%s, inversion_locale=%u", + // enforce_path_inversion_ ? "true" : "false", + // enforce_path_rotation_ ? "true" : "false", + // inversion_locale_); + + } + + if (transformed_plan.poses.empty()) { + throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); + } + + return transformed_plan; +} + +double PathHandler::getMaxCostmapDist() +{ + const auto & costmap = costmap_->getCostmap(); + return static_cast(std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY())) * + costmap->getResolution() * 0.50; +} + +void PathHandler::setPath(const nav_msgs::msg::Path & plan) +{ + global_plan_ = plan; + global_plan_up_to_inversion_ = plan; + if (!enforce_path_inversion_ && !enforce_path_rotation_) { + inversion_locale_ = 0u; + return; + } + + // Find and restrict to the first rotation or inversion constraint + float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f; + inversion_locale_ = utils::removePosesAfterFirstInversion( + global_plan_up_to_inversion_, rotation_threshold); +} + +nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;} + +void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) +{ + plan.poses.erase(plan.poses.begin(), end); +} + +geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal( + const builtin_interfaces::msg::Time & stamp) +{ + auto goal = global_plan_.poses.back(); + goal.header.frame_id = global_plan_.header.frame_id; + goal.header.stamp = stamp; + if (goal.header.frame_id.empty()) { + throw nav2_core::ControllerTFError("Goal pose has an empty frame_id"); + } + geometry_msgs::msg::PoseStamped transformed_goal; + if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *tf_buffer_, + costmap_->getGlobalFrameID(), transform_tolerance_)) + { + throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame"); + } + return transformed_goal; +} + +bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose) +{ + // Keep full path if we are within tolerance of the inversion pose + const auto last_pose = global_plan_up_to_inversion_.poses.back(); + float distance = hypotf( + robot_pose.pose.position.x - last_pose.pose.position.x, + robot_pose.pose.position.y - last_pose.pose.position.y); + + float angle_distance = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), + tf2::getYaw(last_pose.pose.orientation)); + + return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance_; +} + +} // namespace mppi \ No newline at end of file diff --git a/nav2_mppi_controller/src/path_handler copy.cpp b/nav2_mppi_controller/src/path_handler copy.cpp new file mode 100644 index 00000000000..b49e8991306 --- /dev/null +++ b/nav2_mppi_controller/src/path_handler copy.cpp @@ -0,0 +1,247 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// Copyright (c) 2023 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. + +#include "nav2_mppi_controller/tools/path_handler.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/robot_utils.hpp" + +namespace mppi +{ + +void PathHandler::initialize( + nav2::LifecycleNode::WeakPtr parent, const std::string & name, + std::shared_ptr costmap, + std::shared_ptr buffer, ParametersHandler * param_handler) +{ + name_ = name; + costmap_ = costmap; + tf_buffer_ = buffer; + auto node = parent.lock(); + logger_ = node->get_logger(); + parameters_handler_ = param_handler; + + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(max_robot_pose_search_dist_, "max_robot_pose_search_dist", getMaxCostmapDist()); + getParam(prune_distance_, "prune_distance", 1.5); + getParam(transform_tolerance_, "transform_tolerance", 0.1); + getParam(enforce_path_inversion_, "enforce_path_inversion", false); + getParam(enforce_path_rotation_, "enforce_path_rotation", false); + + if (enforce_path_inversion_ || enforce_path_rotation_) { + getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2); + getParam(inversion_yaw_tolerance_, "inversion_yaw_tolerance", 0.4); + getParam(minimum_rotation_angle_, "minimum_rotation_angle", 0.785); + inversion_locale_ = 0u; + } +} + +std::pair +PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( + const geometry_msgs::msg::PoseStamped & global_pose) +{ + using nav2_util::geometry_utils::euclidean_distance; + + auto begin = global_plan_up_to_inversion_.poses.begin(); + + // Limit the search for the closest pose up to max_robot_pose_search_dist on the path + auto closest_pose_upper_bound = + nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), + max_robot_pose_search_dist_); + + // Find closest point to the robot + auto closest_point = nav2_util::geometry_utils::min_by( + begin, closest_pose_upper_bound, + [&global_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(global_pose, ps); + }); + + nav_msgs::msg::Path transformed_plan; + transformed_plan.header.frame_id = costmap_->getGlobalFrameID(); + transformed_plan.header.stamp = global_pose.header.stamp; + + auto pruned_plan_end = + nav2_util::geometry_utils::first_after_integrated_distance( + closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); + + unsigned int mx, my; + // Find the furthest relevant pose on the path to consider within costmap + // bounds + // Transforming it to the costmap frame in the same loop + for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end; + ++global_plan_pose) + { + // Transform from global plan frame to costmap frame + geometry_msgs::msg::PoseStamped costmap_plan_pose; + global_plan_pose->header.stamp = global_pose.header.stamp; + global_plan_pose->header.frame_id = global_plan_.header.frame_id; + nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_buffer_, + costmap_->getGlobalFrameID(), transform_tolerance_); + + // Check if pose is inside the costmap + if (!costmap_->getCostmap()->worldToMap( + costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my)) + { + return {transformed_plan, closest_point}; + } + + // Filling the transformed plan to return with the transformed pose + transformed_plan.poses.push_back(costmap_plan_pose); + } + + return {transformed_plan, closest_point}; +} + +geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame( + const geometry_msgs::msg::PoseStamped & pose) +{ + if (global_plan_up_to_inversion_.poses.empty()) { + throw nav2_core::InvalidPath("Received plan with zero length"); + } + + geometry_msgs::msg::PoseStamped robot_pose; + if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_buffer_, + global_plan_up_to_inversion_.header.frame_id, transform_tolerance_)) + { + throw nav2_core::ControllerTFError( + "Unable to transform robot pose into global plan's frame"); + } + + return robot_pose; +} + +nav_msgs::msg::Path PathHandler::transformPath( + const geometry_msgs::msg::PoseStamped & robot_pose) +{ + // Find relevant bounds of path to use + geometry_msgs::msg::PoseStamped global_pose = + transformToGlobalPlanFrame(robot_pose); + auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose); + + prunePlan(global_plan_up_to_inversion_, lower_bound); + + if ((enforce_path_inversion_ || enforce_path_rotation_) && inversion_locale_ != 0u) { + if (isWithinInversionTolerances(global_pose)) { + // Robot has reached the inversion/rotation point, unlock the rest of the path + prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); + global_plan_up_to_inversion_ = global_plan_; + + double max_yaw_delta = 0.0; + + for (size_t i = 1; i < global_plan_.poses.size(); i++) { + double yaw_prev = tf2::getYaw(global_plan_.poses[i-1].pose.orientation); + double yaw_curr = tf2::getYaw(global_plan_.poses[i].pose.orientation); + double diff = fabs(angles::shortest_angular_distance(yaw_prev, yaw_curr)); + if (diff > max_yaw_delta) { + max_yaw_delta = diff; + } + } + + // Recompute locale on the updated path + float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f; + inversion_locale_ = utils::removePosesAfterFirstInversion( + global_plan_up_to_inversion_, rotation_threshold); + + RCLCPP_INFO( + logger_, + "[MPPI PathHandler] setPath: rotation_threshold=%.3f, inversion_locale=%u, path_size=%zu", + rotation_threshold, + inversion_locale_, + global_plan_up_to_inversion_.poses.size()); + + } + } + else{ + // RCLCPP_INFO( + // logger_, + // "[MPPI PathHandler] step: enforce_path_inversion=%s, enforce_path_rotation=%s, inversion_locale=%u", + // enforce_path_inversion_ ? "true" : "false", + // enforce_path_rotation_ ? "true" : "false", + // inversion_locale_); + + } + + if (transformed_plan.poses.empty()) { + throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); + } + + return transformed_plan; +} + +double PathHandler::getMaxCostmapDist() +{ + const auto & costmap = costmap_->getCostmap(); + return static_cast(std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY())) * + costmap->getResolution() * 0.50; +} + +void PathHandler::setPath(const nav_msgs::msg::Path & plan) +{ + global_plan_ = plan; + global_plan_up_to_inversion_ = plan; + if (!enforce_path_inversion_ && !enforce_path_rotation_) { + inversion_locale_ = 0u; + return; + } + + // Find and restrict to the first rotation or inversion constraint + float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f; + inversion_locale_ = utils::removePosesAfterFirstInversion( + global_plan_up_to_inversion_, rotation_threshold); +} + +nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;} + +void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) +{ + plan.poses.erase(plan.poses.begin(), end); +} + +geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal( + const builtin_interfaces::msg::Time & stamp) +{ + auto goal = global_plan_.poses.back(); + goal.header.frame_id = global_plan_.header.frame_id; + goal.header.stamp = stamp; + if (goal.header.frame_id.empty()) { + throw nav2_core::ControllerTFError("Goal pose has an empty frame_id"); + } + geometry_msgs::msg::PoseStamped transformed_goal; + if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *tf_buffer_, + costmap_->getGlobalFrameID(), transform_tolerance_)) + { + throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame"); + } + return transformed_goal; +} + +bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose) +{ + // Keep full path if we are within tolerance of the inversion pose + const auto last_pose = global_plan_up_to_inversion_.poses.back(); + float distance = hypotf( + robot_pose.pose.position.x - last_pose.pose.position.x, + robot_pose.pose.position.y - last_pose.pose.position.y); + + float angle_distance = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), + tf2::getYaw(last_pose.pose.orientation)); + + return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance_; +} + +} // namespace mppi \ No newline at end of file diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 93593982c5c..b49e8991306 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -17,6 +17,7 @@ #include "nav2_mppi_controller/tools/path_handler.hpp" #include "nav2_mppi_controller/tools/utils.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/robot_utils.hpp" namespace mppi { @@ -38,9 +39,12 @@ void PathHandler::initialize( getParam(prune_distance_, "prune_distance", 1.5); getParam(transform_tolerance_, "transform_tolerance", 0.1); getParam(enforce_path_inversion_, "enforce_path_inversion", false); - if (enforce_path_inversion_) { + getParam(enforce_path_rotation_, "enforce_path_rotation", false); + + if (enforce_path_inversion_ || enforce_path_rotation_) { getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2); - getParam(inversion_yaw_tolerance, "inversion_yaw_tolerance", 0.4); + getParam(inversion_yaw_tolerance_, "inversion_yaw_tolerance", 0.4); + getParam(minimum_rotation_angle_, "minimum_rotation_angle", 0.785); inversion_locale_ = 0u; } } @@ -85,7 +89,8 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( geometry_msgs::msg::PoseStamped costmap_plan_pose; global_plan_pose->header.stamp = global_pose.header.stamp; global_plan_pose->header.frame_id = global_plan_.header.frame_id; - transformPose(costmap_->getGlobalFrameID(), *global_plan_pose, costmap_plan_pose); + nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_buffer_, + costmap_->getGlobalFrameID(), transform_tolerance_); // Check if pose is inside the costmap if (!costmap_->getCostmap()->worldToMap( @@ -109,7 +114,9 @@ geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame( } geometry_msgs::msg::PoseStamped robot_pose; - if (!transformPose(global_plan_up_to_inversion_.header.frame_id, pose, robot_pose)) { + if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_buffer_, + global_plan_up_to_inversion_.header.frame_id, transform_tolerance_)) + { throw nav2_core::ControllerTFError( "Unable to transform robot pose into global plan's frame"); } @@ -127,13 +134,46 @@ nav_msgs::msg::Path PathHandler::transformPath( prunePlan(global_plan_up_to_inversion_, lower_bound); - if (enforce_path_inversion_ && inversion_locale_ != 0u) { + if ((enforce_path_inversion_ || enforce_path_rotation_) && inversion_locale_ != 0u) { if (isWithinInversionTolerances(global_pose)) { + // Robot has reached the inversion/rotation point, unlock the rest of the path prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); global_plan_up_to_inversion_ = global_plan_; - inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + + double max_yaw_delta = 0.0; + + for (size_t i = 1; i < global_plan_.poses.size(); i++) { + double yaw_prev = tf2::getYaw(global_plan_.poses[i-1].pose.orientation); + double yaw_curr = tf2::getYaw(global_plan_.poses[i].pose.orientation); + double diff = fabs(angles::shortest_angular_distance(yaw_prev, yaw_curr)); + if (diff > max_yaw_delta) { + max_yaw_delta = diff; + } + } + + // Recompute locale on the updated path + float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f; + inversion_locale_ = utils::removePosesAfterFirstInversion( + global_plan_up_to_inversion_, rotation_threshold); + + RCLCPP_INFO( + logger_, + "[MPPI PathHandler] setPath: rotation_threshold=%.3f, inversion_locale=%u, path_size=%zu", + rotation_threshold, + inversion_locale_, + global_plan_up_to_inversion_.poses.size()); + } } + else{ + // RCLCPP_INFO( + // logger_, + // "[MPPI PathHandler] step: enforce_path_inversion=%s, enforce_path_rotation=%s, inversion_locale=%u", + // enforce_path_inversion_ ? "true" : "false", + // enforce_path_rotation_ ? "true" : "false", + // inversion_locale_); + + } if (transformed_plan.poses.empty()) { throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); @@ -142,27 +182,6 @@ nav_msgs::msg::Path PathHandler::transformPath( return transformed_plan; } -bool PathHandler::transformPose( - const std::string & frame, const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose) const -{ - if (in_pose.header.frame_id == frame) { - out_pose = in_pose; - return true; - } - - try { - tf_buffer_->transform( - in_pose, out_pose, frame, - tf2::durationFromSec(transform_tolerance_)); - out_pose.header.frame_id = frame; - return true; - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what()); - } - return false; -} - double PathHandler::getMaxCostmapDist() { const auto & costmap = costmap_->getCostmap(); @@ -173,10 +192,16 @@ double PathHandler::getMaxCostmapDist() void PathHandler::setPath(const nav_msgs::msg::Path & plan) { global_plan_ = plan; - global_plan_up_to_inversion_ = global_plan_; - if (enforce_path_inversion_) { - inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + global_plan_up_to_inversion_ = plan; + if (!enforce_path_inversion_ && !enforce_path_rotation_) { + inversion_locale_ = 0u; + return; } + + // Find and restrict to the first rotation or inversion constraint + float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f; + inversion_locale_ = utils::removePosesAfterFirstInversion( + global_plan_up_to_inversion_, rotation_threshold); } nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;} @@ -196,7 +221,9 @@ geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal( throw nav2_core::ControllerTFError("Goal pose has an empty frame_id"); } geometry_msgs::msg::PoseStamped transformed_goal; - if (!transformPose(costmap_->getGlobalFrameID(), goal, transformed_goal)) { + if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *tf_buffer_, + costmap_->getGlobalFrameID(), transform_tolerance_)) + { throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame"); } return transformed_goal; @@ -214,7 +241,7 @@ bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStam tf2::getYaw(robot_pose.pose.orientation), tf2::getYaw(last_pose.pose.orientation)); - return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance; + return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance_; } -} // namespace mppi +} // namespace mppi \ No newline at end of file diff --git a/nav2_mppi_controller/src/path_handler_mod.cpp b/nav2_mppi_controller/src/path_handler_mod.cpp new file mode 100644 index 00000000000..b49e8991306 --- /dev/null +++ b/nav2_mppi_controller/src/path_handler_mod.cpp @@ -0,0 +1,247 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// Copyright (c) 2023 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. + +#include "nav2_mppi_controller/tools/path_handler.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/robot_utils.hpp" + +namespace mppi +{ + +void PathHandler::initialize( + nav2::LifecycleNode::WeakPtr parent, const std::string & name, + std::shared_ptr costmap, + std::shared_ptr buffer, ParametersHandler * param_handler) +{ + name_ = name; + costmap_ = costmap; + tf_buffer_ = buffer; + auto node = parent.lock(); + logger_ = node->get_logger(); + parameters_handler_ = param_handler; + + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(max_robot_pose_search_dist_, "max_robot_pose_search_dist", getMaxCostmapDist()); + getParam(prune_distance_, "prune_distance", 1.5); + getParam(transform_tolerance_, "transform_tolerance", 0.1); + getParam(enforce_path_inversion_, "enforce_path_inversion", false); + getParam(enforce_path_rotation_, "enforce_path_rotation", false); + + if (enforce_path_inversion_ || enforce_path_rotation_) { + getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2); + getParam(inversion_yaw_tolerance_, "inversion_yaw_tolerance", 0.4); + getParam(minimum_rotation_angle_, "minimum_rotation_angle", 0.785); + inversion_locale_ = 0u; + } +} + +std::pair +PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( + const geometry_msgs::msg::PoseStamped & global_pose) +{ + using nav2_util::geometry_utils::euclidean_distance; + + auto begin = global_plan_up_to_inversion_.poses.begin(); + + // Limit the search for the closest pose up to max_robot_pose_search_dist on the path + auto closest_pose_upper_bound = + nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), + max_robot_pose_search_dist_); + + // Find closest point to the robot + auto closest_point = nav2_util::geometry_utils::min_by( + begin, closest_pose_upper_bound, + [&global_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(global_pose, ps); + }); + + nav_msgs::msg::Path transformed_plan; + transformed_plan.header.frame_id = costmap_->getGlobalFrameID(); + transformed_plan.header.stamp = global_pose.header.stamp; + + auto pruned_plan_end = + nav2_util::geometry_utils::first_after_integrated_distance( + closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); + + unsigned int mx, my; + // Find the furthest relevant pose on the path to consider within costmap + // bounds + // Transforming it to the costmap frame in the same loop + for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end; + ++global_plan_pose) + { + // Transform from global plan frame to costmap frame + geometry_msgs::msg::PoseStamped costmap_plan_pose; + global_plan_pose->header.stamp = global_pose.header.stamp; + global_plan_pose->header.frame_id = global_plan_.header.frame_id; + nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_buffer_, + costmap_->getGlobalFrameID(), transform_tolerance_); + + // Check if pose is inside the costmap + if (!costmap_->getCostmap()->worldToMap( + costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my)) + { + return {transformed_plan, closest_point}; + } + + // Filling the transformed plan to return with the transformed pose + transformed_plan.poses.push_back(costmap_plan_pose); + } + + return {transformed_plan, closest_point}; +} + +geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame( + const geometry_msgs::msg::PoseStamped & pose) +{ + if (global_plan_up_to_inversion_.poses.empty()) { + throw nav2_core::InvalidPath("Received plan with zero length"); + } + + geometry_msgs::msg::PoseStamped robot_pose; + if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_buffer_, + global_plan_up_to_inversion_.header.frame_id, transform_tolerance_)) + { + throw nav2_core::ControllerTFError( + "Unable to transform robot pose into global plan's frame"); + } + + return robot_pose; +} + +nav_msgs::msg::Path PathHandler::transformPath( + const geometry_msgs::msg::PoseStamped & robot_pose) +{ + // Find relevant bounds of path to use + geometry_msgs::msg::PoseStamped global_pose = + transformToGlobalPlanFrame(robot_pose); + auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose); + + prunePlan(global_plan_up_to_inversion_, lower_bound); + + if ((enforce_path_inversion_ || enforce_path_rotation_) && inversion_locale_ != 0u) { + if (isWithinInversionTolerances(global_pose)) { + // Robot has reached the inversion/rotation point, unlock the rest of the path + prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); + global_plan_up_to_inversion_ = global_plan_; + + double max_yaw_delta = 0.0; + + for (size_t i = 1; i < global_plan_.poses.size(); i++) { + double yaw_prev = tf2::getYaw(global_plan_.poses[i-1].pose.orientation); + double yaw_curr = tf2::getYaw(global_plan_.poses[i].pose.orientation); + double diff = fabs(angles::shortest_angular_distance(yaw_prev, yaw_curr)); + if (diff > max_yaw_delta) { + max_yaw_delta = diff; + } + } + + // Recompute locale on the updated path + float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f; + inversion_locale_ = utils::removePosesAfterFirstInversion( + global_plan_up_to_inversion_, rotation_threshold); + + RCLCPP_INFO( + logger_, + "[MPPI PathHandler] setPath: rotation_threshold=%.3f, inversion_locale=%u, path_size=%zu", + rotation_threshold, + inversion_locale_, + global_plan_up_to_inversion_.poses.size()); + + } + } + else{ + // RCLCPP_INFO( + // logger_, + // "[MPPI PathHandler] step: enforce_path_inversion=%s, enforce_path_rotation=%s, inversion_locale=%u", + // enforce_path_inversion_ ? "true" : "false", + // enforce_path_rotation_ ? "true" : "false", + // inversion_locale_); + + } + + if (transformed_plan.poses.empty()) { + throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); + } + + return transformed_plan; +} + +double PathHandler::getMaxCostmapDist() +{ + const auto & costmap = costmap_->getCostmap(); + return static_cast(std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY())) * + costmap->getResolution() * 0.50; +} + +void PathHandler::setPath(const nav_msgs::msg::Path & plan) +{ + global_plan_ = plan; + global_plan_up_to_inversion_ = plan; + if (!enforce_path_inversion_ && !enforce_path_rotation_) { + inversion_locale_ = 0u; + return; + } + + // Find and restrict to the first rotation or inversion constraint + float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f; + inversion_locale_ = utils::removePosesAfterFirstInversion( + global_plan_up_to_inversion_, rotation_threshold); +} + +nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;} + +void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) +{ + plan.poses.erase(plan.poses.begin(), end); +} + +geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal( + const builtin_interfaces::msg::Time & stamp) +{ + auto goal = global_plan_.poses.back(); + goal.header.frame_id = global_plan_.header.frame_id; + goal.header.stamp = stamp; + if (goal.header.frame_id.empty()) { + throw nav2_core::ControllerTFError("Goal pose has an empty frame_id"); + } + geometry_msgs::msg::PoseStamped transformed_goal; + if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *tf_buffer_, + costmap_->getGlobalFrameID(), transform_tolerance_)) + { + throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame"); + } + return transformed_goal; +} + +bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose) +{ + // Keep full path if we are within tolerance of the inversion pose + const auto last_pose = global_plan_up_to_inversion_.poses.back(); + float distance = hypotf( + robot_pose.pose.position.x - last_pose.pose.position.x, + robot_pose.pose.position.y - last_pose.pose.position.y); + + float angle_distance = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), + tf2::getYaw(last_pose.pose.orientation)); + + return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance_; +} + +} // namespace mppi \ No newline at end of file diff --git a/nav2_mppi_controller/src/path_handler_original.cpp b/nav2_mppi_controller/src/path_handler_original.cpp new file mode 100644 index 00000000000..55931f6bc62 --- /dev/null +++ b/nav2_mppi_controller/src/path_handler_original.cpp @@ -0,0 +1,205 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// Copyright (c) 2023 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. + +#include "nav2_mppi_controller/tools/path_handler.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/robot_utils.hpp" + +namespace mppi +{ + +void PathHandler::initialize( + nav2::LifecycleNode::WeakPtr parent, const std::string & name, + std::shared_ptr costmap, + std::shared_ptr buffer, ParametersHandler * param_handler) +{ + name_ = name; + costmap_ = costmap; + tf_buffer_ = buffer; + auto node = parent.lock(); + logger_ = node->get_logger(); + parameters_handler_ = param_handler; + + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(max_robot_pose_search_dist_, "max_robot_pose_search_dist", getMaxCostmapDist()); + getParam(prune_distance_, "prune_distance", 1.5); + getParam(transform_tolerance_, "transform_tolerance", 0.1); + getParam(enforce_path_inversion_, "enforce_path_inversion", false); + if (enforce_path_inversion_) { + getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2); + getParam(inversion_yaw_tolerance, "inversion_yaw_tolerance", 0.4); + inversion_locale_ = 0u; + } +} + +std::pair +PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( + const geometry_msgs::msg::PoseStamped & global_pose) +{ + using nav2_util::geometry_utils::euclidean_distance; + + auto begin = global_plan_up_to_inversion_.poses.begin(); + + // Limit the search for the closest pose up to max_robot_pose_search_dist on the path + auto closest_pose_upper_bound = + nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), + max_robot_pose_search_dist_); + + // Find closest point to the robot + auto closest_point = nav2_util::geometry_utils::min_by( + begin, closest_pose_upper_bound, + [&global_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(global_pose, ps); + }); + + nav_msgs::msg::Path transformed_plan; + transformed_plan.header.frame_id = costmap_->getGlobalFrameID(); + transformed_plan.header.stamp = global_pose.header.stamp; + + auto pruned_plan_end = + nav2_util::geometry_utils::first_after_integrated_distance( + closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); + + unsigned int mx, my; + // Find the furthest relevant pose on the path to consider within costmap + // bounds + // Transforming it to the costmap frame in the same loop + for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end; + ++global_plan_pose) + { + // Transform from global plan frame to costmap frame + geometry_msgs::msg::PoseStamped costmap_plan_pose; + global_plan_pose->header.stamp = global_pose.header.stamp; + global_plan_pose->header.frame_id = global_plan_.header.frame_id; + nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_buffer_, + costmap_->getGlobalFrameID(), transform_tolerance_); + + // Check if pose is inside the costmap + if (!costmap_->getCostmap()->worldToMap( + costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my)) + { + return {transformed_plan, closest_point}; + } + + // Filling the transformed plan to return with the transformed pose + transformed_plan.poses.push_back(costmap_plan_pose); + } + + return {transformed_plan, closest_point}; +} + +geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame( + const geometry_msgs::msg::PoseStamped & pose) +{ + if (global_plan_up_to_inversion_.poses.empty()) { + throw nav2_core::InvalidPath("Received plan with zero length"); + } + + geometry_msgs::msg::PoseStamped robot_pose; + if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_buffer_, + global_plan_up_to_inversion_.header.frame_id, transform_tolerance_)) + { + throw nav2_core::ControllerTFError( + "Unable to transform robot pose into global plan's frame"); + } + + return robot_pose; +} + +nav_msgs::msg::Path PathHandler::transformPath( + const geometry_msgs::msg::PoseStamped & robot_pose) +{ + // Find relevant bounds of path to use + geometry_msgs::msg::PoseStamped global_pose = + transformToGlobalPlanFrame(robot_pose); + auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose); + + prunePlan(global_plan_up_to_inversion_, lower_bound); + + if (enforce_path_inversion_ && inversion_locale_ != 0u) { + if (isWithinInversionTolerances(global_pose)) { + prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); + global_plan_up_to_inversion_ = global_plan_; + inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } + } + + if (transformed_plan.poses.empty()) { + throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); + } + + return transformed_plan; +} + +double PathHandler::getMaxCostmapDist() +{ + const auto & costmap = costmap_->getCostmap(); + return static_cast(std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY())) * + costmap->getResolution() * 0.50; +} + +void PathHandler::setPath(const nav_msgs::msg::Path & plan) +{ + global_plan_ = plan; + global_plan_up_to_inversion_ = global_plan_; + if (enforce_path_inversion_) { + inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } +} + +nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;} + +void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) +{ + plan.poses.erase(plan.poses.begin(), end); +} + +geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal( + const builtin_interfaces::msg::Time & stamp) +{ + auto goal = global_plan_.poses.back(); + goal.header.frame_id = global_plan_.header.frame_id; + goal.header.stamp = stamp; + if (goal.header.frame_id.empty()) { + throw nav2_core::ControllerTFError("Goal pose has an empty frame_id"); + } + geometry_msgs::msg::PoseStamped transformed_goal; + if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *tf_buffer_, + costmap_->getGlobalFrameID(), transform_tolerance_)) + { + throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame"); + } + return transformed_goal; +} + +bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose) +{ + // Keep full path if we are within tolerance of the inversion pose + const auto last_pose = global_plan_up_to_inversion_.poses.back(); + float distance = hypotf( + robot_pose.pose.position.x - last_pose.pose.position.x, + robot_pose.pose.position.y - last_pose.pose.position.y); + + float angle_distance = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), + tf2::getYaw(last_pose.pose.orientation)); + + return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance; +} + +} // namespace mppi diff --git a/nav2_mppi_controller/src/trajectory_validators/fixing_trajectory_validator.cpp b/nav2_mppi_controller/src/trajectory_validators/fixing_trajectory_validator.cpp new file mode 100644 index 00000000000..7c17b4e5a3e --- /dev/null +++ b/nav2_mppi_controller/src/trajectory_validators/fixing_trajectory_validator.cpp @@ -0,0 +1,407 @@ +// src/validators/auto_fixing_trajectory_validator.cpp +#include "nav2_mppi_controller/fixing_trajectory_validator.hpp" +#include +#include + +namespace mppi { + +void AutoFixingTrajectoryValidator::initialize( + const nav2::LifecycleNode::WeakPtr & parent, + const std::string & name, + const std::shared_ptr costmap, + ParametersHandler * param_handler, + const std::shared_ptr tf_buffer, + const models::OptimizerSettings & settings) +{ + OptimalTrajectoryValidator::initialize(parent, name, costmap, param_handler, tf_buffer, settings); + auto node = node_.lock(); + auto get = param_handler_->getParamGetter(name_); + + model_dt_ = settings.model_dt; + get(auto_fix_ttc_window_, "auto_fix_ttc_window", 0.8f); + + costmap_ = costmap_ros_->getCostmap(); + + get(consider_footprint_, "consider_footprint", true); + get(safe_clearance_, "auto_fix_safe_clearance", 0.12); + get(step_gain_, "auto_fix_step_gain", 0.6); + get(max_step_, "auto_fix_max_step", 0.05); + get(max_total_disp_, "auto_fix_max_total_displacement", 0.25); + get(max_iters_, "auto_fix_max_iters", 6); + get(smooth_iters_, "auto_fix_smooth_iters", 2); + + // Clamping parametrów względem rozdzielczości costmapy – odporność na złe YAML + const double r = costmap_->getResolution(); + auto clamp = [&](double &v, double lo, double hi) { v = std::clamp(v, lo, hi); }; + clamp(safe_clearance_, 0.5 * r, 1.5 * r); + clamp(max_step_, 0.25 * r, 1.5 * r); + clamp(max_total_disp_, 1.5 * r, 10.0 * r); + step_gain_ = std::clamp(step_gain_, 0.2, 0.9); + + if (consider_footprint_) { + fp_checker_ = std::make_unique>(costmap_); + } + + RCLCPP_INFO(node->get_logger(), + "[%s] AutoFixingTrajectoryValidator initialized: safe=%.2f step_gain=%.2f " + "max_step=%.2f max_disp=%.2f iters=%d smooth=%d lookahead=%.2fs (samples=%u)", + name_.c_str(), safe_clearance_, step_gain_, max_step_, max_total_disp_, + max_iters_, smooth_iters_, collision_lookahead_time_, traj_samples_to_evaluate_); +} + +bool AutoFixingTrajectoryValidator::isCollisionAt(double x, double y, double theta) const +{ + if (consider_footprint_) { + const double c = fp_checker_->footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint()); + // W tym Nav2 checkerze kolizja/OOB ➜ LETHAL (254). Traktuj też INSCRIBED (253) jako nieakceptowalne. + return (c >= static_cast(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)); + } + + unsigned int mx, my; + if (!costmap_->worldToMap(x, y, mx, my)) { + return true; // poza mapą = niebezpieczne + } + const unsigned char c = costmap_->getCost(mx, my); + return (c >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE); +} + +double AutoFixingTrajectoryValidator::clearanceAt(double x, double y) const +{ + // Prosty promieniowy skan (jeśli masz EDT/SDF – podmień) + static const int K = 12; + static const double MAX_R = 0.6; + double best = MAX_R; + for (int k = 0; k < K; ++k) { + const double ang = 2.0 * M_PI * k / K; + const double cs = std::cos(ang), sn = std::sin(ang); + double r = 0.0; + while (r < MAX_R) { + const double px = x + r * cs; + const double py = y + r * sn; + unsigned int mx, my; + if (!costmap_->worldToMap(px, py, mx, my)) break; + const unsigned char c = costmap_->getCost(mx, my); + if (c == nav2_costmap_2d::LETHAL_OBSTACLE || + c == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { + best = std::min(best, r); + break; + } + r += costmap_->getResolution(); + } + } + return best; +} + +bool AutoFixingTrajectoryValidator::isSafe(double x, double y, double theta) const +{ + if (isCollisionAt(x, y, theta)) return false; + return clearanceAt(x, y) >= safe_clearance_; +} + +std::optional AutoFixingTrajectoryValidator::gradientAt(double x, double y) const +{ + unsigned int mx, my; + if (!costmap_->worldToMap(x, y, mx, my)) return std::nullopt; + + auto sample = [&](int dx, int dy) -> double { + const int ix = static_cast(mx) + dx; + const int iy = static_cast(my) + dy; + if (ix < 0 || iy < 0 || + ix >= static_cast(costmap_->getSizeInCellsX()) || + iy >= static_cast(costmap_->getSizeInCellsY())) { + return 0.0; + } + return static_cast(costmap_->getCost(ix, iy)); + }; + + const double cx1 = sample(+1, 0), cx0 = sample(-1, 0); + const double cy1 = sample(0, +1), cy0 = sample(0, -1); + const double dx_cost = (cx1 - cx0) / (2.0 * costmap_->getResolution()); + const double dy_cost = (cy1 - cy0) / (2.0 * costmap_->getResolution()); + + Eigen::Vector2d g(dx_cost, dy_cost); + if (g.norm() < 1e-6) return std::nullopt; + return g; // kierunek wzrostu kosztu ⇒ „odpychamy” w +grad +} + +void AutoFixingTrajectoryValidator::laplacianSmoothXY( + Eigen::ArrayXXf & traj, const std::vector &locked, int iters) const +{ + const int N = traj.rows(); + if (N < 3 || iters <= 0) return; + for (int it = 0; it < iters; ++it) { + for (int i = 1; i < N - 1; ++i) { + if (locked[i]) continue; + const Eigen::Vector2f prev = traj.row(i - 1).segment<2>(0); + const Eigen::Vector2f curr = traj.row(i).segment<2>(0); + const Eigen::Vector2f next = traj.row(i + 1).segment<2>(0); + const Eigen::Vector2f lap = (prev - 2.f * curr + next); + traj(i, 0) += 0.25f * lap.x(); + traj(i, 1) += 0.25f * lap.y(); + } + } +} + +void AutoFixingTrajectoryValidator::smoothYaw(Eigen::ArrayXXf & traj) const +{ + const int N = traj.rows(); + if (N < 3) return; + for (int i = 1; i < N - 1; ++i) { + const float t_prev = traj(i - 1, 2), t_cur = traj(i, 2), t_next = traj(i + 1, 2); + traj(i, 2) = 0.25f * t_prev + 0.5f * t_cur + 0.25f * t_next; + } +} + +bool AutoFixingTrajectoryValidator::tryRepair(Eigen::ArrayXXf & traj, int start_idx, int end_idx) const +{ + const int Ncap = static_cast(std::min(traj.rows(), traj_samples_to_evaluate_)); + const int s = std::clamp(start_idx, 0, Ncap); + const int e = std::clamp(end_idx, 0, Ncap); + if (e - s <= 0) return true; + + std::vector accum(e - s, 0.0); + std::vector locked(e - s, false); + + auto safe_idx = [&](int i) { return isSafe(traj(i, 0), traj(i, 1), traj(i, 2)); }; + + for (int i = s; i < e; ++i) { + locked[i - s] = safe_idx(i); + } + + for (int it = 0; it < max_iters_; ++it) { + bool improved = false; + + for (int i = s; i < e; ++i) { + if (locked[i - s]) continue; + + const double x = traj(i, 0), y = traj(i, 1), th = traj(i, 2); + double clr = clearanceAt(x, y); + + // kierunek: gradient kosztu lub fallback + Eigen::Vector2d dir(0.0, 0.0); + if (auto g = gradientAt(x, y)) { + dir = -(*g); + } else { + const int K = 16; + double best_gain = -1e9; + for (int k = 0; k < K; ++k) { + const double ang = 2.0 * M_PI * k / K; + const Eigen::Vector2d cand(std::cos(ang), std::sin(ang)); + const double nx = x + 0.02 * cand.x(); + const double ny = y + 0.02 * cand.y(); + const double gain = clearanceAt(nx, ny) - clr; + if (gain > best_gain) { best_gain = gain; dir = cand; } + } + } + + const double nrm = dir.norm(); + if (nrm < 1e-6) continue; + dir /= nrm; + + const bool in_collision = isCollisionAt(x, y, th); + double step = 0.0; + if (in_collision) { + // Silniejszy krok startowy, żeby wyjść poza INSCRIBED + const double min_step = std::max({ 0.5 * costmap_->getResolution(), + 0.25 * safe_clearance_, + safe_clearance_ + 0.5 * costmap_->getResolution() }); + step = std::min(std::max(min_step, step_gain_ * (safe_clearance_ + costmap_->getResolution())), + max_step_); + } else { + clr = clearanceAt(x, y); + const double deficit = std::max(0.0, safe_clearance_ - clr); + step = std::min(max_step_, step_gain_ * deficit); + } + double room = std::max(0.0, max_total_disp_ - accum[i - s]); + if (room <= 1e-6) continue; + if (step > room) step = room; + + + // limit przemieszczenia per-punkt (względem zakresu [s,e)) + if (accum[i - s] + step > max_total_disp_) { + step = std::max(0.0, max_total_disp_ - accum[i - s]); + } + if (step <= 1e-6) continue; + + double nx = x + step * dir.x(); + double ny = y + step * dir.y(); + + // krótki line-search: nie wchodź w kolizję + int ls = 0; + while (ls < 4 && isCollisionAt(nx, ny, th)) { + step *= 0.5; nx = x + step * dir.x(); ny = y + step * dir.y(); ++ls; + } + + // jeśli footprint nadal „zbyt blisko”, wysuń delikatnie w górę + if (consider_footprint_) { + int ls_up = 0; + while (ls_up < 4) { + const double c = fp_checker_->footprintCostAtPose(nx, ny, th, costmap_ros_->getRobotFootprint()); + if (c < static_cast(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) break; + double extra = 0.5 * step; + + // nie przekraczaj max_total_disp_ + room = std::max(0.0, max_total_disp_ - accum[i - s] - step); + if (room <= 1e-6) break; + if (extra > room) extra = room; + + nx += extra * dir.x(); + ny += extra * dir.y(); + step += extra; + ++ls_up; + } + } + + if (step <= 1e-6) continue; // nic nie zrobiliśmy + + traj(i, 0) = static_cast(nx); + traj(i, 1) = static_cast(ny); + accum[i - s] += step; + improved = true; + } + + // smoothing z pełnym wektorem locków w przestrzeni [0..rows) + std::vector locked_full(traj.rows(), false); + for (int i = s; i < e; ++i) locked_full[i] = locked[i - s]; + laplacianSmoothXY(traj, locked_full, smooth_iters_); + smoothYaw(traj); + + // zaktualizuj maskę bezpiecznych w zakresie + bool all_safe = true; + for (int i = s; i < e; ++i) { + if (!safe_idx(i)) { all_safe = false; break; } + locked[i - s] = true; + } + if (all_safe) return true; + if (!improved) break; + } + + // częściowo lub wcale się nie udało + return false; +} + +ValidationResult AutoFixingTrajectoryValidator::validateTrajectory( + const Eigen::ArrayXXf & optimal_trajectory, + const models::ControlSequence & control_sequence, + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::Twist & robot_speed, + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal) +{ + (void)control_sequence; + (void)robot_pose; + (void)robot_speed; + (void)plan; + (void)goal; + + auto node = node_.lock(); + RCLCPP_DEBUG_THROTTLE(node->get_logger(), *node->get_clock(), 1000, + "[%s] validateTrajectory called. samples=%u", name_.c_str(), traj_samples_to_evaluate_); + + has_fixed_ = false; + fixed_traj_ = optimal_trajectory; // kopia robocza + + // 1) szybka weryfikacja + int first_coll_idx = -1; + const size_t limit = std::min(traj_samples_to_evaluate_, fixed_traj_.rows()); + for (size_t i = 0; i < limit; ++i) { + const double x = fixed_traj_(i, 0), y = fixed_traj_(i, 1), th = fixed_traj_(i, 2); + if (!isSafe(x, y, th)) { first_coll_idx = static_cast(i); break; } + } + + if (first_coll_idx == 0) { + const double old_disp = max_total_disp_; + const int old_iters = max_iters_; + const double old_step = max_step_; + + max_total_disp_ = std::max(max_total_disp_, 0.8); // było 0.5 + max_iters_ = std::max(max_iters_, 14); // było 10 + max_step_ = std::max(max_step_, 0.08); // ≥ r + + const int window_samples = std::max(1, + static_cast(std::ceil(auto_fix_ttc_window_ / std::max(1e-6f, model_dt_)))); + const bool ok0 = tryRepair(fixed_traj_, 0, std::min(limit, window_samples)); + + // Przywróć + max_total_disp_ = old_disp; + max_iters_ = old_iters; + max_step_ = old_step; + + if (!ok0) { + RCLCPP_WARN_THROTTLE(node->get_logger(), *node->get_clock(), 1000, + "[%s] Collision at sample 0: requesting soft reset for re-sampling.", + name_.c_str()); + return ValidationResult::SOFT_RESET; + } + + // Drugi pass: potwierdź brak kolizji po naprawie TTC=0 na całym oknie + bool clean = true; + for (size_t j = 0; j < limit; ++j) { + if (!isSafe(fixed_traj_(j,0), fixed_traj_(j,1), fixed_traj_(j,2))) { clean = false; break; } + } + if (clean) { + has_fixed_ = true; + RCLCPP_INFO_THROTTLE(node->get_logger(), *node->get_clock(), 1000, + "[%s] Auto-fix SUCCESS at TTC=0: using repaired trajectory.", name_.c_str()); + return ValidationResult::SUCCESS; // ważne: NIE resetuj, pozwól kontrolerowi użyć fixed_traj_ + } + + // Inaczej wpadnij do standardowej ścieżki naprawy dalej w kodzie +} + + + + if (first_coll_idx > 0) { + const double ttc = static_cast(first_coll_idx) * static_cast(model_dt_); + RCLCPP_WARN_THROTTLE( + node->get_logger(), *node->get_clock(), 1000, + "[%s] Collision predicted in %.2f s at sample %d (%.2f, %.2f). Attempting auto-fix...", + name_.c_str(), ttc, first_coll_idx, fixed_traj_(first_coll_idx, 0), fixed_traj_(first_coll_idx, 1)); + + // zakres naprawy: [first - pre, first + window] + const int window_samples = std::max(1, static_cast(std::ceil( + auto_fix_ttc_window_ / std::max(1e-6f, model_dt_)))); + const int pre = std::min(6, first_coll_idx); // ~ 6 próbek wstecz + const int start_idx = first_coll_idx - pre; + const int end_idx = std::min(static_cast(limit), first_coll_idx + window_samples); + + // 2) próba naprawy + const bool ok = tryRepair(fixed_traj_, start_idx, end_idx); + if (!ok) { + RCLCPP_ERROR_THROTTLE( + node->get_logger(), *node->get_clock(), 1000, + "[%s] Auto-fix FAILED: trajectory still in collision after repair attempts.", + name_.c_str()); + return ValidationResult::SOFT_RESET; + } + + // 3) potwierdź brak kolizji po naprawie + for (size_t j = 0; j < limit; ++j) { + if (!isSafe(fixed_traj_(j, 0), fixed_traj_(j, 1), fixed_traj_(j, 2))) { + RCLCPP_ERROR_THROTTLE( + node->get_logger(), *node->get_clock(), 1000, + "[%s] Auto-fix incomplete: remaining collision at sample %zu.", + name_.c_str(), j); + return ValidationResult::SOFT_RESET; + } + } + + has_fixed_ = true; + RCLCPP_INFO_THROTTLE( + node->get_logger(), *node->get_clock(), 1000, + "[%s] Auto-fix SUCCESS: repaired up to %.2f s window; TTC was %.2f s.", + name_.c_str(), (first_coll_idx + window_samples) * model_dt_, ttc); + + return ValidationResult::SUCCESS; + } + + // już było dobrze + return ValidationResult::SUCCESS; +} + +} // namespace mppi + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(mppi::AutoFixingTrajectoryValidator, mppi::OptimalTrajectoryValidator) diff --git a/nav2_mppi_controller/src/trajectory_visualizer copy 2.cpp b/nav2_mppi_controller/src/trajectory_visualizer copy 2.cpp new file mode 100644 index 00000000000..6960b5676c6 --- /dev/null +++ b/nav2_mppi_controller/src/trajectory_visualizer copy 2.cpp @@ -0,0 +1,156 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 "nav2_mppi_controller/tools/trajectory_visualizer.hpp" + +namespace mppi +{ + +void TrajectoryVisualizer::on_configure( + nav2::LifecycleNode::WeakPtr parent, const std::string & name, + const std::string & frame_id, ParametersHandler * parameters_handler) +{ + auto node = parent.lock(); + logger_ = node->get_logger(); + frame_id_ = frame_id; + trajectories_publisher_ = + node->create_publisher("~/candidate_trajectories"); + transformed_path_pub_ = node->create_publisher( + "~/transformed_global_plan"); + optimal_path_pub_ = node->create_publisher("~/optimal_path"); + parameters_handler_ = parameters_handler; + + auto getParam = parameters_handler->getParamGetter(name + ".TrajectoryVisualizer"); + + getParam(trajectory_step_, "trajectory_step", 5); + getParam(time_step_, "time_step", 3); + + reset(); +} + +void TrajectoryVisualizer::on_cleanup() +{ + trajectories_publisher_.reset(); + transformed_path_pub_.reset(); + optimal_path_pub_.reset(); +} + +void TrajectoryVisualizer::on_activate() +{ + trajectories_publisher_->on_activate(); + transformed_path_pub_->on_activate(); + optimal_path_pub_->on_activate(); +} + +void TrajectoryVisualizer::on_deactivate() +{ + trajectories_publisher_->on_deactivate(); + transformed_path_pub_->on_deactivate(); + optimal_path_pub_->on_deactivate(); +} + +void TrajectoryVisualizer::add( + const Eigen::ArrayXXf & trajectory, + const std::string & marker_namespace, + const builtin_interfaces::msg::Time & cmd_stamp) +{ + size_t size = trajectory.rows(); + if (!size) { + return; + } + + auto add_marker = [&](auto i) { + float component = static_cast(i) / static_cast(size); + + auto pose = utils::createPose(trajectory(i, 0), trajectory(i, 1), 0.06); + auto scale = + i != size - 1 ? + utils::createScale(0.03, 0.03, 0.07) : + utils::createScale(0.07, 0.07, 0.09); + auto color = utils::createColor(0, component, component, 1); + auto marker = utils::createMarker( + marker_id_++, pose, scale, color, frame_id_, marker_namespace); + points_->markers.push_back(marker); + + // populate optimal path + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header.frame_id = frame_id_; + pose_stamped.pose = pose; + + tf2::Quaternion quaternion_tf2; + quaternion_tf2.setRPY(0., 0., trajectory(i, 2)); + pose_stamped.pose.orientation = tf2::toMsg(quaternion_tf2); + + optimal_path_->poses.push_back(pose_stamped); + }; + + optimal_path_->header.stamp = cmd_stamp; + optimal_path_->header.frame_id = frame_id_; + for (size_t i = 0; i < size; i++) { + add_marker(i); + } +} + +void TrajectoryVisualizer::add( + const models::Trajectories & trajectories, const std::string & marker_namespace) +{ + size_t n_rows = trajectories.x.rows(); + size_t n_cols = trajectories.x.cols(); + const float shape_1 = static_cast(n_cols); + points_->markers.reserve(floor(n_rows / trajectory_step_) * floor(n_cols * time_step_)); + + for (size_t i = 0; i < n_rows; i += trajectory_step_) { + for (size_t j = 0; j < n_cols; j += time_step_) { + const float j_flt = static_cast(j); + float blue_component = 1.0f - j_flt / shape_1; + float green_component = j_flt / shape_1; + + auto pose = utils::createPose(trajectories.x(i, j), trajectories.y(i, j), 0.03); + auto scale = utils::createScale(0.03, 0.03, 0.03); + auto color = utils::createColor(0, green_component, blue_component, 1); + auto marker = utils::createMarker( + marker_id_++, pose, scale, color, frame_id_, marker_namespace); + + points_->markers.push_back(marker); + } + } +} + +void TrajectoryVisualizer::reset() +{ + marker_id_ = 0; + points_ = std::make_unique(); + optimal_path_ = std::make_unique(); +} + +void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan) +{ + if (trajectories_publisher_->get_subscription_count() > 0) { + trajectories_publisher_->publish(std::move(points_)); + } + + if (optimal_path_pub_->get_subscription_count() > 0) { + optimal_path_pub_->publish(std::move(optimal_path_)); + } + + reset(); + + if (transformed_path_pub_->get_subscription_count() > 0) { + auto plan_ptr = std::make_unique(plan); + transformed_path_pub_->publish(std::move(plan_ptr)); + } +} + +} // namespace mppi diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 6960b5676c6..28141994791 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include +#include +#include #include "nav2_mppi_controller/tools/trajectory_visualizer.hpp" namespace mppi @@ -25,17 +27,38 @@ void TrajectoryVisualizer::on_configure( auto node = parent.lock(); logger_ = node->get_logger(); frame_id_ = frame_id; - trajectories_publisher_ = - node->create_publisher("~/candidate_trajectories"); - transformed_path_pub_ = node->create_publisher( - "~/transformed_global_plan"); - optimal_path_pub_ = node->create_publisher("~/optimal_path"); parameters_handler_ = parameters_handler; - - auto getParam = parameters_handler->getParamGetter(name + ".TrajectoryVisualizer"); - + auto getParam = parameters_handler->getParamGetter(name + ".Visualization"); getParam(trajectory_step_, "trajectory_step", 5); getParam(time_step_, "time_step", 3); + getParam(publish_trajectories_with_total_cost_, "publish_trajectories_with_total_cost", false); + getParam(publish_trajectories_with_individual_cost_, "publish_trajectories_with_individual_cost", + false); + getParam(publish_optimal_footprints_, "publish_optimal_footprints", false); + getParam(publish_optimal_trajectory_msg_, "publish_optimal_trajectory_msg", false); + getParam(publish_transformed_path_, "publish_transformed_path", false); + getParam(publish_optimal_path_, "publish_optimal_path", false); + getParam(footprint_downsample_factor_, "footprint_downsample_factor", 3); + + if (publish_trajectories_with_total_cost_ || publish_trajectories_with_individual_cost_) { + trajectories_publisher_ = + node->create_publisher("~/candidate_trajectories"); + } + if (publish_transformed_path_) { + transformed_path_pub_ = node->create_publisher( + "~/transformed_global_plan"); + } + if (publish_optimal_path_) { + optimal_path_pub_ = node->create_publisher("~/optimal_path"); + } + if (publish_optimal_footprints_) { + optimal_footprints_pub_ = node->create_publisher( + "~/optimal_footprints"); + } + if (publish_optimal_trajectory_msg_) { + optimal_trajectory_msg_pub_ = node->create_publisher( + "~/optimal_trajectory"); + } reset(); } @@ -45,20 +68,46 @@ void TrajectoryVisualizer::on_cleanup() trajectories_publisher_.reset(); transformed_path_pub_.reset(); optimal_path_pub_.reset(); + optimal_footprints_pub_.reset(); + optimal_trajectory_msg_pub_.reset(); } void TrajectoryVisualizer::on_activate() { - trajectories_publisher_->on_activate(); - transformed_path_pub_->on_activate(); - optimal_path_pub_->on_activate(); + if (trajectories_publisher_) { + trajectories_publisher_->on_activate(); + } + if (transformed_path_pub_) { + transformed_path_pub_->on_activate(); + } + if (optimal_path_pub_) { + optimal_path_pub_->on_activate(); + } + if (optimal_footprints_pub_) { + optimal_footprints_pub_->on_activate(); + } + if (optimal_trajectory_msg_pub_) { + optimal_trajectory_msg_pub_->on_activate(); + } } void TrajectoryVisualizer::on_deactivate() { - trajectories_publisher_->on_deactivate(); - transformed_path_pub_->on_deactivate(); - optimal_path_pub_->on_deactivate(); + if (trajectories_publisher_) { + trajectories_publisher_->on_deactivate(); + } + if (transformed_path_pub_) { + transformed_path_pub_->on_deactivate(); + } + if (optimal_path_pub_) { + optimal_path_pub_->on_deactivate(); + } + if (optimal_footprints_pub_) { + optimal_footprints_pub_->on_deactivate(); + } + if (optimal_trajectory_msg_pub_) { + optimal_trajectory_msg_pub_->on_deactivate(); + } } void TrajectoryVisualizer::add( @@ -104,27 +153,132 @@ void TrajectoryVisualizer::add( } void TrajectoryVisualizer::add( - const models::Trajectories & trajectories, const std::string & marker_namespace) + const models::Trajectories & trajectories, + const Eigen::ArrayXf & total_costs, + const std::vector> & individual_critics_cost, + const builtin_interfaces::msg::Time & cmd_stamp, + const std::optional> & trajectories_in_collision) +{ + // Check if we should visualize per-critic costs + bool visualize_per_critic = !individual_critics_cost.empty() && + publish_trajectories_with_individual_cost_ && + trajectories_publisher_ && trajectories_publisher_->get_subscription_count() > 0; + + size_t n_rows = trajectories.x.rows(); + points_->markers.reserve(n_rows / trajectory_step_); + + // Visualize total costs if requested + if (publish_trajectories_with_total_cost_) { + createTrajectoryMarkers(trajectories, total_costs, "Total Costs", cmd_stamp, trajectories_in_collision); + } + + // Visualize each critic's contribution if requested + if (visualize_per_critic) { + for (const auto & [critic_name, costs] : individual_critics_cost) { + createTrajectoryMarkers(trajectories, costs, critic_name, cmd_stamp, trajectories_in_collision); + } + } +} + +void TrajectoryVisualizer::createTrajectoryMarkers( + const models::Trajectories & trajectories, + const Eigen::ArrayXf & costs, + const std::string & ns, + const builtin_interfaces::msg::Time & cmd_stamp, + const std::optional> & trajectories_in_collision) { size_t n_rows = trajectories.x.rows(); size_t n_cols = trajectories.x.cols(); - const float shape_1 = static_cast(n_cols); - points_->markers.reserve(floor(n_rows / trajectory_step_) * floor(n_cols * time_step_)); + + // Find min/max cost for normalization, excluding trajectories in collision + float min_cost = std::numeric_limits::max(); + float max_cost = std::numeric_limits::lowest(); + + for (Eigen::Index i = 0; i < costs.size(); ++i) { + // Skip trajectories in collision when computing min/max for normalization + bool in_collision = trajectories_in_collision && + i < static_cast(trajectories_in_collision->size()) && + (*trajectories_in_collision)[i]; + + if (!in_collision) { + min_cost = std::min(min_cost, costs(i)); + max_cost = std::max(max_cost, costs(i)); + } + } + + float cost_range = max_cost - min_cost; + + // Avoid division by zero + if (cost_range < 1e-6f) { + cost_range = 1.0f; + } for (size_t i = 0; i < n_rows; i += trajectory_step_) { - for (size_t j = 0; j < n_cols; j += time_step_) { - const float j_flt = static_cast(j); - float blue_component = 1.0f - j_flt / shape_1; - float green_component = j_flt / shape_1; + float red_component, green_component, blue_component; - auto pose = utils::createPose(trajectories.x(i, j), trajectories.y(i, j), 0.03); - auto scale = utils::createScale(0.03, 0.03, 0.03); - auto color = utils::createColor(0, green_component, blue_component, 1); - auto marker = utils::createMarker( - marker_id_++, pose, scale, color, frame_id_, marker_namespace); + // Check if trajectory is in collision + bool in_collision = trajectories_in_collision && + i < trajectories_in_collision->size() && + (*trajectories_in_collision)[i]; - points_->markers.push_back(marker); + if (in_collision) { + // Fuschia color for trajectories in collision (magenta/pink) + red_component = 1.0f; + green_component = 0.0f; + blue_component = 1.0f; + } else { + // Check if cost is zero (no contribution from this critic) + bool zero_cost = std::abs(costs(i)) < 1e-6f; + + if (zero_cost) { + // Gray color for zero cost (no contribution) + red_component = 0.5f; + green_component = 0.5f; + blue_component = 0.5f; + } else { + // Normal gradient for trajectories + float normalized_cost = (costs(i) - min_cost) / cost_range; + normalized_cost = std::clamp(normalized_cost, 0.0f, 1.0f); + + // Color scheme: Green (low cost) -> Yellow -> Red (high cost) + blue_component = 0.0f; + if (normalized_cost < 0.5f) { + // Green to Yellow (0.0 - 0.5) + red_component = normalized_cost * 2.0f; + green_component = 1.0f; + } else { + // Yellow to Red (0.5 - 1.0) + red_component = 1.0f; + green_component = 2.0f * (1.0f - normalized_cost); + } + } } + + // Create line strip marker for this trajectory + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame_id_; + marker.header.stamp = cmd_stamp; + marker.ns = ns; + marker.id = marker_id_++; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.01; // Line width + marker.color.r = red_component; + marker.color.g = green_component; + marker.color.b = blue_component; + marker.color.a = 1.0; + + // Add all points in this trajectory to the line strip + for (size_t j = 0; j < n_cols; j += time_step_) { + geometry_msgs::msg::Point point; + point.x = trajectories.x(i, j); + point.y = trajectories.y(i, j); + point.z = 0.0f; + marker.points.push_back(point); + } + + points_->markers.push_back(marker); } } @@ -135,22 +289,168 @@ void TrajectoryVisualizer::reset() optimal_path_ = std::make_unique(); } -void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan) +void TrajectoryVisualizer::visualize( + nav_msgs::msg::Path plan, + const Eigen::ArrayXXf & optimal_trajectory, + const models::ControlSequence & control_sequence, + float model_dt, + const builtin_interfaces::msg::Time & stamp, + std::shared_ptr costmap_ros, + const models::Trajectories & candidate_trajectories, + const Eigen::ArrayXf & costs, + const std::vector> & critic_costs, + const std::optional> & trajectories_in_collision) { - if (trajectories_publisher_->get_subscription_count() > 0) { + // Create header with frame from costmap + std_msgs::msg::Header header; + header.stamp = stamp; + header.frame_id = costmap_ros->getGlobalFrameID(); + + // Visualize trajectories with total costs + if (publish_trajectories_with_total_cost_ || + (!publish_trajectories_with_individual_cost_ || critic_costs.empty())) + { + add(candidate_trajectories, costs, {}, stamp, trajectories_in_collision); + } + + // Visualize trajectories with individual critic costs + if (publish_trajectories_with_individual_cost_ && !critic_costs.empty()) { + add(candidate_trajectories, costs, critic_costs, stamp, trajectories_in_collision); + } + + // Add optimal trajectory to populate optimal_path_ + if (publish_optimal_path_ && optimal_trajectory.rows() > 0) { + add(optimal_trajectory, "Optimal Trajectory", stamp); + } + + // Publish trajectories + if (trajectories_publisher_ && trajectories_publisher_->get_subscription_count() > 0) { trajectories_publisher_->publish(std::move(points_)); } - if (optimal_path_pub_->get_subscription_count() > 0) { + // Publish optimal path if enabled + if (publish_optimal_path_ && optimal_path_pub_ && + optimal_path_pub_->get_subscription_count() > 0) + { optimal_path_pub_->publish(std::move(optimal_path_)); } + // Publish optimal footprints if enabled + if (publish_optimal_footprints_ && optimal_footprints_pub_ && + optimal_footprints_pub_->get_subscription_count() > 0 && + costmap_ros != nullptr && optimal_trajectory.rows() > 0) + { + auto footprints_msg = createFootprintMarkers(optimal_trajectory, header, costmap_ros); + optimal_footprints_pub_->publish(std::move(footprints_msg)); + } + + // Publish optimal trajectory message if enabled + if (publish_optimal_trajectory_msg_ && optimal_trajectory_msg_pub_ && + optimal_trajectory_msg_pub_->get_subscription_count() > 0) + { + auto trajectory_msg = utils::toTrajectoryMsg( + optimal_trajectory, + control_sequence, + model_dt, + header); + optimal_trajectory_msg_pub_->publish(std::move(trajectory_msg)); + } + reset(); - if (transformed_path_pub_->get_subscription_count() > 0) { + // Publish transformed path + if (transformed_path_pub_ && transformed_path_pub_->get_subscription_count() > 0) { auto plan_ptr = std::make_unique(plan); transformed_path_pub_->publish(std::move(plan_ptr)); } } -} // namespace mppi +void TrajectoryVisualizer::visualize(nav_msgs::msg::Path plan) +{ + // Simplified version for testing that only publishes what's been added + if (trajectories_publisher_ && trajectories_publisher_->get_subscription_count() > 0) { + trajectories_publisher_->publish(std::move(points_)); + } + + if (publish_optimal_path_ && optimal_path_pub_ && + optimal_path_pub_->get_subscription_count() > 0) + { + optimal_path_pub_->publish(std::move(optimal_path_)); + } + + reset(); + + if (transformed_path_pub_ && transformed_path_pub_->get_subscription_count() > 0) { + auto plan_ptr = std::make_unique(plan); + transformed_path_pub_->publish(std::move(plan_ptr)); + } +} + +visualization_msgs::msg::MarkerArray TrajectoryVisualizer::createFootprintMarkers( + const Eigen::ArrayXXf & trajectory, + const std_msgs::msg::Header & header, + std::shared_ptr costmap_ros) +{ + visualization_msgs::msg::MarkerArray marker_array; + if (trajectory.rows() == 0) { + return marker_array; + } + + // Get robot footprint + std::vector robot_footprint = + costmap_ros->getRobotFootprint(); + + // Skip if footprint is empty or very small + if (robot_footprint.size() < 3) { + return marker_array; + } + + // Create header for markers + std_msgs::msg::Header costmap_header; + costmap_header.frame_id = costmap_ros->getGlobalFrameID(); + costmap_header.stamp = header.stamp; + + int marker_id = 0; + for (int i = 0; i < trajectory.rows(); i += footprint_downsample_factor_) { + double x = trajectory(i, 0); + double y = trajectory(i, 1); + double theta = trajectory(i, 2); + + // Create oriented footprint + geometry_msgs::msg::PolygonStamped oriented_footprint; + oriented_footprint.header = costmap_header; + nav2_costmap_2d::transformFootprint(x, y, theta, robot_footprint, oriented_footprint); + // Create marker for this footprint + visualization_msgs::msg::Marker marker; + marker.header = costmap_header; + marker.ns = "optimal_footprints"; + marker.id = marker_id++; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation.w = 1.0; + // Set marker scale and color + marker.scale.x = 0.02; // Line width + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + marker.color.a = 0.8; + // Add footprint points to marker + for (const auto & point : oriented_footprint.polygon.points) { + geometry_msgs::msg::Point p; + p.x = point.x; + p.y = point.y; + p.z = 0.0; + marker.points.push_back(p); + } + // Close the polygon by adding the first point again + if (!marker.points.empty()) { + marker.points.push_back(marker.points[0]); + } + + marker_array.markers.push_back(marker); + } + + return marker_array; +} + +} // namespace mppi \ No newline at end of file diff --git a/nav2_mppi_controller/test/critic_manager_test copy 2.cpp b/nav2_mppi_controller/test/critic_manager_test copy 2.cpp new file mode 100644 index 00000000000..ff0116ed7de --- /dev/null +++ b/nav2_mppi_controller/test/critic_manager_test copy 2.cpp @@ -0,0 +1,160 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_mppi_controller/critic_manager.hpp" + +// Tests critic manager + +using namespace mppi; // NOLINT +using namespace mppi::critics; // NOLINT + +class DummyCritic : public CriticFunction +{ +public: + virtual void initialize() {initialized_ = true;} + virtual void score(CriticData & /*data*/) {scored_ = true;} + bool initialized_{false}, scored_{false}; +}; + +class CriticManagerWrapper : public CriticManager +{ +public: + CriticManagerWrapper() + : CriticManager() {} + + virtual ~CriticManagerWrapper() = default; + + virtual void loadCritics() + { + critics_.clear(); + auto instance = std::unique_ptr(new DummyCritic); + critics_.push_back(std::move(instance)); + critics_.back()->on_configure( + parent_, name_, name_ + "." + "DummyCritic", costmap_ros_, + parameters_handler_); + } + + std::string getFullNameWrapper(const std::string & name) + { + return getFullName(name); + } + + bool getDummyCriticInitialized() + { + const auto critic = critics_[0].get(); + if (critic == nullptr) { + return false; + } + const auto dummy_critic = dynamic_cast(critic); + return dummy_critic == nullptr ? false : dummy_critic->initialized_; + } + + bool getDummyCriticScored() + { + const auto critic = critics_[0].get(); + if (critic == nullptr) { + return false; + } + const auto dummy_critic = dynamic_cast(critic); + return dummy_critic == nullptr ? false : dummy_critic->scored_; + } +}; + +class CriticManagerWrapperEnum : public CriticManager +{ +public: + CriticManagerWrapperEnum() + : CriticManager() {} + + virtual ~CriticManagerWrapperEnum() = default; + + unsigned int getCriticNum() + { + return critics_.size(); + } +}; + +TEST(CriticManagerTests, BasicCriticOperations) +{ + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + ParametersHandler param_handler(node, name); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + // Configuration should get parameters and initialize critic functions + CriticManagerWrapper critic_manager; + critic_manager.on_configure(node, "critic_manager", costmap_ros, ¶m_handler); + EXPECT_TRUE(critic_manager.getDummyCriticInitialized()); + + // Evaluation of critics should score them, but only if failure flag is not set + models::State state; + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + models::Path path; + geometry_msgs::msg::Pose goal; + Eigen::ArrayXf costs; + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + std::nullopt, std::nullopt}; + + data.fail_flag = true; + EXPECT_FALSE(critic_manager.getDummyCriticScored()); + data.fail_flag = false; + critic_manager.evalTrajectoriesScores(data); + EXPECT_TRUE(critic_manager.getDummyCriticScored()); + + // This should get the full namespaced name of the critics + EXPECT_EQ(critic_manager.getFullNameWrapper("name"), std::string("mppi::critics::name")); +} + +TEST(CriticManagerTests, CriticLoadingTest) +{ + auto node = std::make_shared("my_node"); + node->declare_parameter( + "critic_manager.critics", + rclcpp::ParameterValue(std::vector{"ConstraintCritic", "PreferForwardCritic"})); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + ParametersHandler param_handler(node, name); + rclcpp_lifecycle::State state; + costmap_ros->on_configure(state); + + // This should grab the critics parameter and load the 2 requested plugins + CriticManagerWrapperEnum critic_manager; + critic_manager.on_configure(node, "critic_manager", costmap_ros, ¶m_handler); + EXPECT_EQ(critic_manager.getCriticNum(), 2u); +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + rclcpp::init(0, nullptr); + + int result = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + + return result; +} diff --git a/nav2_mppi_controller/test/critic_manager_test.cpp b/nav2_mppi_controller/test/critic_manager_test.cpp index ff0116ed7de..f29b0ac2d41 100644 --- a/nav2_mppi_controller/test/critic_manager_test.cpp +++ b/nav2_mppi_controller/test/critic_manager_test.cpp @@ -114,7 +114,8 @@ TEST(CriticManagerTests, BasicCriticOperations) Eigen::ArrayXf costs; float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, std::nullopt, + nullptr, nullptr, std::nullopt, std::nullopt}; data.fail_flag = true; @@ -157,4 +158,4 @@ int main(int argc, char **argv) rclcpp::shutdown(); return result; -} +} \ No newline at end of file diff --git a/nav2_mppi_controller/test/critics_tests copy 2.cpp b/nav2_mppi_controller/test/critics_tests copy 2.cpp new file mode 100644 index 00000000000..88e823019a7 --- /dev/null +++ b/nav2_mppi_controller/test/critics_tests copy 2.cpp @@ -0,0 +1,794 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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. + +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" +#include "nav2_mppi_controller/motion_models.hpp" +#include "nav2_mppi_controller/critics/constraint_critic.hpp" +#include "nav2_mppi_controller/critics/goal_angle_critic.hpp" +#include "nav2_mppi_controller/critics/goal_critic.hpp" +#include "nav2_mppi_controller/critics/obstacles_critic.hpp" +#include "nav2_mppi_controller/critics/cost_critic.hpp" +#include "nav2_mppi_controller/critics/path_align_critic.hpp" +#include "nav2_mppi_controller/critics/path_angle_critic.hpp" +#include "nav2_mppi_controller/critics/path_follow_critic.hpp" +#include "nav2_mppi_controller/critics/prefer_forward_critic.hpp" +#include "nav2_mppi_controller/critics/twirling_critic.hpp" +#include "nav2_mppi_controller/critics/velocity_deadband_critic.hpp" +#include "utils_test.cpp" // NOLINT + +// Tests the various critic plugin functions + +// ROS lock used from utils_test.cpp + +using namespace mppi; // NOLINT +using namespace mppi::critics; // NOLINT +using namespace mppi::utils; // NOLINT + +class PathAngleCriticWrapper : public PathAngleCritic +{ +public: + PathAngleCriticWrapper() + : PathAngleCritic() + { + } + + void setMode(int mode) + { + mode_ = static_cast(mode); + } +}; + +TEST(CriticTests, ConstraintsCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + ParametersHandler param_handler(node, name); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + // provide velocities in constraints, should not have any costs + state.vx = 0.40 * Eigen::ArrayXXf::Ones(1000, 30); + state.vy = Eigen::ArrayXXf::Zero(1000, 30); + state.wz = Eigen::ArrayXXf::Ones(1000, 30); + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + models::Path path; + geometry_msgs::msg::Pose goal; + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; + data.motion_model = std::make_shared(); + + // Initialization testing + + // Make sure initializes correctly and that defaults are reasonable + ConstraintCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + EXPECT_TRUE(critic.getMaxVelConstraint() > 0.0); + EXPECT_TRUE(critic.getMinVelConstraint() < 0.0); + + // Scoring testing + critic.score(data); + EXPECT_NEAR(costs.sum(), 0, 1e-6); + + // provide out of maximum velocity constraint + state.vx.row(999).setConstant(0.60f); + critic.score(data); + EXPECT_GT(costs.sum(), 0); + // 4.0 weight * 0.1 model_dt * 0.1 error introduced * 30 timesteps = 1.2 + EXPECT_NEAR(costs(999), 1.2, 0.01); + costs.setZero(); + + // provide out of minimum velocity constraint + state.vx.row(1).setConstant(-0.45f); + critic.score(data); + EXPECT_GT(costs.sum(), 0); + // 4.0 weight * 0.1 model_dt * 0.1 error introduced * 30 timesteps = 1.2 + EXPECT_NEAR(costs(1), 1.2, 0.01); + costs.setZero(); + + // Now with ackermann, all in constraint so no costs to score + state.vx.setConstant(0.40f); + state.wz.setConstant(1.5f); + data.motion_model = std::make_shared(¶m_handler, node->get_name()); + critic.score(data); + EXPECT_NEAR(costs.sum(), 0, 1e-6); + + // Now violating the ackermann constraints + state.wz.setConstant(2.5f); + critic.score(data); + EXPECT_GT(costs.sum(), 0); + // 4.0 weight * 0.1 model_dt * (0.2 - 0.4/2.5) * 30 timesteps = 0.48 + EXPECT_NEAR(costs(1), 0.48, 0.01); +} + +TEST(CriticTests, ObstacleCriticMisalignedParams) { + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + ParametersHandler param_handler(node, name); + auto getParam = param_handler.getParamGetter("critic"); + bool consider_footprint; + getParam(consider_footprint, "consider_footprint", true); + + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + ObstaclesCritic critic; + // Expect throw when settings mismatched + EXPECT_THROW( + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler), + nav2_core::ControllerException + ); +} + +TEST(CriticTests, ObstacleCriticAlignedParams) { + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + ParametersHandler param_handler(node, name); + auto getParam = param_handler.getParamGetter("critic"); + bool consider_footprint; + getParam(consider_footprint, "consider_footprint", false); + + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + ObstaclesCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); +} + + +TEST(CriticTests, CostCriticMisAlignedParams) { + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + ParametersHandler param_handler(node, name); + rclcpp_lifecycle::State lstate; + auto getParam = param_handler.getParamGetter("critic"); + bool consider_footprint; + getParam(consider_footprint, "consider_footprint", true); + costmap_ros->on_configure(lstate); + + CostCritic critic; + // Expect throw when settings mismatched + EXPECT_THROW( + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler), + nav2_core::ControllerException + ); +} + +TEST(CriticTests, CostCriticAlignedParams) { + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + ParametersHandler param_handler(node, name); + rclcpp_lifecycle::State lstate; + auto getParam = param_handler.getParamGetter("critic"); + bool consider_footprint; + getParam(consider_footprint, "consider_footprint", false); + costmap_ros->on_configure(lstate); + + CostCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); +} + +TEST(CriticTests, GoalAngleCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + ParametersHandler param_handler(node, name); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + geometry_msgs::msg::Pose goal; + path.reset(10); + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; + data.motion_model = std::make_shared(); + + // Initialization testing + + // Make sure initializes correctly + GoalAngleCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide state poses and path too far from `threshold_to_consider` to consider + state.pose.pose.position.x = 1.0; + path.x(9) = 10.0; + path.y(9) = 0.0; + path.yaws(9) = 3.14; + goal.position.x = 10.0; + goal.position.y = 0.0; + goal.orientation.x = 0.0; + goal.orientation.y = 0.0; + goal.orientation.z = 1.0; + goal.orientation.w = 0.0; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + critic.score(data); + EXPECT_NEAR(costs.sum(), 0, 1e-6); + + // Let's move it even closer, just to be sure it still doesn't trigger + state.pose.pose.position.x = 9.2; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + critic.score(data); + EXPECT_NEAR(costs.sum(), 0, 1e-6); + + // provide state pose and path below `threshold_to_consider` to consider + state.pose.pose.position.x = 9.7; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + critic.score(data); + EXPECT_GT(costs.sum(), 0); + EXPECT_NEAR(costs(0), 9.42, 0.02); // (3.14 - 0.0) * 3.0 weight +} + +TEST(CriticTests, GoalCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + ParametersHandler param_handler(node, name); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + geometry_msgs::msg::Pose goal; + path.reset(10); + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; + data.motion_model = std::make_shared(); + + // Initialization testing + + // Make sure initializes correctly + GoalCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing with all trajectories set to 0 + + // provide state poses and path far, should not trigger + state.pose.pose.position.x = 1.0; + path.x(9) = 10.0; + path.y(9) = 0.0; + goal.position.x = 10.0; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + critic.score(data); + EXPECT_NEAR(costs(2), 0.0, 1e-6); // (0 * 5.0 weight + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // Should all be 0 * 1000 + costs.setZero(); + + // provide state pose and path close + path.x(9) = 0.5; + path.y(9) = 0.0; + goal.position.x = 0.5; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + critic.score(data); + EXPECT_NEAR(costs(2), 2.5, 1e-6); // (sqrt(10.0 * 10.0) * 5.0 weight + EXPECT_NEAR(costs.sum(), 2500.0, 1e-3); // should be 2.5 * 1000 +} + +TEST(CriticTests, PathAngleCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + ParametersHandler param_handler(node, name); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + state.reset(1000, 30); + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + geometry_msgs::msg::Pose goal; + path.reset(10); + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; + data.motion_model = std::make_shared(); + TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally + + // Initialization testing + + // Make sure initializes correctly + PathAngleCriticWrapper critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide state poses and path close, within pose tolerance so won't do anything + state.pose.pose.position.x = 0.0; + state.pose.pose.position.y = 0.0; + path.x(9) = 0.15; + goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + + // provide state pose and path close but outside of tol. with less than PI/2 angular diff. + path.x(9) = 0.95; + goal.position.x = 0.95; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + data.furthest_reached_path_point = 2; // So it grabs the 2 + offset_from_furthest_ = 6th point + path.x(6) = 1.0; // angle between path point and pose = 0 < max_angle_to_furthest_ + path.y(6) = 0.0; + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = -1.0; // angle between path point and pose > max_angle_to_furthest_ + path.y(6) = 4.0; + critic.score(data); + EXPECT_GT(costs.sum(), 0.0); + EXPECT_NEAR(costs(0), 3.9947, 1e-2); // atan2(4,-1) [1.81] * 2.2 weight + + // Set mode to no directional preferences + reset costs + critic.setMode(1); + costs.setZero(); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = 1.0; // angle between path point and pose < max_angle_to_furthest_ + path.y(6) = 0.0; + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = -1.0; // angle between path pt and pose < max_angle_to_furthest_ IF non-directional + path.y(6) = 0.0; + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = -1.0; // angle between path point and pose < max_angle_to_furthest_ + path.y(6) = 4.0; + critic.score(data); + EXPECT_GT(costs.sum(), 0.0); + // should use reverse orientation as the closer angle in no dir preference mode + EXPECT_NEAR(costs(0), 2.9167, 1e-2); + + // Set mode to consider path directionality + reset costs + critic.setMode(2); + costs.setZero(); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = 1.0; // angle between path point and pose < max_angle_to_furthest_ + path.y(6) = 0.0; + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = -1.0; // angle between path pt and pose < max_angle_to_furthest_ IF non-directional + path.y(6) = 0.0; + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = -1.0; // angle between path point and pose < max_angle_to_furthest_ + path.y(6) = 4.0; + critic.score(data); + EXPECT_GT(costs.sum(), 0.0); + // should use reverse orientation as the closer angle in no dir preference mode + EXPECT_NEAR(costs(0), 2.9167, 1e-2); + + PathAngleMode mode; + mode = PathAngleMode::FORWARD_PREFERENCE; + EXPECT_EQ(modeToStr(mode), std::string("Forward Preference")); + mode = PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS; + EXPECT_EQ(modeToStr(mode), std::string("Consider Feasible Path Orientations")); + mode = PathAngleMode::NO_DIRECTIONAL_PREFERENCE; + EXPECT_EQ(modeToStr(mode), std::string("No Directional Preference")); + mode = static_cast(4); + EXPECT_EQ(modeToStr(mode), std::string("Invalid mode!")); +} + +TEST(CriticTests, PreferForwardCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + ParametersHandler param_handler(node, name); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + state.reset(1000, 30); + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + geometry_msgs::msg::Pose goal; + path.reset(10); + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; + data.motion_model = std::make_shared(); + TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally + + // Initialization testing + + // Make sure initializes correctly + PreferForwardCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide state poses and path far away, not within positional tolerances + state.pose.pose.position.x = 1.0; + path.x(9) = 10.0; + goal.position.x = 10.0; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f); + + // provide state pose and path close to trigger behavior but with all forward motion + path.x(9) = 0.15; + goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + state.vx.setOnes(); + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f); + + // provide state pose and path close to trigger behavior but with all reverse motion + state.vx.setConstant(-1.0f); + critic.score(data); + EXPECT_GT(costs.sum(), 0.0f); + EXPECT_NEAR(costs(0), 15.0f, 1e-3f); // 1.0 * 0.1 model_dt * 5.0 weight * 30 length +} + +TEST(CriticTests, TwirlingCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + ParametersHandler param_handler(node, name); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + state.reset(1000, 30); + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + geometry_msgs::msg::Pose goal; + path.reset(10); + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; + data.motion_model = std::make_shared(); + TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally + data.goal_checker = &goal_checker; + + // Initialization testing + + // Make sure initializes correctly + TwirlingCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide state poses and path far away, not within positional tolerances + state.pose.pose.position.x = 1.0; + path.x(9) = 10.0; + goal.position.x = 10.0; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + + // provide state pose and path close to trigger behavior but with no angular variation + path.x(9) = 0.15; + goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + state.wz.setZero(); + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + + // Provide nearby with some motion + state.wz.row(0).setConstant(10.0f); + critic.score(data); + EXPECT_NEAR(costs(0), 100.0, 1e-6); // (mean(10.0) * 10.0 weight + costs.setZero(); + + // Now try again with some wiggling noise + std::mt19937 engine; + std::normal_distribution normal_dist = std::normal_distribution(0.0f, 0.5f); + state.wz.row(0) = Eigen::ArrayXf::NullaryExpr(30, [&] () {return normal_dist(engine);}); + critic.score(data); + EXPECT_NEAR(costs(0), 2.581, 4e-1); // (mean of noise with mu=0, sigma=0.5 * 10.0 weight +} + +TEST(CriticTests, PathFollowCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + ParametersHandler param_handler(node, name); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + state.reset(1000, 30); + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + geometry_msgs::msg::Pose goal; + path.reset(6); + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; + data.motion_model = std::make_shared(); + TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally + data.goal_checker = &goal_checker; + + // Initialization testing + + // Make sure initializes correctly + PathFollowCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide state poses and goal close within positional tolerances + state.pose.pose.position.x = 2.0; + path.x(5) = 1.8; + goal.position.x = 1.8; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + + // provide state pose and path far enough to enable + // pose differential is (0, 0) and (0.15, 0) + path.x(5) = 0.15; + goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + critic.score(data); + EXPECT_NEAR(costs.sum(), 750.0, 1e-2); // 0.15 * 5 weight * 1000 +} + +TEST(CriticTests, PathAlignCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + ParametersHandler param_handler(node, name); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + state.reset(1000, 30); + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + geometry_msgs::msg::Pose goal; + path.reset(10); + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; + data.motion_model = std::make_shared(); + TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally + data.goal_checker = &goal_checker; + + // Initialization testing + + // Make sure initializes correctly + PathAlignCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide state poses and path close within positional tolerances + state.pose.pose.position.x = 1.0; + path.x(9) = 0.85; + goal.position.x = 0.85; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + + // provide state pose and path far enough to enable + // but data furthest point reached is 0 and offset default is 20, so returns + path.x(9) = 0.15; + goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + + // provide state pose and path far enough to enable, with data to pass condition + // but with empty trajectories and paths, should still be zero + *data.furthest_reached_path_point = 21; + path.x(9) = 0.15; + goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); + + // provide state pose and path far enough to enable, with data to pass condition + // and with a valid path to pass invalid path condition + state.pose.pose.position.x = 0.0; + data.path_pts_valid.reset(); // Recompute on new path + path.reset(22); + path.x(0) = 0; + path.x(1) = 0.1; + path.x(2) = 0.2; + path.x(3) = 0.3; + path.x(4) = 0.4; + path.x(5) = 0.5; + path.x(6) = 0.6; + path.x(7) = 0.7; + path.x(8) = 0.8; + path.x(9) = 0.9; + path.x(10) = 0.9; + path.x(11) = 0.9; + path.x(12) = 0.9; + path.x(13) = 0.9; + path.x(14) = 0.9; + path.x(15) = 0.9; + path.x(16) = 0.9; + path.x(17) = 0.9; + path.x(18) = 0.9; + path.x(19) = 0.9; + path.x(20) = 0.9; + path.x(21) = 0.9; + goal.position.x = 0.9; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + generated_trajectories.x.setConstant(0.66f); + critic.score(data); + // 0.66 * 1000 * 10 weight * 6 num pts eval / 6 normalization term + EXPECT_NEAR(costs.sum(), 6600.0, 1e-2); + + // provide state pose and path far enough to enable, with data to pass condition + // but path is blocked in collision + auto * costmap = costmap_ros->getCostmap(); + // island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution + for (unsigned int i = 11; i <= 30; ++i) { // 1.1m-3m + for (unsigned int j = 11; j <= 30; ++j) { // 1.1m-3m + costmap->setCost(i, j, 254); + } + } + + data.path_pts_valid.reset(); // Recompute on new path + costs.setZero(); + path.x.setConstant(1.5f); + path.y.setConstant(1.5f); + goal.position.x = 1.5; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); + critic.score(data); + EXPECT_NEAR(costs.sum(), 0.0, 1e-6); +} + +TEST(CriticTests, VelocityDeadbandCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + std::string name = "test"; + ParametersHandler param_handler(node, name); + auto getParam = param_handler.getParamGetter("critic"); + std::vector deadband_velocities_; + getParam(deadband_velocities_, "deadband_velocities", std::vector{0.08, 0.08, 0.08}); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + state.reset(1000, 30); + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + models::Path path; + geometry_msgs::msg::Pose goal; + Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; + data.motion_model = std::make_shared(); + + // Initialization testing + + // Make sure initializes correctly and that defaults are reasonable + VelocityDeadbandCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide velocities out of deadband bounds, should not have any costs + state.vx.setConstant(0.80f); + state.vy.setConstant(0.60f); + state.wz.setConstant(0.80f); + critic.score(data); + EXPECT_NEAR(costs.sum(), 0, 1e-6); + + // Test cost value + state.vx.setConstant(0.01f); + state.vy.setConstant(0.02f); + state.wz.setConstant(0.021f); + critic.score(data); + // 35.0 weight * 0.1 model_dt * (0.07 + 0.06 + 0.059) * 30 timesteps = 56.7 + EXPECT_NEAR(costs(1), 19.845, 0.01); +} diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 813cba14318..7c54114b24b 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -79,8 +79,8 @@ TEST(CriticTests, ConstraintsCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, - false, nullptr, nullptr, std::nullopt, std::nullopt}; + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, + false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); // Initialization testing @@ -227,8 +227,8 @@ TEST(CriticTests, GoalAngleCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, - false, nullptr, nullptr, std::nullopt, std::nullopt}; + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, + false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); // Initialization testing @@ -251,16 +251,19 @@ TEST(CriticTests, GoalAngleCritic) goal.orientation.y = 0.0; goal.orientation.z = 1.0; goal.orientation.w = 0.0; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0, 1e-6); // Let's move it even closer, just to be sure it still doesn't trigger state.pose.pose.position.x = 9.2; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0, 1e-6); // provide state pose and path below `threshold_to_consider` to consider state.pose.pose.position.x = 9.7; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_GT(costs.sum(), 0); EXPECT_NEAR(costs(0), 9.42, 0.02); // (3.14 - 0.0) * 3.0 weight @@ -287,8 +290,8 @@ TEST(CriticTests, GoalCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, - false, nullptr, nullptr, std::nullopt, std::nullopt}; + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, + false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); // Initialization testing @@ -305,6 +308,7 @@ TEST(CriticTests, GoalCritic) path.x(9) = 10.0; path.y(9) = 0.0; goal.position.x = 10.0; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs(2), 0.0, 1e-6); // (0 * 5.0 weight EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // Should all be 0 * 1000 @@ -314,6 +318,7 @@ TEST(CriticTests, GoalCritic) path.x(9) = 0.5; path.y(9) = 0.0; goal.position.x = 0.5; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs(2), 2.5, 1e-6); // (sqrt(10.0 * 10.0) * 5.0 weight EXPECT_NEAR(costs.sum(), 2500.0, 1e-3); // should be 2.5 * 1000 @@ -341,8 +346,8 @@ TEST(CriticTests, PathAngleCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, - false, nullptr, nullptr, std::nullopt, std::nullopt}; + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, + false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally @@ -360,12 +365,14 @@ TEST(CriticTests, PathAngleCritic) state.pose.pose.position.y = 0.0; path.x(9) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path close but outside of tol. with less than PI/2 angular diff. path.x(9) = 0.95; goal.position.x = 0.95; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); data.furthest_reached_path_point = 2; // So it grabs the 2 + offset_from_furthest_ = 6th point path.x(6) = 1.0; // angle between path point and pose = 0 < max_angle_to_furthest_ path.y(6) = 0.0; @@ -460,8 +467,8 @@ TEST(CriticTests, PreferForwardCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, - false, nullptr, nullptr, std::nullopt, std::nullopt}; + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, + false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally @@ -478,12 +485,14 @@ TEST(CriticTests, PreferForwardCritic) state.pose.pose.position.x = 1.0; path.x(9) = 10.0; goal.position.x = 10.0; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f); // provide state pose and path close to trigger behavior but with all forward motion path.x(9) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); state.vx.setOnes(); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f); @@ -517,8 +526,8 @@ TEST(CriticTests, TwirlingCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, - false, nullptr, nullptr, std::nullopt, std::nullopt}; + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, + false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally data.goal_checker = &goal_checker; @@ -536,12 +545,14 @@ TEST(CriticTests, TwirlingCritic) state.pose.pose.position.x = 1.0; path.x(9) = 10.0; goal.position.x = 10.0; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path close to trigger behavior but with no angular variation path.x(9) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); state.wz.setZero(); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -582,8 +593,8 @@ TEST(CriticTests, PathFollowCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, - false, nullptr, nullptr, std::nullopt, std::nullopt}; + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, + false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally data.goal_checker = &goal_checker; @@ -601,6 +612,7 @@ TEST(CriticTests, PathFollowCritic) state.pose.pose.position.x = 2.0; path.x(5) = 1.8; goal.position.x = 1.8; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -608,6 +620,7 @@ TEST(CriticTests, PathFollowCritic) // pose differential is (0, 0) and (0.15, 0) path.x(5) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 750.0, 1e-2); // 0.15 * 5 weight * 1000 } @@ -634,8 +647,8 @@ TEST(CriticTests, PathAlignCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, - false, nullptr, nullptr, std::nullopt, std::nullopt}; + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, + false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally data.goal_checker = &goal_checker; @@ -653,6 +666,7 @@ TEST(CriticTests, PathAlignCritic) state.pose.pose.position.x = 1.0; path.x(9) = 0.85; goal.position.x = 0.85; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -660,6 +674,7 @@ TEST(CriticTests, PathAlignCritic) // but data furthest point reached is 0 and offset default is 20, so returns path.x(9) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -668,6 +683,7 @@ TEST(CriticTests, PathAlignCritic) *data.furthest_reached_path_point = 21; path.x(9) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -699,6 +715,7 @@ TEST(CriticTests, PathAlignCritic) path.x(20) = 0.9; path.x(21) = 0.9; goal.position.x = 0.9; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); generated_trajectories.x.setConstant(0.66f); critic.score(data); // 0.66 * 1000 * 10 weight * 6 num pts eval / 6 normalization term @@ -719,6 +736,7 @@ TEST(CriticTests, PathAlignCritic) path.x.setConstant(1.5f); path.y.setConstant(1.5f); goal.position.x = 1.5; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); } @@ -746,8 +764,8 @@ TEST(CriticTests, VelocityDeadbandCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, - false, nullptr, nullptr, std::nullopt, std::nullopt}; + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, + false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); // Initialization testing @@ -773,4 +791,4 @@ TEST(CriticTests, VelocityDeadbandCritic) critic.score(data); // 35.0 weight * 0.1 model_dt * (0.07 + 0.06 + 0.059) * 30 timesteps = 56.7 EXPECT_NEAR(costs(1), 19.845, 0.01); -} +} \ No newline at end of file diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index d201e20eeb8..c88422f3a84 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -21,7 +21,8 @@ #include #include #include -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_mppi_controller/optimizer.hpp" #include "nav2_mppi_controller/tools/parameters_handler.hpp" @@ -79,7 +80,8 @@ TEST_P(OptimizerSuite, OptimizerTest) { auto goal = path.poses.back().pose; nav2_core::GoalChecker * dummy_goal_checker{nullptr}; - auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, goal, dummy_goal_checker); + auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, goal, + dummy_goal_checker); EXPECT_GT(trajectory.rows(), 0); EXPECT_GT(trajectory.cols(), 0); } diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index 510d9ef300b..eec9ea0ec57 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -18,7 +18,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "nav2_mppi_controller/optimizer.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" // Tests main optimizer functions diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp index 375978e98da..e0f17e48411 100644 --- a/nav2_mppi_controller/test/path_handler_test.cpp +++ b/nav2_mppi_controller/test/path_handler_test.cpp @@ -18,7 +18,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "nav2_mppi_controller/tools/path_handler.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" // Tests path handling @@ -46,13 +46,6 @@ class PathHandlerWrapper : public PathHandler return getGlobalPlanConsideringBoundsInCostmapFrame(pose); } - bool transformPoseWrapper( - const std::string & frame, const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose) const - { - return transformPose(frame, in_pose, out_pose); - } - geometry_msgs::msg::PoseStamped transformToGlobalPlanFrameWrapper( const geometry_msgs::msg::PoseStamped & pose) { @@ -180,12 +173,10 @@ TEST(PathHandlerTests, TestTransforms) path.poses[i].header.frame_id = "map"; } - geometry_msgs::msg::PoseStamped robot_pose, output_pose; + geometry_msgs::msg::PoseStamped robot_pose; robot_pose.header.frame_id = "odom"; robot_pose.pose.position.x = 2.5; - EXPECT_TRUE(handler.transformPoseWrapper("map", robot_pose, output_pose)); - EXPECT_EQ(output_pose.pose.position.x, 2.5); EXPECT_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose), std::runtime_error); handler.setPath(path); diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests copy 2.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests copy 2.cpp new file mode 100644 index 00000000000..7e710fc7375 --- /dev/null +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests copy 2.cpp @@ -0,0 +1,229 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp" + +// Tests trajectory visualization + +using namespace mppi; // NOLINT + +TEST(TrajectoryVisualizerTests, StateTransition) +{ + auto node = std::make_shared("my_node"); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); + + TrajectoryVisualizer vis; + vis.on_configure(node, "my_name", "map", parameters_handler.get()); + vis.on_activate(); + vis.on_deactivate(); + vis.on_cleanup(); +} + +TEST(TrajectoryVisualizerTests, VisPathRepub) +{ + auto node = std::make_shared("my_node"); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); + nav_msgs::msg::Path received_path; + nav_msgs::msg::Path pub_path; + pub_path.header.frame_id = "fake_frame"; + pub_path.poses.resize(5); + + auto my_sub = node->create_subscription( + "~/transformed_global_plan", + [&](const nav_msgs::msg::Path msg) {received_path = msg;}); + + TrajectoryVisualizer vis; + vis.on_configure(node, "my_name", "map", parameters_handler.get()); + vis.on_activate(); + vis.visualize(pub_path); + + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_EQ(received_path.poses.size(), 5u); + EXPECT_EQ(received_path.header.frame_id, "fake_frame"); +} + +TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) +{ + auto node = std::make_shared("my_node"); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); + + visualization_msgs::msg::MarkerArray received_msg; + auto my_sub = node->create_subscription( + "~/candidate_trajectories", + [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;}); + + // optimal_trajectory empty, should fail to publish + Eigen::ArrayXXf optimal_trajectory; + TrajectoryVisualizer vis; + vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); + vis.on_activate(); + builtin_interfaces::msg::Time bogus_stamp; + vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp); + nav_msgs::msg::Path bogus_path; + vis.visualize(bogus_path); + + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_EQ(received_msg.markers.size(), 0u); + + // Now populated with content, should publish + optimal_trajectory = Eigen::ArrayXXf::Ones(20, 3); + vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp); + vis.visualize(bogus_path); + + rclcpp::spin_some(node->get_node_base_interface()); + + // Should have 20 trajectory points in the map frame + EXPECT_EQ(received_msg.markers.size(), 20u); + EXPECT_EQ(received_msg.markers[0].header.frame_id, "fkmap"); + + // Check IDs are properly populated + EXPECT_EQ(received_msg.markers[0].id, 0); + EXPECT_EQ(received_msg.markers[1].id, 1); + EXPECT_EQ(received_msg.markers[10].id, 10); + + // Check poses are correct + EXPECT_EQ(received_msg.markers[0].pose.position.x, 1); + EXPECT_EQ(received_msg.markers[0].pose.position.y, 1); + EXPECT_EQ(received_msg.markers[0].pose.position.z, 0.06); + + // Check that scales are rational + EXPECT_EQ(received_msg.markers[0].scale.x, 0.03); + EXPECT_EQ(received_msg.markers[0].scale.y, 0.03); + EXPECT_EQ(received_msg.markers[0].scale.z, 0.07); + + EXPECT_EQ(received_msg.markers[19].scale.x, 0.07); + EXPECT_EQ(received_msg.markers[19].scale.y, 0.07); + EXPECT_EQ(received_msg.markers[19].scale.z, 0.09); + + // Check that the colors are rational + for (unsigned int i = 0; i != received_msg.markers.size() - 1; i++) { + EXPECT_LT(received_msg.markers[i].color.g, received_msg.markers[i + 1].color.g); + EXPECT_LT(received_msg.markers[i].color.b, received_msg.markers[i + 1].color.b); + EXPECT_EQ(received_msg.markers[i].color.r, received_msg.markers[i + 1].color.r); + EXPECT_EQ(received_msg.markers[i].color.a, received_msg.markers[i + 1].color.a); + } +} + +TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) +{ + auto node = std::make_shared("my_node"); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); + + visualization_msgs::msg::MarkerArray received_msg; + auto my_sub = node->create_subscription( + "~/candidate_trajectories", + [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;}); + + models::Trajectories candidate_trajectories; + candidate_trajectories.x = Eigen::ArrayXXf::Ones(200, 12); + candidate_trajectories.y = Eigen::ArrayXXf::Ones(200, 12); + candidate_trajectories.yaws = Eigen::ArrayXXf::Ones(200, 12); + + TrajectoryVisualizer vis; + vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); + vis.on_activate(); + vis.add(candidate_trajectories, "Candidate Trajectories"); + nav_msgs::msg::Path bogus_path; + vis.visualize(bogus_path); + + rclcpp::spin_some(node->get_node_base_interface()); + // 40 * 4, for 5 trajectory steps + 3 point steps + EXPECT_EQ(received_msg.markers.size(), 160u); +} + +TEST(TrajectoryVisualizerTests, VisOptimalPath) +{ + auto node = std::make_shared("my_node"); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); + builtin_interfaces::msg::Time cmd_stamp; + cmd_stamp.sec = 5; + cmd_stamp.nanosec = 10; + + nav_msgs::msg::Path received_path; + auto my_sub = node->create_subscription( + "~/optimal_path", + [&](const nav_msgs::msg::Path msg) {received_path = msg;}); + + // optimal_trajectory empty, should fail to publish + Eigen::ArrayXXf optimal_trajectory; + TrajectoryVisualizer vis; + vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); + vis.on_activate(); + vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); + nav_msgs::msg::Path bogus_path; + vis.visualize(bogus_path); + + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_EQ(received_path.poses.size(), 0u); + + // Now populated with content, should publish + optimal_trajectory.resize(20, 3); + for (unsigned int i = 0; i != optimal_trajectory.rows() - 1; i++) { + optimal_trajectory(i, 0) = static_cast(i); + optimal_trajectory(i, 1) = static_cast(i); + optimal_trajectory(i, 2) = static_cast(i); + } + vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); + vis.visualize(bogus_path); + + rclcpp::spin_some(node->get_node_base_interface()); + + // Should have a 20 points path in the map frame and with same stamp than velocity command + EXPECT_EQ(received_path.poses.size(), 20u); + EXPECT_EQ(received_path.header.frame_id, "fkmap"); + EXPECT_EQ(received_path.header.stamp.sec, cmd_stamp.sec); + EXPECT_EQ(received_path.header.stamp.nanosec, cmd_stamp.nanosec); + + tf2::Quaternion quat; + for (unsigned int i = 0; i != received_path.poses.size() - 1; i++) { + // Poses should be in map frame too + EXPECT_EQ(received_path.poses[i].header.frame_id, "fkmap"); + + // Check positions are correct + EXPECT_EQ(received_path.poses[i].pose.position.x, static_cast(i)); + EXPECT_EQ(received_path.poses[i].pose.position.y, static_cast(i)); + EXPECT_EQ(received_path.poses[i].pose.position.z, 0.06); + + // Check orientations are correct + quat.setRPY(0., 0., optimal_trajectory(i, 2)); + auto expected_orientation = tf2::toMsg(quat); + EXPECT_EQ(received_path.poses[i].pose.orientation.x, expected_orientation.x); + EXPECT_EQ(received_path.poses[i].pose.orientation.y, expected_orientation.y); + EXPECT_EQ(received_path.poses[i].pose.orientation.z, expected_orientation.z); + EXPECT_EQ(received_path.poses[i].pose.orientation.w, expected_orientation.w); + } +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + rclcpp::init(0, nullptr); + + int result = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + + return result; +} diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index 7e710fc7375..64aefddd874 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -50,12 +50,15 @@ TEST(TrajectoryVisualizerTests, VisPathRepub) "~/transformed_global_plan", [&](const nav_msgs::msg::Path msg) {received_path = msg;}); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "map", parameters_handler.get()); vis.on_activate(); vis.visualize(pub_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_EQ(received_path.poses.size(), 5u); EXPECT_EQ(received_path.header.frame_id, "fake_frame"); } @@ -70,6 +73,8 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) auto my_sub = node->create_subscription( "~/candidate_trajectories", [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;}); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // optimal_trajectory empty, should fail to publish Eigen::ArrayXXf optimal_trajectory; @@ -81,7 +86,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) nav_msgs::msg::Path bogus_path; vis.visualize(bogus_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_EQ(received_msg.markers.size(), 0u); // Now populated with content, should publish @@ -89,7 +94,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp); vis.visualize(bogus_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Should have 20 trajectory points in the map frame EXPECT_EQ(received_msg.markers.size(), 20u); @@ -128,25 +133,33 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) auto node = std::make_shared("my_node"); std::string name = "test"; auto parameters_handler = std::make_unique(node, name); + builtin_interfaces::msg::Time cmd_stamp; + cmd_stamp.sec = 5; + cmd_stamp.nanosec = 10; visualization_msgs::msg::MarkerArray received_msg; auto my_sub = node->create_subscription( "~/candidate_trajectories", [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;}); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + models::Trajectories candidate_trajectories; candidate_trajectories.x = Eigen::ArrayXXf::Ones(200, 12); candidate_trajectories.y = Eigen::ArrayXXf::Ones(200, 12); candidate_trajectories.yaws = Eigen::ArrayXXf::Ones(200, 12); + Eigen::ArrayXf costs = Eigen::ArrayXf::Random(200); + TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); vis.on_activate(); - vis.add(candidate_trajectories, "Candidate Trajectories"); + vis.add(candidate_trajectories, costs, {}, cmd_stamp); nav_msgs::msg::Path bogus_path; vis.visualize(bogus_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // 40 * 4, for 5 trajectory steps + 3 point steps EXPECT_EQ(received_msg.markers.size(), 160u); } @@ -165,6 +178,9 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) "~/optimal_path", [&](const nav_msgs::msg::Path msg) {received_path = msg;}); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + // optimal_trajectory empty, should fail to publish Eigen::ArrayXXf optimal_trajectory; TrajectoryVisualizer vis; @@ -174,7 +190,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) nav_msgs::msg::Path bogus_path; vis.visualize(bogus_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_EQ(received_path.poses.size(), 0u); // Now populated with content, should publish @@ -187,7 +203,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); vis.visualize(bogus_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Should have a 20 points path in the map frame and with same stamp than velocity command EXPECT_EQ(received_path.poses.size(), 20u); @@ -226,4 +242,4 @@ int main(int argc, char **argv) rclcpp::shutdown(); return result; -} +} \ No newline at end of file diff --git a/nav2_mppi_controller/test/utils/utils.hpp b/nav2_mppi_controller/test/utils/utils.hpp index b0185ce7cfe..2772ea9a858 100644 --- a/nav2_mppi_controller/test/utils/utils.hpp +++ b/nav2_mppi_controller/test/utils/utils.hpp @@ -22,7 +22,7 @@ #include -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" diff --git a/nav2_mppi_controller/test/utils_test copy 2.cpp b/nav2_mppi_controller/test/utils_test copy 2.cpp new file mode 100644 index 00000000000..70733e37db4 --- /dev/null +++ b/nav2_mppi_controller/test/utils_test copy 2.cpp @@ -0,0 +1,614 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" +#include "nav2_mppi_controller/models/path.hpp" + +// Tests noise generator object + +using namespace mppi::utils; // NOLINT +using namespace mppi; // NOLINT + +class TestGoalChecker : public nav2_core::GoalChecker +{ +public: + TestGoalChecker() {} + + virtual void initialize( + const nav2::LifecycleNode::WeakPtr & /*parent*/, + const std::string & /*plugin_name*/, + const std::shared_ptr/*costmap_ros*/) {} + + virtual void reset() {} + + virtual bool isGoalReached( + const geometry_msgs::msg::Pose & /*query_pose*/, + const geometry_msgs::msg::Pose & /*goal_pose*/, + const geometry_msgs::msg::Twist & /*velocity*/) {return false;} + + virtual bool getTolerances( + geometry_msgs::msg::Pose & pose_tolerance, + geometry_msgs::msg::Twist & /*vel_tolerance*/) + { + pose_tolerance.position.x = 0.25; + pose_tolerance.position.y = 0.25; + return true; + } +}; + +TEST(UtilsTests, MarkerPopulationUtils) +{ + auto pose = createPose(1.0, 2.0, 3.0); + EXPECT_EQ(pose.position.x, 1.0); + EXPECT_EQ(pose.position.y, 2.0); + EXPECT_EQ(pose.position.z, 3.0); + EXPECT_EQ(pose.orientation.w, 1.0); + + auto scale = createScale(1.0, 2.0, 3.0); + EXPECT_EQ(scale.x, 1.0); + EXPECT_EQ(scale.y, 2.0); + EXPECT_EQ(scale.z, 3.0); + + auto color = createColor(1.0, 2.0, 3.0, 0.0); + EXPECT_EQ(color.r, 1.0); + EXPECT_EQ(color.g, 2.0); + EXPECT_EQ(color.b, 3.0); + EXPECT_EQ(color.a, 0.0); + + auto marker = createMarker(999, pose, scale, color, "map", "ns"); + EXPECT_EQ(marker.header.frame_id, "map"); + EXPECT_EQ(marker.id, 999); + EXPECT_EQ(marker.pose, pose); + EXPECT_EQ(marker.scale, scale); + EXPECT_EQ(marker.color, color); + EXPECT_EQ(marker.ns, "ns"); +} + +TEST(UtilsTests, ConversionTests) +{ + geometry_msgs::msg::TwistStamped output; + builtin_interfaces::msg::Time time; + + // Check population is correct + output = toTwistStamped(0.5, 0.3, time, "map"); + EXPECT_NEAR(output.twist.linear.x, 0.5, 1e-6); + EXPECT_NEAR(output.twist.linear.y, 0.0, 1e-6); + EXPECT_NEAR(output.twist.angular.z, 0.3, 1e-6); + EXPECT_EQ(output.header.frame_id, "map"); + EXPECT_EQ(output.header.stamp, time); + + output = toTwistStamped(0.5, 0.4, 0.3, time, "map"); + EXPECT_NEAR(output.twist.linear.x, 0.5, 1e-6); + EXPECT_NEAR(output.twist.linear.y, 0.4, 1e-6); + EXPECT_NEAR(output.twist.angular.z, 0.3, 1e-6); + EXPECT_EQ(output.header.frame_id, "map"); + EXPECT_EQ(output.header.stamp, time); + + nav_msgs::msg::Path path; + path.poses.resize(5); + path.poses[2].pose.position.x = 5; + path.poses[2].pose.position.y = 50; + models::Path path_t = toTensor(path); + + // Check population is correct + EXPECT_EQ(path_t.x.rows(), 5u); + EXPECT_EQ(path_t.y.rows(), 5u); + EXPECT_EQ(path_t.yaws.rows(), 5u); + EXPECT_EQ(path_t.x(2), 5); + EXPECT_EQ(path_t.y(2), 50); + EXPECT_NEAR(path_t.yaws(2), 0.0, 1e-6); +} + +TEST(UtilsTests, AnglesTests) +{ + // Test angle normalization by creating insane angles + Eigen::ArrayXf angles(100); + angles.setConstant(1.0f); + + for (unsigned int i = 0; i != angles.size(); i++) { + angles(i) = i * i; + if (i % 2 == 0) { + angles(i) *= -1; + } + } + + auto norm_ang = normalize_angles(angles).eval(); + for (unsigned int i = 0; i != norm_ang.size(); i++) { + EXPECT_TRUE((norm_ang(i) >= -M_PIF) && (norm_ang(i) <= M_PIF)); + } + + // Test shortest angular distance + Eigen::ArrayXf zero_angles(100); + zero_angles.setZero(); + auto ang_dist = shortest_angular_distance(angles, zero_angles).eval(); + for (unsigned int i = 0; i != ang_dist.size(); i++) { + EXPECT_TRUE((ang_dist(i) >= -M_PIF) && (ang_dist(i) <= M_PIF)); + } + + // Test point-pose angle + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + pose.orientation.w = 1.0; + double point_x = 1.0, point_y = 0.0; + bool forward_preference = true; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6); + forward_preference = false; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6); + point_x = -1.0; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6); + forward_preference = true; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), M_PI, 1e-6); + + // Test point-pose angle with goal yaws + point_x = 1.0; + double point_yaw = 0.0; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, point_yaw), 0.0, 1e-6); + point_yaw = M_PI; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, point_yaw), M_PI, 1e-6); + point_yaw = 0.1; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, point_yaw), 0.0, 1e-3); + point_yaw = 3.04159; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, point_yaw), M_PI, 1e-3); +} + +TEST(UtilsTests, FurthestAndClosestReachedPoint) +{ + models::State state; + models::Trajectories generated_trajectories; + generated_trajectories.reset(100, 2); + models::Path path; + geometry_msgs::msg::Pose goal; + path.reset(10); + Eigen::ArrayXf costs; + float model_dt = 0.1; + + CriticData data = + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + std::nullopt, std::nullopt}; /// Caution, keep references + + // Attempt to set furthest point if notionally set, should not change + data.furthest_reached_path_point = 99999; + setPathFurthestPointIfNotSet(data); + EXPECT_EQ(data.furthest_reached_path_point, 99999); + + // Attempt to set if not set already with no other information, should fail + CriticData data2 = + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + std::nullopt, std::nullopt}; /// Caution, keep references + setPathFurthestPointIfNotSet(data2); + EXPECT_EQ(data2.furthest_reached_path_point, 0); + + // Test the actual computation of the path point reached + generated_trajectories.x = Eigen::ArrayXXf::Ones(100, 2); + generated_trajectories.y = Eigen::ArrayXXf::Zero(100, 2); + generated_trajectories.yaws = Eigen::ArrayXXf::Zero(100, 2); + + nav_msgs::msg::Path plan; + plan.poses.resize(10); + for (unsigned int i = 0; i != plan.poses.size(); i++) { + plan.poses[i].pose.position.x = 0.2 * i; + plan.poses[i].pose.position.y = 0.0; + } + path = toTensor(plan); + + CriticData data3 = + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + std::nullopt, std::nullopt}; /// Caution, keep references + EXPECT_EQ(findPathFurthestReachedPoint(data3), 5); +} + +TEST(UtilsTests, findPathCosts) +{ + models::State state; + models::Trajectories generated_trajectories; + models::Path path; + geometry_msgs::msg::Pose goal; + path.reset(50); + Eigen::ArrayXf costs; + float model_dt = 0.1; + + CriticData data = + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + std::nullopt, std::nullopt}; /// Caution, keep references + + // Test not set if already set, should not change + data.path_pts_valid = std::vector(10, false); + for (unsigned int i = 0; i != 10; i++) { + (*data.path_pts_valid)[i] = false; + } + EXPECT_TRUE(data.path_pts_valid); + setPathCostsIfNotSet(data, nullptr); + EXPECT_EQ(data.path_pts_valid->size(), 10u); + + CriticData data3 = + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + std::nullopt, std::nullopt}; /// Caution, keep references + + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + auto * costmap = costmap_ros->getCostmap(); + // island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution + for (unsigned int i = 10; i <= 30; ++i) { // 1m-3m + for (unsigned int j = 10; j <= 30; ++j) { // 1m-3m + costmap->setCost(i, j, 254); + } + } + for (unsigned int i = 40; i <= 45; ++i) { // 4m-4.5m + for (unsigned int j = 45; j <= 45; ++j) { // 4m-4.5m + costmap->setCost(i, j, 253); + } + } + + path.x(1) = 999999999; // OFF COSTMAP + path.y(1) = 999999999; + path.x(10) = 1.5; // IN LETHAL + path.y(10) = 1.5; + path.x(20) = 4.2; // IN INFLATED + path.y(20) = 4.2; + + // This should be evaluated and have real outputs now + setPathCostsIfNotSet(data3, costmap_ros); + EXPECT_TRUE(data3.path_pts_valid.has_value()); + for (unsigned int i = 0; i != path.x.size() - 1; i++) { + if (i == 1 || i == 10) { + EXPECT_FALSE((*data3.path_pts_valid)[i]); + } else { + EXPECT_TRUE((*data3.path_pts_valid)[i]); + } + } +} + +TEST(UtilsTests, SmootherTest) +{ + models::ControlSequence noisey_sequence, sequence_init; + noisey_sequence.vx = 0.2 * Eigen::ArrayXf::Ones(30); + noisey_sequence.vy = 0.0 * Eigen::ArrayXf::Ones(30); + noisey_sequence.wz = 0.3 * Eigen::ArrayXf::Ones(30); + + // Make the sequence noisy + std::mt19937 engine; + std::normal_distribution normal_dist = std::normal_distribution(0.0f, 0.2f); + auto noises = Eigen::ArrayXf::NullaryExpr( + 30, [&] () {return normal_dist(engine);}); + noisey_sequence.vx += noises; + noisey_sequence.vy += noises; + noisey_sequence.wz += noises; + sequence_init = noisey_sequence; + + std::array history, history_init; + history[3].vx = 0.1; + history[3].vy = 0.0; + history[3].wz = 0.3; + history[2].vx = 0.1; + history[2].vy = 0.0; + history[2].wz = 0.3; + history[1].vx = 0.1; + history[1].vy = 0.0; + history[1].wz = 0.3; + history[0].vx = 0.0; + history[0].vy = 0.0; + history[0].wz = 0.0; + history_init = history; + + models::OptimizerSettings settings; + settings.shift_control_sequence = false; // so result stores 0th value in history + + savitskyGolayFilter(noisey_sequence, history, settings); + + // Check history is propagated backward + EXPECT_NEAR(history_init[3].vx, history[2].vx, 0.02); + EXPECT_NEAR(history_init[3].vy, history[2].vy, 0.02); + EXPECT_NEAR(history_init[3].wz, history[2].wz, 0.02); + + // Check history element is updated for first command + EXPECT_NEAR(history[3].vx, 0.2, 0.05); + EXPECT_NEAR(history[3].vy, 0.0, 0.035); + EXPECT_NEAR(history[3].wz, 0.23, 0.02); + + // Check that path is smoother + float smoothed_val{0}, original_val{0}; + for (unsigned int i = 1; i != noisey_sequence.vx.size() - 1; i++) { + smoothed_val += fabs(noisey_sequence.vx(i) - 0.2); + smoothed_val += fabs(noisey_sequence.vy(i) - 0.0); + smoothed_val += fabs(noisey_sequence.wz(i) - 0.3); + + original_val += fabs(sequence_init.vx(i) - 0.2); + original_val += fabs(sequence_init.vy(i) - 0.0); + original_val += fabs(sequence_init.wz(i) - 0.3); + } + + EXPECT_LT(smoothed_val, original_val); +} + +TEST(UtilsTests, FindPathInversionTest) +{ + // Straight path, no inversions to be found + nav_msgs::msg::Path path; + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::findFirstPathInversion(path), 10u); + + // To short to process + path.poses.erase(path.poses.begin(), path.poses.begin() + 7); + EXPECT_EQ(utils::findFirstPathInversion(path), 3u); + + // Has inversion at index 10, so should return 11 for the first point afterwards + // 0 1 2 3 4 5 6 7 8 9 10 **9** 8 7 6 5 4 3 2 1 + path.poses.clear(); + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 10 - i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::findFirstPathInversion(path), 11u); +} + +TEST(UtilsTests, RemovePosesAfterPathInversionTest) +{ + nav_msgs::msg::Path path; + // straight path + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u); + + // try empty path + path.poses.clear(); + EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u); + + // cusping path + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 10 - i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 11u); + // Check to see if removed + EXPECT_EQ(path.poses.size(), 11u); + EXPECT_EQ(path.poses.back().pose.position.x, 10); +} + +TEST(UtilsTests, ShiftColumnsByOnePlaceTest) +{ + // Try with scalar value + Eigen::ArrayXf scalar_val(1); + scalar_val(0) = 5; + utils::shiftColumnsByOnePlace(scalar_val, 1); + EXPECT_EQ(scalar_val.size(), 1); + EXPECT_EQ(scalar_val(0), 5); + + // Try with one dimensional array, shift right + Eigen::ArrayXf array_1d(4); + array_1d << 1, 2, 3, 4; + utils::shiftColumnsByOnePlace(array_1d, 1); + EXPECT_EQ(array_1d.size(), 4); + EXPECT_EQ(array_1d(0), 1); + EXPECT_EQ(array_1d(1), 1); + EXPECT_EQ(array_1d(2), 2); + EXPECT_EQ(array_1d(3), 3); + + // Try with one dimensional array, shift left + array_1d(1) = 5; + utils::shiftColumnsByOnePlace(array_1d, -1); + EXPECT_EQ(array_1d.size(), 4); + EXPECT_EQ(array_1d(0), 5); + EXPECT_EQ(array_1d(1), 2); + EXPECT_EQ(array_1d(2), 3); + EXPECT_EQ(array_1d(2), 3); + + // Try with two dimensional array, shift right + // 1 2 3 4 1 1 2 3 + // 5 6 7 8 -> 5 5 6 7 + // 9 10 11 12 9 9 10 11 + Eigen::ArrayXXf array_2d(3, 4); + array_2d << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12; + utils::shiftColumnsByOnePlace(array_2d, 1); + EXPECT_EQ(array_2d.rows(), 3); + EXPECT_EQ(array_2d.cols(), 4); + EXPECT_EQ(array_2d(0, 0), 1); + EXPECT_EQ(array_2d(1, 0), 5); + EXPECT_EQ(array_2d(2, 0), 9); + EXPECT_EQ(array_2d(0, 1), 1); + EXPECT_EQ(array_2d(1, 1), 5); + EXPECT_EQ(array_2d(2, 1), 9); + EXPECT_EQ(array_2d(0, 2), 2); + EXPECT_EQ(array_2d(1, 2), 6); + EXPECT_EQ(array_2d(2, 2), 10); + EXPECT_EQ(array_2d(0, 3), 3); + EXPECT_EQ(array_2d(1, 3), 7); + EXPECT_EQ(array_2d(2, 3), 11); + + array_2d.col(0).setZero(); + + // Try with two dimensional array, shift left + // 0 1 2 3 1 2 3 3 + // 0 5 6 7 -> 5 6 7 7 + // 0 9 10 11 9 10 11 11 + utils::shiftColumnsByOnePlace(array_2d, -1); + EXPECT_EQ(array_2d.rows(), 3); + EXPECT_EQ(array_2d.cols(), 4); + EXPECT_EQ(array_2d(0, 0), 1); + EXPECT_EQ(array_2d(1, 0), 5); + EXPECT_EQ(array_2d(2, 0), 9); + EXPECT_EQ(array_2d(0, 1), 2); + EXPECT_EQ(array_2d(1, 1), 6); + EXPECT_EQ(array_2d(2, 1), 10); + EXPECT_EQ(array_2d(0, 2), 3); + EXPECT_EQ(array_2d(1, 2), 7); + EXPECT_EQ(array_2d(2, 2), 11); + EXPECT_EQ(array_2d(0, 3), 3); + EXPECT_EQ(array_2d(1, 3), 7); + EXPECT_EQ(array_2d(2, 3), 11); + + // Try with invalid direction value. + EXPECT_THROW(utils::shiftColumnsByOnePlace(array_2d, -2), std::logic_error); +} + +TEST(UtilsTests, NormalizeYawsBetweenPointsTest) +{ + Eigen::ArrayXf last_yaws(10); + last_yaws.setZero(); + + Eigen::ArrayXf yaw_between_points(10); + yaw_between_points.setZero(10); + + // Try with both angles 0 + Eigen::ArrayXf yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws, + yaw_between_points); + EXPECT_TRUE(yaws_between_points_corrected.isApprox(yaw_between_points)); + + // Try with yaw between points as pi/4 + yaw_between_points.setConstant(M_PIF_2 / 2); + yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws, + yaw_between_points); + EXPECT_TRUE(yaws_between_points_corrected.isApprox(yaw_between_points)); + + // Try with yaw between points as pi/2 + yaw_between_points.setConstant(M_PIF_2); + yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws, + yaw_between_points); + EXPECT_TRUE(yaws_between_points_corrected.isApprox(yaw_between_points)); + + // Try with a few yaw between points more than pi/2 + yaw_between_points[1] = 1.2 * M_PIF_2; + yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws, + yaw_between_points); + EXPECT_NEAR(yaws_between_points_corrected[1], -0.8 * M_PIF_2, 1e-3); + EXPECT_NEAR(yaws_between_points_corrected[0], yaw_between_points[0], 1e-3); + EXPECT_NEAR(yaws_between_points_corrected[9], yaw_between_points[9], 1e-3); + + // Try with goal angle 0 + float goal_angle = 0; + yaws_between_points_corrected = utils::normalize_yaws_between_points(goal_angle, + yaw_between_points); + EXPECT_NEAR(yaws_between_points_corrected[1], -0.8 * M_PIF_2, 1e-3); +} + +TEST(UtilsTests, toTrajectoryMsgTest) +{ + Eigen::ArrayXXf trajectory(5, 3); + trajectory << + 0.0, 0.0, 0.0, + 1.0, 1.0, 1.0, + 2.0, 2.0, 2.0, + 3.0, 3.0, 3.0, + 4.0, 4.0, 4.0; + + models::ControlSequence control_sequence; + control_sequence.vx = Eigen::ArrayXf::Ones(5); + control_sequence.wz = Eigen::ArrayXf::Ones(5); + control_sequence.vy = Eigen::ArrayXf::Zero(5); + + std_msgs::msg::Header header; + header.frame_id = "map"; + header.stamp = rclcpp::Time(100, 0, RCL_ROS_TIME); + + auto trajectory_msg = utils::toTrajectoryMsg( + trajectory, control_sequence, 1.0, header); + + EXPECT_EQ(trajectory_msg->header.frame_id, "map"); + EXPECT_EQ(trajectory_msg->header.stamp, header.stamp); + EXPECT_EQ(trajectory_msg->points.size(), 5u); + EXPECT_EQ(trajectory_msg->points[0].pose.position.x, 0.0); + EXPECT_EQ(trajectory_msg->points[0].pose.position.y, 0.0); + EXPECT_EQ(trajectory_msg->points[1].pose.position.x, 1.0); + EXPECT_EQ(trajectory_msg->points[1].pose.position.y, 1.0); + EXPECT_EQ(trajectory_msg->points[2].pose.position.x, 2.0); + EXPECT_EQ(trajectory_msg->points[2].pose.position.y, 2.0); + EXPECT_EQ(trajectory_msg->points[3].pose.position.x, 3.0); + EXPECT_EQ(trajectory_msg->points[3].pose.position.y, 3.0); + EXPECT_EQ(trajectory_msg->points[4].pose.position.x, 4.0); + EXPECT_EQ(trajectory_msg->points[4].pose.position.y, 4.0); + + EXPECT_EQ(trajectory_msg->points[0].velocity.linear.x, 1.0); + EXPECT_EQ(trajectory_msg->points[0].velocity.linear.y, 0.0); + EXPECT_EQ(trajectory_msg->points[0].velocity.angular.z, 1.0); + EXPECT_EQ(trajectory_msg->points[1].velocity.linear.x, 1.0); + EXPECT_EQ(trajectory_msg->points[1].velocity.linear.y, 0.0); + EXPECT_EQ(trajectory_msg->points[1].velocity.angular.z, 1.0); + EXPECT_EQ(trajectory_msg->points[2].velocity.linear.x, 1.0); + EXPECT_EQ(trajectory_msg->points[2].velocity.linear.y, 0.0); + EXPECT_EQ(trajectory_msg->points[2].velocity.angular.z, 1.0); + EXPECT_EQ(trajectory_msg->points[3].velocity.linear.x, 1.0); + EXPECT_EQ(trajectory_msg->points[3].velocity.linear.y, 0.0); + EXPECT_EQ(trajectory_msg->points[3].velocity.angular.z, 1.0); + EXPECT_EQ(trajectory_msg->points[4].velocity.linear.x, 1.0); + EXPECT_EQ(trajectory_msg->points[4].velocity.linear.y, 0.0); + EXPECT_EQ(trajectory_msg->points[4].velocity.angular.z, 1.0); + + EXPECT_EQ(trajectory_msg->points[0].time_from_start, rclcpp::Duration(0, 0)); + EXPECT_EQ(trajectory_msg->points[1].time_from_start, rclcpp::Duration(1, 0)); + EXPECT_EQ(trajectory_msg->points[2].time_from_start, rclcpp::Duration(2, 0)); + EXPECT_EQ(trajectory_msg->points[3].time_from_start, rclcpp::Duration(3, 0)); + EXPECT_EQ(trajectory_msg->points[4].time_from_start, rclcpp::Duration(4, 0)); +} + +TEST(UtilsTests, getLastPathPoseTest) +{ + nav_msgs::msg::Path path; + path.poses.resize(10); + path.poses[9].pose.position.x = 5.0; + path.poses[9].pose.position.y = 50.0; + path.poses[9].pose.orientation.x = 0.0; + path.poses[9].pose.orientation.y = 0.0; + path.poses[9].pose.orientation.z = 1.0; + path.poses[9].pose.orientation.w = 0.0; + + models::Path path_t = toTensor(path); + geometry_msgs::msg::Pose last_path_pose = utils::getLastPathPose(path_t); + + EXPECT_EQ(last_path_pose.position.x, 5); + EXPECT_EQ(last_path_pose.position.y, 50); + EXPECT_NEAR(last_path_pose.orientation.x, 0.0, 1e-3); + EXPECT_NEAR(last_path_pose.orientation.y, 0.0, 1e-3); + EXPECT_NEAR(last_path_pose.orientation.z, 1.0, 1e-3); + EXPECT_NEAR(last_path_pose.orientation.w, 0.0, 1e-3); +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + rclcpp::init(0, nullptr); + + int result = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + + return result; +} diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 6f70e19cfa5..792ae839b22 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -116,56 +116,6 @@ TEST(UtilsTests, ConversionTests) EXPECT_NEAR(path_t.yaws(2), 0.0, 1e-6); } -TEST(UtilsTests, WithTolTests) -{ - geometry_msgs::msg::Pose pose; - pose.position.x = 10.0; - pose.position.y = 1.0; - - nav2_core::GoalChecker * goal_checker = new TestGoalChecker; - - nav_msgs::msg::Path path; - path.poses.resize(2); - geometry_msgs::msg::Pose & goal = path.poses.back().pose; - - // Create CriticData with state and goal initialized - models::State state; - state.pose.pose = pose; - models::Trajectories generated_trajectories; - models::Path path_critic; - Eigen::ArrayXf costs; - float model_dt; - CriticData data = { - state, generated_trajectories, path_critic, goal, - costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; - - // Test not in tolerance - goal.position.x = 0.0; - goal.position.y = 0.0; - EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, goal)); - EXPECT_FALSE(withinPositionGoalTolerance(0.25, pose, goal)); - - // Test in tolerance - goal.position.x = 9.8; - goal.position.y = 0.95; - EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal)); - EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal)); - - goal.position.x = 10.0; - goal.position.y = 0.76; - EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal)); - EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal)); - - goal.position.x = 9.76; - goal.position.y = 1.0; - EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal)); - EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal)); - - delete goal_checker; - goal_checker = nullptr; - EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, goal)); -} - TEST(UtilsTests, AnglesTests) { // Test angle normalization by creating insane angles @@ -231,7 +181,8 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, std::nullopt, nullptr, + nullptr, std::nullopt, std::nullopt}; /// Caution, keep references // Attempt to set furthest point if notionally set, should not change @@ -241,7 +192,8 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) // Attempt to set if not set already with no other information, should fail CriticData data2 = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, std::nullopt, nullptr, + nullptr, std::nullopt, std::nullopt}; /// Caution, keep references setPathFurthestPointIfNotSet(data2); EXPECT_EQ(data2.furthest_reached_path_point, 0); @@ -260,7 +212,8 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) path = toTensor(plan); CriticData data3 = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, std::nullopt, nullptr, + nullptr, std::nullopt, std::nullopt}; /// Caution, keep references EXPECT_EQ(findPathFurthestReachedPoint(data3), 5); } @@ -276,7 +229,8 @@ TEST(UtilsTests, findPathCosts) float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, std::nullopt, nullptr, + nullptr, std::nullopt, std::nullopt}; /// Caution, keep references // Test not set if already set, should not change @@ -289,7 +243,8 @@ TEST(UtilsTests, findPathCosts) EXPECT_EQ(data.path_pts_valid->size(), 10u); CriticData data3 = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, std::nullopt, nullptr, + nullptr, std::nullopt, std::nullopt}; /// Caution, keep references auto costmap_ros = std::make_shared( @@ -650,61 +605,6 @@ TEST(UtilsTests, getLastPathPoseTest) EXPECT_NEAR(last_path_pose.orientation.w, 0.0, 1e-3); } -TEST(UtilsTests, getCriticGoalTest) -{ - geometry_msgs::msg::Pose pose; - pose.position.x = 10.0; - pose.position.y = 1.0; - - nav_msgs::msg::Path path; - path.poses.resize(10); - path.poses[9].pose.position.x = 5.0; - path.poses[9].pose.position.y = 50.0; - path.poses[9].pose.orientation.x = 0.0; - path.poses[9].pose.orientation.y = 0.0; - path.poses[9].pose.orientation.z = 1.0; - path.poses[9].pose.orientation.w = 0.0; - - geometry_msgs::msg::Pose goal; - goal.position.x = 6.0; - goal.position.y = 60.0; - goal.orientation.x = 0.0; - goal.orientation.y = 0.0; - goal.orientation.z = 0.0; - goal.orientation.w = 1.0; - - // Create CriticData with state and goal initialized - models::State state; - state.pose.pose = pose; - models::Trajectories generated_trajectories; - models::Path path_t = toTensor(path); - Eigen::ArrayXf costs; - float model_dt; - CriticData data = { - state, generated_trajectories, path_t, goal, - costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; - - bool enforce_path_inversion = true; - geometry_msgs::msg::Pose target_goal = utils::getCriticGoal(data, enforce_path_inversion); - - EXPECT_EQ(target_goal.position.x, 5); - EXPECT_EQ(target_goal.position.y, 50); - EXPECT_NEAR(target_goal.orientation.x, 0.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.y, 0.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.z, 1.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.w, 0.0, 1e-3); - - enforce_path_inversion = false; - target_goal = utils::getCriticGoal(data, enforce_path_inversion); - - EXPECT_EQ(target_goal.position.x, 6); - EXPECT_EQ(target_goal.position.y, 60); - EXPECT_NEAR(target_goal.orientation.x, 0.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.y, 0.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.z, 0.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.w, 1.0, 1e-3); -} - int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); @@ -716,4 +616,4 @@ int main(int argc, char **argv) rclcpp::shutdown(); return result; -} +} \ No newline at end of file diff --git a/nav2_mppi_controller/trajectory_validators.xml b/nav2_mppi_controller/trajectory_validators.xml index 5a4da246123..fb02d0dac9a 100644 --- a/nav2_mppi_controller/trajectory_validators.xml +++ b/nav2_mppi_controller/trajectory_validators.xml @@ -3,5 +3,12 @@ Default trajectory validator for MPPI controller + + + Trajectory validator with automatic collision repair before rejection. + + diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 6252ab32e4e..41311173486 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_msgs) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(nav2_common REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(nav_msgs REQUIRED) @@ -10,6 +11,7 @@ find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) find_package(geographic_msgs) find_package(action_msgs REQUIRED) +find_package(unique_identifier_msgs REQUIRED) nav2_package() @@ -29,6 +31,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/VoxelGrid.msg" "msg/BehaviorTreeStatusChange.msg" "msg/BehaviorTreeLog.msg" + "msg/CriticsStats.msg" "msg/Particle.msg" "msg/ParticleCloud.msg" "msg/WaypointStatus.msg" @@ -39,6 +42,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Trajectory.msg" "msg/TrajectoryPoint.msg" "srv/GetCosts.srv" + "msg/PolygonObject.msg" + "msg/CircleObject.msg" "srv/GetCostmap.srv" "srv/IsPathValid.srv" "srv/ClearCostmapExceptRegion.srv" @@ -50,6 +55,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SaveMap.srv" "srv/SetInitialPose.srv" "srv/ReloadDockDatabase.srv" + "srv/AddShapes.srv" + "srv/RemoveShapes.srv" + "srv/GetShapes.srv" "srv/SetRouteGraph.srv" "srv/DynamicEdges.srv" "action/AssistedTeleop.action" @@ -70,7 +78,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/UndockRobot.action" "action/ComputeRoute.action" "action/ComputeAndTrackRoute.action" - DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs geographic_msgs + DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs geographic_msgs unique_identifier_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/nav2_msgs/msg/CircleObject.msg b/nav2_msgs/msg/CircleObject.msg new file mode 100644 index 00000000000..2aa64adab62 --- /dev/null +++ b/nav2_msgs/msg/CircleObject.msg @@ -0,0 +1,6 @@ +std_msgs/Header header +unique_identifier_msgs/UUID uuid +geometry_msgs/Point32 center +float32 radius +bool fill +int8 value diff --git a/nav2_msgs/msg/CriticsStats.msg b/nav2_msgs/msg/CriticsStats.msg new file mode 100644 index 00000000000..f02406228df --- /dev/null +++ b/nav2_msgs/msg/CriticsStats.msg @@ -0,0 +1,5 @@ +# Critics statistics message +builtin_interfaces/Time stamp +string[] critics +bool[] changed +float32[] costs_sum diff --git a/nav2_msgs/msg/PolygonObject.msg b/nav2_msgs/msg/PolygonObject.msg new file mode 100644 index 00000000000..b5c9721c629 --- /dev/null +++ b/nav2_msgs/msg/PolygonObject.msg @@ -0,0 +1,5 @@ +std_msgs/Header header +unique_identifier_msgs/UUID uuid +geometry_msgs/Point32[] points +bool closed +int8 value diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 12a085fbf88..33de880662e 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -12,6 +12,7 @@ ament_cmake nav2_common + backward_ros rclcpp std_msgs builtin_interfaces @@ -20,6 +21,7 @@ action_msgs nav_msgs geographic_msgs + unique_identifier_msgs rosidl_interface_packages diff --git a/nav2_msgs/srv/AddShapes.srv b/nav2_msgs/srv/AddShapes.srv new file mode 100644 index 00000000000..1b3b5753874 --- /dev/null +++ b/nav2_msgs/srv/AddShapes.srv @@ -0,0 +1,6 @@ +# Add/update vector objects on map + +CircleObject[] circles +PolygonObject[] polygons +--- +bool success diff --git a/nav2_msgs/srv/GetShapes.srv b/nav2_msgs/srv/GetShapes.srv new file mode 100644 index 00000000000..79b7d6c431c --- /dev/null +++ b/nav2_msgs/srv/GetShapes.srv @@ -0,0 +1,5 @@ +# Get vector objects which are now being applied on map + +--- +CircleObject[] circles +PolygonObject[] polygons diff --git a/nav2_msgs/srv/RemoveShapes.srv b/nav2_msgs/srv/RemoveShapes.srv new file mode 100644 index 00000000000..a001f9360ef --- /dev/null +++ b/nav2_msgs/srv/RemoveShapes.srv @@ -0,0 +1,6 @@ +# Remove vector objects from map + +bool all_objects +unique_identifier_msgs/UUID[] uuids +--- +bool success diff --git a/nav2_navfn_planner/CMakeLists.txt b/nav2_navfn_planner/CMakeLists.txt index df0a924a40a..1b50472013f 100644 --- a/nav2_navfn_planner/CMakeLists.txt +++ b/nav2_navfn_planner/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_navfn_planner) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index e00bcefaf29..fe4c11b373a 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -12,6 +12,7 @@ ament_cmake nav2_common + backward_ros builtin_interfaces geometry_msgs nav2_costmap_2d diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index fab5d3f0fcf..dd5ac3f3e5f 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -353,11 +353,11 @@ NavfnPlanner::smoothApproachToGoal( const geometry_msgs::msg::Pose & goal, nav_msgs::msg::Path & plan) { - // Replace the last pose of the computed path if it's actually further away - // to the second to last pose than the goal pose. if (plan.poses.size() >= 2) { auto second_to_last_pose = plan.poses.end()[-2]; auto last_pose = plan.poses.back(); + // Replace the last pose of the computed path if it's actually further away + // to the second to last pose than the goal pose. if ( squared_distance(last_pose.pose, second_to_last_pose.pose) > squared_distance(goal, second_to_last_pose.pose)) @@ -365,6 +365,11 @@ NavfnPlanner::smoothApproachToGoal( plan.poses.back().pose = goal; return; } + // Replace the last pose of the computed path if its position matches but orientation differs + if (squared_distance(last_pose.pose, goal) < 1e-6) { + plan.poses.back().pose = goal; + return; + } } geometry_msgs::msg::PoseStamped goal_copy; goal_copy.pose = goal; diff --git a/nav2_planner/CMakeLists.txt b/nav2_planner/CMakeLists.txt index 9c2e44e69b2..0285fbe9c1a 100644 --- a/nav2_planner/CMakeLists.txt +++ b/nav2_planner/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_planner) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(nav2_common REQUIRED) diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index c9fc2a46dd6..372f4c7c2f9 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -32,8 +32,8 @@ #include "nav2_util/robot_utils.hpp" #include "nav2_ros_common/simple_action_server.hpp" #include "nav2_ros_common/service_server.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index b0cdf1c8846..50ad1add559 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -10,6 +10,7 @@ ament_cmake nav2_common + backward_ros geometry_msgs lifecycle_msgs nav2_core diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 81752e25644..998220d6533 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -428,9 +428,17 @@ void PlannerServer::computePlanThroughPoses() throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path"); } - // Concatenate paths together - concat_path.poses.insert( - concat_path.poses.end(), curr_path.poses.begin(), curr_path.poses.end()); + // Concatenate paths together, but skip the first pose of subsequent paths + // to avoid duplicating the connection point + if (i == 0) { + // First path: add all poses + concat_path.poses.insert( + concat_path.poses.end(), curr_path.poses.begin(), curr_path.poses.end()); + } else if (curr_path.poses.size() > 1) { + // Subsequent paths: skip the first pose to avoid duplication + concat_path.poses.insert( + concat_path.poses.end(), curr_path.poses.begin() + 1, curr_path.poses.end()); + } concat_path.header = curr_path.header; } diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt index 2cd43c87154..bec081d3987 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -1,8 +1,9 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_regulated_pure_pursuit_controller) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp index cc8b7915330..135a304dc26 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp @@ -43,7 +43,7 @@ class PathHandler * @brief Constructor for nav2_regulated_pure_pursuit_controller::PathHandler */ PathHandler( - tf2::Duration transform_tolerance, + double transform_tolerance, std::shared_ptr tf, std::shared_ptr costmap_ros); @@ -65,18 +65,6 @@ class PathHandler const geometry_msgs::msg::PoseStamped & pose, double max_robot_pose_search_dist, bool reject_unit_path = false); - /** - * @brief Transform a pose to another frame. - * @param frame Frame ID to transform to - * @param in_pose Pose input to transform - * @param out_pose transformed output - * @return bool if successful - */ - bool transformPose( - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose) const; - void setPlan(const nav_msgs::msg::Path & path) {global_plan_ = path;} nav_msgs::msg::Path getPlan() {return global_plan_;} @@ -89,7 +77,7 @@ class PathHandler double getCostmapMaxExtent() const; rclcpp::Logger logger_ {rclcpp::get_logger("RPPPathHandler")}; - tf2::Duration transform_tolerance_; + double transform_tolerance_; std::shared_ptr tf_; std::shared_ptr costmap_ros_; nav_msgs::msg::Path global_plan_; diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 2eb901dc11f..fd79208d291 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -174,32 +174,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign); - /** - * @brief Find the intersection a circle and a line segment. - * This assumes the circle is centered at the origin. - * If no intersection is found, a floating point error will occur. - * @param p1 first endpoint of line segment - * @param p2 second endpoint of line segment - * @param r radius of circle - * @return point of intersection - */ - static geometry_msgs::msg::Point circleSegmentIntersection( - const geometry_msgs::msg::Point & p1, - const geometry_msgs::msg::Point & p2, - double r); - - /** - * @brief Get lookahead point - * @param lookahead_dist Optimal lookahead distance - * @param path Current global path - * @param interpolate_after_goal If true, interpolate the lookahead point after the goal based - * on the orientation given by the position of the last two pose of the path - * @return Lookahead point - */ - geometry_msgs::msg::PoseStamped getLookAheadPoint( - const double &, const nav_msgs::msg::Path &, - bool interpolate_after_goal = false); - /** * @brief checks for the cusp position * @param pose Pose input to determine the cusp position diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 454b9542c7d..70df60839d7 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -12,6 +12,7 @@ nav2_common angles + backward_ros geometry_msgs nav2_core nav2_costmap_2d diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp index 1fcc39b7e37..c91bb8dc8f7 100644 --- a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp @@ -23,6 +23,7 @@ #include "nav2_core/controller_exceptions.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" namespace nav2_regulated_pure_pursuit_controller { @@ -30,7 +31,7 @@ namespace nav2_regulated_pure_pursuit_controller using nav2_util::geometry_utils::euclidean_distance; PathHandler::PathHandler( - tf2::Duration transform_tolerance, + double transform_tolerance, std::shared_ptr tf, std::shared_ptr costmap_ros) : transform_tolerance_(transform_tolerance), tf_(tf), costmap_ros_(costmap_ros) @@ -60,7 +61,9 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( // let's get the pose of the robot in the frame of the plan geometry_msgs::msg::PoseStamped robot_pose; - if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { + if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_, global_plan_.header.frame_id, + transform_tolerance_)) + { throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); } @@ -101,7 +104,9 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( stamped_pose.header.frame_id = global_plan_.header.frame_id; stamped_pose.header.stamp = robot_pose.header.stamp; stamped_pose.pose = global_plan_pose.pose; - if (!transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose)) { + if (!nav2_util::transformPoseInTargetFrame(stamped_pose, transformed_pose, *tf_, + costmap_ros_->getBaseFrameID(), transform_tolerance_)) + { throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); } transformed_pose.pose.position.z = 0.0; @@ -128,24 +133,4 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( return transformed_plan; } -bool PathHandler::transformPose( - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose) const -{ - if (in_pose.header.frame_id == frame) { - out_pose = in_pose; - return true; - } - - try { - tf_->transform(in_pose, out_pose, frame, transform_tolerance_); - out_pose.header.frame_id = frame; - return true; - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what()); - } - return false; -} - } // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index d8153ea16bd..fced1dff734 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -25,6 +25,7 @@ #include "nav2_core/controller_exceptions.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" +#include "nav2_util/controller_utils.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" using std::hypot; @@ -61,7 +62,7 @@ void RegulatedPurePursuitController::configure( // Handles global path transformations path_handler_ = std::make_unique( - tf2::durationFromSec(params_->transform_tolerance), tf_, costmap_ros_); + params_->transform_tolerance, tf_, costmap_ros_); // Checks for imminent collisions collision_checker_ = std::make_unique(node, costmap_ros_, params_); @@ -204,7 +205,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } // Get the particular point on the path at the lookahead distance - auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); + auto carrot_pose = nav2_util::getLookAheadPoint(lookahead_dist, transformed_plan); auto rotate_to_path_carrot_pose = carrot_pose; carrot_pub_->publish(createCarrotMsg(carrot_pose)); @@ -214,7 +215,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity double regulation_curvature = lookahead_curvature; if (params_->use_fixed_curvature_lookahead) { - auto curvature_lookahead_pose = getLookAheadPoint( + auto curvature_lookahead_pose = nav2_util::getLookAheadPoint( curv_lookahead_dist, transformed_plan, params_->interpolate_curvature_after_goal); rotate_to_path_carrot_pose = curvature_lookahead_pose; @@ -358,100 +359,6 @@ void RegulatedPurePursuitController::rotateToHeading( } } -geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersection( - const geometry_msgs::msg::Point & p1, - const geometry_msgs::msg::Point & p2, - double r) -{ - // Formula for intersection of a line with a circle centered at the origin, - // modified to always return the point that is on the segment between the two points. - // https://mathworld.wolfram.com/Circle-LineIntersection.html - // This works because the poses are transformed into the robot frame. - // This can be derived from solving the system of equations of a line and a circle - // which results in something that is just a reformulation of the quadratic formula. - // Interactive illustration in doc/circle-segment-intersection.ipynb as well as at - // https://www.desmos.com/calculator/td5cwbuocd - double x1 = p1.x; - double x2 = p2.x; - double y1 = p1.y; - double y2 = p2.y; - - double dx = x2 - x1; - double dy = y2 - y1; - double dr2 = dx * dx + dy * dy; - double D = x1 * y2 - x2 * y1; - - // Augmentation to only return point within segment - double d1 = x1 * x1 + y1 * y1; - double d2 = x2 * x2 + y2 * y2; - double dd = d2 - d1; - - geometry_msgs::msg::Point p; - double sqrt_term = std::sqrt(r * r * dr2 - D * D); - p.x = (D * dy + std::copysign(1.0, dd) * dx * sqrt_term) / dr2; - p.y = (-D * dx + std::copysign(1.0, dd) * dy * sqrt_term) / dr2; - return p; -} - -geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( - const double & lookahead_dist, - const nav_msgs::msg::Path & transformed_plan, - bool interpolate_after_goal) -{ - // Find the first pose which is at a distance greater than the lookahead distance - auto goal_pose_it = std::find_if( - transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) { - return hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist; - }); - - // If the no pose is not far enough, take the last pose - if (goal_pose_it == transformed_plan.poses.end()) { - if (interpolate_after_goal) { - auto last_pose_it = std::prev(transformed_plan.poses.end()); - auto prev_last_pose_it = std::prev(last_pose_it); - - double end_path_orientation = atan2( - last_pose_it->pose.position.y - prev_last_pose_it->pose.position.y, - last_pose_it->pose.position.x - prev_last_pose_it->pose.position.x); - - // Project the last segment out to guarantee it is beyond the look ahead - // distance - auto projected_position = last_pose_it->pose.position; - projected_position.x += cos(end_path_orientation) * lookahead_dist; - projected_position.y += sin(end_path_orientation) * lookahead_dist; - - // Use the circle intersection to find the position at the correct look - // ahead distance - const auto interpolated_position = circleSegmentIntersection( - last_pose_it->pose.position, projected_position, lookahead_dist); - - geometry_msgs::msg::PoseStamped interpolated_pose; - interpolated_pose.header = last_pose_it->header; - interpolated_pose.pose.position = interpolated_position; - return interpolated_pose; - } else { - goal_pose_it = std::prev(transformed_plan.poses.end()); - } - } else if (goal_pose_it != transformed_plan.poses.begin()) { - // Find the point on the line segment between the two poses - // that is exactly the lookahead distance away from the robot pose (the origin) - // This can be found with a closed form for the intersection of a segment and a circle - // Because of the way we did the std::find_if, prev_pose is guaranteed to be inside the circle, - // and goal_pose is guaranteed to be outside the circle. - auto prev_pose_it = std::prev(goal_pose_it); - auto point = circleSegmentIntersection( - prev_pose_it->pose.position, - goal_pose_it->pose.position, lookahead_dist); - geometry_msgs::msg::PoseStamped pose; - pose.header.frame_id = prev_pose_it->header.frame_id; - pose.header.stamp = goal_pose_it->header.stamp; - pose.pose.position = point; - return pose; - } - - return *goal_pose_it; -} - void RegulatedPurePursuitController::applyConstraints( const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/, const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign) diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 197cae4c5a0..4b3dbb905f8 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -52,28 +52,6 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure return getLookAheadDistance(twist); } - static geometry_msgs::msg::Point circleSegmentIntersectionWrapper( - const geometry_msgs::msg::Point & p1, - const geometry_msgs::msg::Point & p2, - double r) - { - return circleSegmentIntersection(p1, p2, r); - } - - geometry_msgs::msg::PoseStamped - projectCarrotPastGoalWrapper( - const double & dist, - const nav_msgs::msg::Path & path) - { - return getLookAheadPoint(dist, path, true); - } - - geometry_msgs::msg::PoseStamped getLookAheadPointWrapper( - const double & dist, const nav_msgs::msg::Path & path) - { - return getLookAheadPoint(dist, path); - } - bool shouldRotateToPathWrapper( const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) { @@ -207,239 +185,6 @@ TEST(RegulatedPurePursuitTest, findVelocitySignChange) EXPECT_EQ(rtn, std::numeric_limits::max()); } -using CircleSegmentIntersectionParam = std::tuple< - std::pair, - std::pair, - double, - std::pair ->; - -class CircleSegmentIntersectionTest - : public ::testing::TestWithParam -{}; - -TEST_P(CircleSegmentIntersectionTest, circleSegmentIntersection) -{ - auto pair1 = std::get<0>(GetParam()); - auto pair2 = std::get<1>(GetParam()); - auto r = std::get<2>(GetParam()); - auto expected_pair = std::get<3>(GetParam()); - auto pair_to_point = [](std::pair p) -> geometry_msgs::msg::Point { - geometry_msgs::msg::Point point; - point.x = p.first; - point.y = p.second; - point.z = 0.0; - return point; - }; - auto p1 = pair_to_point(pair1); - auto p2 = pair_to_point(pair2); - auto actual = BasicAPIRPP::circleSegmentIntersectionWrapper(p1, p2, r); - auto expected_point = pair_to_point(expected_pair); - EXPECT_DOUBLE_EQ(actual.x, expected_point.x); - EXPECT_DOUBLE_EQ(actual.y, expected_point.y); - // Expect that the intersection point is actually r away from the origin - EXPECT_DOUBLE_EQ(r, std::hypot(actual.x, actual.y)); -} - -INSTANTIATE_TEST_SUITE_P( - InterpolationTest, - CircleSegmentIntersectionTest, - testing::Values( - // Origin to the positive X axis - CircleSegmentIntersectionParam{ - {0.0, 0.0}, - {2.0, 0.0}, - 1.0, - {1.0, 0.0} -}, - // Origin to the negative X axis - CircleSegmentIntersectionParam{ - {0.0, 0.0}, - {-2.0, 0.0}, - 1.0, - {-1.0, 0.0} -}, - // Origin to the positive Y axis - CircleSegmentIntersectionParam{ - {0.0, 0.0}, - {0.0, 2.0}, - 1.0, - {0.0, 1.0} -}, - // Origin to the negative Y axis - CircleSegmentIntersectionParam{ - {0.0, 0.0}, - {0.0, -2.0}, - 1.0, - {0.0, -1.0} -}, - // non-origin to the X axis with non-unit circle, with the second point inside - CircleSegmentIntersectionParam{ - {4.0, 0.0}, - {-1.0, 0.0}, - 2.0, - {2.0, 0.0} -}, - // non-origin to the Y axis with non-unit circle, with the second point inside - CircleSegmentIntersectionParam{ - {0.0, 4.0}, - {0.0, -0.5}, - 2.0, - {0.0, 2.0} -}, - // origin to the positive X axis, on the circle - CircleSegmentIntersectionParam{ - {2.0, 0.0}, - {0.0, 0.0}, - 2.0, - {2.0, 0.0} -}, - // origin to the positive Y axis, on the circle - CircleSegmentIntersectionParam{ - {0.0, 0.0}, - {0.0, 2.0}, - 2.0, - {0.0, 2.0} -}, - // origin to the upper-right quadrant (3-4-5 triangle) - CircleSegmentIntersectionParam{ - {0.0, 0.0}, - {6.0, 8.0}, - 5.0, - {3.0, 4.0} -}, - // origin to the lower-left quadrant (3-4-5 triangle) - CircleSegmentIntersectionParam{ - {0.0, 0.0}, - {-6.0, -8.0}, - 5.0, - {-3.0, -4.0} -}, - // origin to the upper-left quadrant (3-4-5 triangle) - CircleSegmentIntersectionParam{ - {0.0, 0.0}, - {-6.0, 8.0}, - 5.0, - {-3.0, 4.0} -}, - // origin to the lower-right quadrant (3-4-5 triangle) - CircleSegmentIntersectionParam{ - {0.0, 0.0}, - {6.0, -8.0}, - 5.0, - {3.0, -4.0} -} -)); - -TEST(RegulatedPurePursuitTest, projectCarrotPastGoal) { - auto ctrl = std::make_shared(); - auto node = std::make_shared("testRPP"); - std::string name = "PathFollower"; - auto tf = std::make_shared(node->get_clock()); - auto costmap = - std::make_shared("fake_costmap"); - rclcpp_lifecycle::State state; - costmap->on_configure(state); - ctrl->configure(node, name, tf, costmap); - - double EPSILON = std::numeric_limits::epsilon(); - - nav_msgs::msg::Path path; - // More than 2 poses - path.poses.resize(4); - path.poses[0].pose.position.x = 0.0; - path.poses[1].pose.position.x = 1.0; - path.poses[2].pose.position.x = 2.0; - path.poses[3].pose.position.x = 3.0; - auto pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); - EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON); - EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON); - - // 2 poses fwd - path.poses.clear(); - path.poses.resize(2); - path.poses[0].pose.position.x = 2.0; - path.poses[1].pose.position.x = 3.0; - pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); - EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON); - EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON); - - // 2 poses at 45° - path.poses.clear(); - path.poses.resize(2); - path.poses[0].pose.position.x = 2.0; - path.poses[0].pose.position.y = 2.0; - path.poses[1].pose.position.x = 3.0; - path.poses[1].pose.position.y = 3.0; - pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); - EXPECT_NEAR(pt.pose.position.x, cos(45.0 * M_PI / 180) * 10.0, EPSILON); - EXPECT_NEAR(pt.pose.position.y, sin(45.0 * M_PI / 180) * 10.0, EPSILON); - - // 2 poses at 90° - path.poses.clear(); - path.poses.resize(2); - path.poses[0].pose.position.x = 0.0; - path.poses[0].pose.position.y = 2.0; - path.poses[1].pose.position.x = 0.0; - path.poses[1].pose.position.y = 3.0; - pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); - EXPECT_NEAR(pt.pose.position.x, cos(90.0 * M_PI / 180) * 10.0, EPSILON); - EXPECT_NEAR(pt.pose.position.y, sin(90.0 * M_PI / 180) * 10.0, EPSILON); - - // 2 poses at 135° - path.poses.clear(); - path.poses.resize(2); - path.poses[0].pose.position.x = -2.0; - path.poses[0].pose.position.y = 2.0; - path.poses[1].pose.position.x = -3.0; - path.poses[1].pose.position.y = 3.0; - pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); - EXPECT_NEAR(pt.pose.position.x, cos(135.0 * M_PI / 180) * 10.0, EPSILON); - EXPECT_NEAR(pt.pose.position.y, sin(135.0 * M_PI / 180) * 10.0, EPSILON); - - // 2 poses back - path.poses.clear(); - path.poses.resize(2); - path.poses[0].pose.position.x = -2.0; - path.poses[1].pose.position.x = -3.0; - pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); - EXPECT_NEAR(pt.pose.position.x, -10.0, EPSILON); - EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON); - - // 2 poses at -135° - path.poses.clear(); - path.poses.resize(2); - path.poses[0].pose.position.x = -2.0; - path.poses[0].pose.position.y = -2.0; - path.poses[1].pose.position.x = -3.0; - path.poses[1].pose.position.y = -3.0; - pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); - EXPECT_NEAR(pt.pose.position.x, cos(-135.0 * M_PI / 180) * 10.0, EPSILON); - EXPECT_NEAR(pt.pose.position.y, sin(-135.0 * M_PI / 180) * 10.0, EPSILON); - - // 2 poses at -90° - path.poses.clear(); - path.poses.resize(2); - path.poses[0].pose.position.x = 0.0; - path.poses[0].pose.position.y = -2.0; - path.poses[1].pose.position.x = 0.0; - path.poses[1].pose.position.y = -3.0; - pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); - EXPECT_NEAR(pt.pose.position.x, cos(-90.0 * M_PI / 180) * 10.0, EPSILON); - EXPECT_NEAR(pt.pose.position.y, sin(-90.0 * M_PI / 180) * 10.0, EPSILON); - - // 2 poses at -45° - path.poses.clear(); - path.poses.resize(2); - path.poses[0].pose.position.x = 2.0; - path.poses[0].pose.position.y = -2.0; - path.poses[1].pose.position.x = 3.0; - path.poses[1].pose.position.y = -3.0; - pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); - EXPECT_NEAR(pt.pose.position.x, cos(-45.0 * M_PI / 180) * 10.0, EPSILON); - EXPECT_NEAR(pt.pose.position.y, sin(-45.0 * M_PI / 180) * 10.0, EPSILON); -} - TEST(RegulatedPurePursuitTest, lookaheadAPI) { auto ctrl = std::make_shared(); @@ -476,24 +221,6 @@ TEST(RegulatedPurePursuitTest, lookaheadAPI) twist.linear.x = 0.0; rtn = ctrl->getLookAheadDistanceWrapper(twist); EXPECT_EQ(rtn, 0.3); - - // test getLookAheadPoint - double dist = 1.0; - nav_msgs::msg::Path path; - path.poses.resize(10); - for (uint i = 0; i != path.poses.size(); i++) { - path.poses[i].pose.position.x = static_cast(i); - } - - // test exact hits - auto pt = ctrl->getLookAheadPointWrapper(dist, path); - EXPECT_EQ(pt.pose.position.x, 1.0); - - // test interpolation - ctrl->configure(node, name, tf, costmap); - dist = 3.8; - pt = ctrl->getLookAheadPointWrapper(dist, path); - EXPECT_EQ(pt.pose.position.x, 3.8); } TEST(RegulatedPurePursuitTest, rotateTests) diff --git a/nav2_ros_common/CMakeLists.txt b/nav2_ros_common/CMakeLists.txt index fc529730ea8..4765454321d 100644 --- a/nav2_ros_common/CMakeLists.txt +++ b/nav2_ros_common/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_ros_common) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(bond REQUIRED) find_package(bondcpp REQUIRED) find_package(geometry_msgs REQUIRED) diff --git a/nav2_ros_common/include/nav2_ros_common/service_client.hpp b/nav2_ros_common/include/nav2_ros_common/service_client.hpp index c1bc2bd8435..1cf32f25fe0 100644 --- a/nav2_ros_common/include/nav2_ros_common/service_client.hpp +++ b/nav2_ros_common/include/nav2_ros_common/service_client.hpp @@ -218,6 +218,16 @@ class ServiceClient return service_name_; } + /** + * @brief Stop any running spin operations on the internal executor + */ + void stop() + { + if (client_) { + callback_group_executor_->cancel(); + } + } + protected: std::string service_name_; rclcpp::Clock::SharedPtr clock_; diff --git a/nav2_ros_common/package.xml b/nav2_ros_common/package.xml index 33731f20e15..e2e2eb2695d 100644 --- a/nav2_ros_common/package.xml +++ b/nav2_ros_common/package.xml @@ -11,6 +11,7 @@ nav2_common + backward_ros bond bondcpp builtin_interfaces diff --git a/nav2_rotation_shim_controller/CMakeLists.txt b/nav2_rotation_shim_controller/CMakeLists.txt index c8140e661a6..68b64fdc0a8 100644 --- a/nav2_rotation_shim_controller/CMakeLists.txt +++ b/nav2_rotation_shim_controller/CMakeLists.txt @@ -1,8 +1,9 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_rotation_shim_controller) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_controller REQUIRED) diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 2505599a071..02fbffd55f0 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -11,6 +11,7 @@ nav2_common angles + backward_ros geometry_msgs nav2_core nav2_controller diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 0b97876182f..ee87908d827 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -24,7 +24,7 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_controller/plugins/simple_goal_checker.hpp" #include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" class RotationShimShim : public nav2_rotation_shim_controller::RotationShimController { diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt index 3ceb96f6753..db9c139c6f1 100644 --- a/nav2_route/CMakeLists.txt +++ b/nav2_route/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_route CXX) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) diff --git a/nav2_route/include/nav2_route/edge_scorer.hpp b/nav2_route/include/nav2_route/edge_scorer.hpp index c294ac47e43..7b6534b50c4 100644 --- a/nav2_route/include/nav2_route/edge_scorer.hpp +++ b/nav2_route/include/nav2_route/edge_scorer.hpp @@ -19,7 +19,7 @@ #include #include -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" #include "nav2_ros_common/node_utils.hpp" diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp index db26121fcef..8e407d66c91 100644 --- a/nav2_route/include/nav2_route/goal_intent_extractor.hpp +++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp @@ -19,7 +19,7 @@ #include #include -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" #include "nav2_core/route_exceptions.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_ros_common/node_utils.hpp" diff --git a/nav2_route/include/nav2_route/graph_loader.hpp b/nav2_route/include/nav2_route/graph_loader.hpp index 2a39da1bee1..c2235e5e914 100644 --- a/nav2_route/include/nav2_route/graph_loader.hpp +++ b/nav2_route/include/nav2_route/graph_loader.hpp @@ -23,8 +23,8 @@ #include #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_route/types.hpp" diff --git a/nav2_route/include/nav2_route/graph_saver.hpp b/nav2_route/include/nav2_route/graph_saver.hpp index 7cc25d83c04..8155689df55 100644 --- a/nav2_route/include/nav2_route/graph_saver.hpp +++ b/nav2_route/include/nav2_route/graph_saver.hpp @@ -23,8 +23,8 @@ #include #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_route/types.hpp" diff --git a/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp b/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp index e20bade1070..e45980f3a45 100644 --- a/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp +++ b/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp @@ -18,7 +18,7 @@ #include #include -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "pluginlib/class_loader.hpp" #include "nav2_route/types.hpp" diff --git a/nav2_route/include/nav2_route/node_spatial_tree.hpp b/nav2_route/include/nav2_route/node_spatial_tree.hpp index 15d9e2a5528..744d30791c7 100644 --- a/nav2_route/include/nav2_route/node_spatial_tree.hpp +++ b/nav2_route/include/nav2_route/node_spatial_tree.hpp @@ -55,7 +55,9 @@ struct GraphAdaptor }; typedef nanoflann::KDTreeSingleIndexAdaptor< - nanoflann::L2_Simple_Adaptor, GraphAdaptor, DIMENSION> kd_tree_t; + nanoflann::L2_Simple_Adaptor, GraphAdaptor, DIMENSION, + unsigned int> + kd_tree_t; /** * @class nav2_route::NodeSpatialTree diff --git a/nav2_route/include/nav2_route/route_planner.hpp b/nav2_route/include/nav2_route/route_planner.hpp index bdadc6c9dd0..cd0c978bc90 100644 --- a/nav2_route/include/nav2_route/route_planner.hpp +++ b/nav2_route/include/nav2_route/route_planner.hpp @@ -22,8 +22,8 @@ #include #include -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_route/types.hpp" #include "nav2_route/utils.hpp" #include "nav2_route/edge_scorer.hpp" diff --git a/nav2_route/include/nav2_route/route_server.hpp b/nav2_route/include/nav2_route/route_server.hpp index 3d384e93c0b..ce52c07e750 100644 --- a/nav2_route/include/nav2_route/route_server.hpp +++ b/nav2_route/include/nav2_route/route_server.hpp @@ -23,8 +23,8 @@ #include #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_ros_common/simple_action_server.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/robot_utils.hpp" diff --git a/nav2_route/include/nav2_route/route_tracker.hpp b/nav2_route/include/nav2_route/route_tracker.hpp index 9c2e7058aec..a2a3d4e897c 100644 --- a/nav2_route/include/nav2_route/route_tracker.hpp +++ b/nav2_route/include/nav2_route/route_tracker.hpp @@ -16,7 +16,7 @@ #include #include -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_ros_common/simple_action_server.hpp" #include "nav2_msgs/action/compute_and_track_route.hpp" diff --git a/nav2_route/include/nav2_route/utils.hpp b/nav2_route/include/nav2_route/utils.hpp index 9ccf65eb41e..9b9e9078e7e 100644 --- a/nav2_route/include/nav2_route/utils.hpp +++ b/nav2_route/include/nav2_route/utils.hpp @@ -59,102 +59,104 @@ inline visualization_msgs::msg::MarkerArray toMsg( const nav2_route::Graph & graph, const std::string & frame, const rclcpp::Time & now) { visualization_msgs::msg::MarkerArray msg; - visualization_msgs::msg::Marker curr_marker; - curr_marker.header.frame_id = frame; - curr_marker.header.stamp = now; - curr_marker.action = 0; - auto getSphereSize = []() { - geometry_msgs::msg::Vector3 v_msg; - v_msg.x = 0.05; - v_msg.y = 0.05; - v_msg.z = 0.05; - return v_msg; - }; + visualization_msgs::msg::Marker nodes_marker; + nodes_marker.header.frame_id = frame; + nodes_marker.header.stamp = now; + nodes_marker.action = 0; + nodes_marker.ns = "route_graph_nodes"; + nodes_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; + nodes_marker.scale.x = 0.1; + nodes_marker.scale.y = 0.1; + nodes_marker.scale.z = 0.1; + nodes_marker.color.r = 1.0; + nodes_marker.color.a = 1.0; + nodes_marker.points.reserve(graph.size()); - auto getSphereColor = []() { - std_msgs::msg::ColorRGBA c_msg; - c_msg.r = 1.0; - c_msg.g = 0.0; - c_msg.b = 0.0; - c_msg.a = 1.0; - return c_msg; - }; + visualization_msgs::msg::Marker edges_marker; + edges_marker.header.frame_id = frame; + edges_marker.header.stamp = now; + edges_marker.action = 0; + edges_marker.ns = "route_graph_edges"; + edges_marker.type = visualization_msgs::msg::Marker::LINE_LIST; + edges_marker.scale.x = 0.05; // Line width + edges_marker.color.g = 1.0; + edges_marker.color.a = 0.5; // Semi-transparent green so bidirectional connections stand out + constexpr size_t points_per_edge = 2; + // This probably under-reserves but saves some initial reallocations + constexpr size_t likely_min_edges_per_node = 2; + edges_marker.points.reserve(graph.size() * points_per_edge * likely_min_edges_per_node); - auto getLineColor = []() { - std_msgs::msg::ColorRGBA c_msg; - c_msg.r = 0.0; - c_msg.g = 1.0; - c_msg.b = 0.0; - c_msg.a = 0.5; // So bi-directional connections stand out overlapping - return c_msg; - }; + geometry_msgs::msg::Point node_pos; + geometry_msgs::msg::Point edge_start; + geometry_msgs::msg::Point edge_end; - unsigned int marker_idx = 1; - for (unsigned int i = 0; i != graph.size(); i++) { - if (graph[i].nodeid == std::numeric_limits::max()) { - continue; // Skip "deleted" nodes - } - curr_marker.ns = "route_graph"; - curr_marker.id = marker_idx++; - curr_marker.type = visualization_msgs::msg::Marker::SPHERE; - curr_marker.pose.position.x = graph[i].coords.x; - curr_marker.pose.position.y = graph[i].coords.y; - curr_marker.scale = getSphereSize(); - curr_marker.color = getSphereColor(); - msg.markers.push_back(curr_marker); + visualization_msgs::msg::Marker node_id_marker; + node_id_marker.header.frame_id = frame; + node_id_marker.header.stamp = now; + node_id_marker.action = 0; + node_id_marker.ns = "route_graph_node_ids"; + node_id_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + node_id_marker.scale.x = 0.1; + node_id_marker.scale.y = 0.1; + node_id_marker.scale.z = 0.1; + node_id_marker.color.a = 1.0; + node_id_marker.color.r = 1.0; + + visualization_msgs::msg::Marker edge_id_marker; + edge_id_marker.header.frame_id = frame; + edge_id_marker.header.stamp = now; + edge_id_marker.action = 0; + edge_id_marker.ns = "route_graph_edge_ids"; + edge_id_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + edge_id_marker.scale.x = 0.1; + edge_id_marker.scale.y = 0.1; + edge_id_marker.scale.z = 0.1; + edge_id_marker.color.a = 1.0; + edge_id_marker.color.g = 1.0; - // Add text - curr_marker.ns = "route_graph_ids"; - curr_marker.id = marker_idx++; - curr_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - curr_marker.pose.position.x = graph[i].coords.x + 0.07; - curr_marker.pose.position.y = graph[i].coords.y; - curr_marker.text = std::to_string(graph[i].nodeid); - curr_marker.scale.z = 0.1; - msg.markers.push_back(curr_marker); + for (const auto & node : graph) { + node_pos.x = node.coords.x; + node_pos.y = node.coords.y; + nodes_marker.points.push_back(node_pos); - for (unsigned int j = 0; j != graph[i].neighbors.size(); j++) { - curr_marker.ns = "route_graph"; - curr_marker.id = marker_idx++; - curr_marker.type = visualization_msgs::msg::Marker::LINE_LIST; - curr_marker.pose.position.x = 0; // Set to 0 since points are relative to this frame - curr_marker.pose.position.y = 0; // Set to 0 since points are relative to this frame - curr_marker.points.resize(2); - curr_marker.points[0].x = graph[i].coords.x; - curr_marker.points[0].y = graph[i].coords.y; - curr_marker.points[1].x = graph[i].neighbors[j].end->coords.x; - curr_marker.points[1].y = graph[i].neighbors[j].end->coords.y; - curr_marker.scale.x = 0.03; - curr_marker.color = getLineColor(); - msg.markers.push_back(curr_marker); - curr_marker.points.clear(); // Reset for next node marker + // Add text for Node ID + node_id_marker.id++; + node_id_marker.pose.position.x = node.coords.x + 0.07; + node_id_marker.pose.position.y = node.coords.y; + node_id_marker.text = std::to_string(node.nodeid); + msg.markers.push_back(node_id_marker); - // Add text - curr_marker.ns = "route_graph_ids"; - curr_marker.id = marker_idx++; - curr_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - curr_marker.pose.position.x = - graph[i].coords.x + ((graph[i].neighbors[j].end->coords.x - graph[i].coords.x) / 2.0) + - 0.07; + for (const auto & neighbor : node.neighbors) { + edge_start.x = node.coords.x; + edge_start.y = node.coords.y; + edge_end.x = neighbor.end->coords.x; + edge_end.y = neighbor.end->coords.y; + edges_marker.points.push_back(edge_start); + edges_marker.points.push_back(edge_end); // Deal with overlapping bi-directional text markers by offsetting locations float y_offset = 0.0; - if (graph[i].nodeid > graph[i].neighbors[j].end->nodeid) { + if (node.nodeid > neighbor.end->nodeid) { y_offset = 0.05; } else { y_offset = -0.05; } + const float x_offset = 0.07; - curr_marker.pose.position.y = - graph[i].coords.y + ((graph[i].neighbors[j].end->coords.y - graph[i].coords.y) / 2.0) + - y_offset; - curr_marker.text = std::to_string(graph[i].neighbors[j].edgeid); - curr_marker.scale.z = 0.1; - msg.markers.push_back(curr_marker); + // Add text for Edge ID + edge_id_marker.id++; + edge_id_marker.pose.position.x = + node.coords.x + ((neighbor.end->coords.x - node.coords.x) / 2.0) + x_offset; + edge_id_marker.pose.position.y = + node.coords.y + ((neighbor.end->coords.y - node.coords.y) / 2.0) + y_offset; + edge_id_marker.text = std::to_string(neighbor.edgeid); + msg.markers.push_back(edge_id_marker); } } + msg.markers.push_back(edges_marker); + msg.markers.push_back(nodes_marker); return msg; } diff --git a/nav2_route/package.xml b/nav2_route/package.xml index 855356eefc2..93fffbf8e1a 100644 --- a/nav2_route/package.xml +++ b/nav2_route/package.xml @@ -10,6 +10,7 @@ ament_cmake nav2_common + backward_ros rclcpp rclcpp_lifecycle std_msgs diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp index 14443b77641..483739aca05 100644 --- a/nav2_route/src/goal_intent_extractor.cpp +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -175,6 +175,7 @@ GoalIntentExtractor::findStartandGoal(const std::shared_ptr goal) node_pose.pose.position.x = node_data.coords.x; node_pose.pose.position.y = node_data.coords.y; node_pose.header.frame_id = node_data.coords.frame_id; + node_pose.header.stamp = start_pose.header.stamp; candidate_nodes.push_back(transformPose(node_pose, costmap_frame_id)); } @@ -202,6 +203,7 @@ GoalIntentExtractor::findStartandGoal(const std::shared_ptr goal) node_pose.pose.position.x = node_data.coords.x; node_pose.pose.position.y = node_data.coords.y; node_pose.header.frame_id = node_data.coords.frame_id; + node_pose.header.stamp = goal_pose.header.stamp; candidate_nodes.push_back(transformPose(node_pose, costmap_frame_id)); } diff --git a/nav2_route/src/path_converter.cpp b/nav2_route/src/path_converter.cpp index ff48773dcea..46776c5f0e9 100644 --- a/nav2_route/src/path_converter.cpp +++ b/nav2_route/src/path_converter.cpp @@ -141,6 +141,11 @@ void PathConverter::interpolateEdge( // Find number of points to populate by given density const float mag = hypotf(x1 - x0, y1 - y0); const unsigned int num_pts = ceil(mag / density_); + // For zero-length edges, we can just push the start point and return + if (num_pts < 1) { + return; + } + const float iterpolated_dist = mag / num_pts; // Find unit vector direction diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp index 2a44005111b..fee6bf7b671 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -35,7 +35,7 @@ RouteServer::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_base_interface(), get_node_timers_interface()); tf_->setCreateTimerInterface(timer_interface); - transform_listener_ = std::make_shared(*tf_); + transform_listener_ = std::make_shared(*tf_, this, true); auto node = shared_from_this(); graph_vis_publisher_ = diff --git a/nav2_route/test/test_edge_scorers.cpp b/nav2_route/test/test_edge_scorers.cpp index 07af4293d0b..8c16ced8306 100644 --- a/nav2_route/test/test_edge_scorers.cpp +++ b/nav2_route/test/test_edge_scorers.cpp @@ -27,8 +27,8 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_2d_publisher.hpp" #include "nav2_core/route_exceptions.hpp" -#include "tf2_ros/static_transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/static_transform_broadcaster.hpp" +#include "tf2_ros/transform_listener.hpp" class RclCppFixture { diff --git a/nav2_route/test/test_goal_intent_extractor.cpp b/nav2_route/test/test_goal_intent_extractor.cpp index c3ac8d176cc..21c49708193 100644 --- a/nav2_route/test/test_goal_intent_extractor.cpp +++ b/nav2_route/test/test_goal_intent_extractor.cpp @@ -19,9 +19,9 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_ros_common/service_client.hpp" #include "nav2_ros_common/node_thread.hpp" diff --git a/nav2_route/test/test_goal_intent_search.cpp b/nav2_route/test/test_goal_intent_search.cpp index a829af0370f..1f3e8fd4b1e 100644 --- a/nav2_route/test/test_goal_intent_search.cpp +++ b/nav2_route/test/test_goal_intent_search.cpp @@ -19,9 +19,9 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_ros_common/service_client.hpp" #include "nav2_ros_common/node_thread.hpp" diff --git a/nav2_route/test/test_graph_loader.cpp b/nav2_route/test/test_graph_loader.cpp index 99341852f78..84b544a1be5 100644 --- a/nav2_route/test/test_graph_loader.cpp +++ b/nav2_route/test/test_graph_loader.cpp @@ -20,7 +20,7 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav2_route/graph_loader.hpp" -#include "tf2_ros/static_transform_broadcaster.h" +#include "tf2_ros/static_transform_broadcaster.hpp" class RclCppFixture { @@ -107,7 +107,6 @@ TEST(GraphLoader, test_transformation_api) tf_broadcaster->sendTransform(transform); rclcpp::Rate(1).sleep(); tf_broadcaster->sendTransform(transform); - rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(1)); rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); graph[0].coords.frame_id = "map_test"; diff --git a/nav2_route/test/test_graph_saver.cpp b/nav2_route/test/test_graph_saver.cpp index a6897f11602..bd69a9bc112 100644 --- a/nav2_route/test/test_graph_saver.cpp +++ b/nav2_route/test/test_graph_saver.cpp @@ -22,7 +22,7 @@ #include "nav2_ros_common/node_utils.hpp" #include "nav2_route/graph_loader.hpp" #include "nav2_route/graph_saver.hpp" -#include "tf2_ros/static_transform_broadcaster.h" +#include "tf2_ros/static_transform_broadcaster.hpp" class RclCppFixture { @@ -143,7 +143,6 @@ TEST(GraphSaver, test_transformation_api) tf_broadcaster->sendTransform(transform); rclcpp::Rate(1).sleep(); tf_broadcaster->sendTransform(transform); - rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(1)); rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); GraphSaver graph_saver(node, tf, frame); diff --git a/nav2_route/test/test_path_converter.cpp b/nav2_route/test/test_path_converter.cpp index 91c8ec749a1..d8af7736cc8 100644 --- a/nav2_route/test/test_path_converter.cpp +++ b/nav2_route/test/test_path_converter.cpp @@ -163,3 +163,15 @@ TEST(PathConverterTest, test_path_converter_interpolation) poses[i].pose.position.y - poses[i + 1].pose.position.y), 0.05); } } + +TEST(PathConverterTest, test_path_converter_zero_length_edge) +{ + auto node = std::make_shared("edge_scorer_test"); + PathConverter converter; + converter.configure(node); + + float x0 = 10.0, y0 = 10.0, x1 = 10.0, y1 = 10.0; + std::vector poses; + converter.interpolateEdge(x0, y0, x1, y1, poses); + ASSERT_TRUE(poses.empty()); +} diff --git a/nav2_route/test/test_route_server.cpp b/nav2_route/test/test_route_server.cpp index f8906529d31..5fb73b4ce0f 100644 --- a/nav2_route/test/test_route_server.cpp +++ b/nav2_route/test/test_route_server.cpp @@ -19,9 +19,9 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "std_srvs/srv/trigger.hpp" #include "nav2_ros_common/service_client.hpp" diff --git a/nav2_route/test/test_route_tracker.cpp b/nav2_route/test/test_route_tracker.cpp index 9d874a1dc5a..07ca24a4e91 100644 --- a/nav2_route/test/test_route_tracker.cpp +++ b/nav2_route/test/test_route_tracker.cpp @@ -19,9 +19,9 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/transform_listener.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_core/route_exceptions.hpp" #include "nav2_route/route_tracker.hpp" diff --git a/nav2_route/test/test_utils_and_types.cpp b/nav2_route/test/test_utils_and_types.cpp index a6b9a7e37e9..25703f95ff3 100644 --- a/nav2_route/test/test_utils_and_types.cpp +++ b/nav2_route/test/test_utils_and_types.cpp @@ -159,7 +159,15 @@ TEST(UtilsTest, test_to_visualization_msg_conversion) graph[3].addEdge(default_cost, &graph[6], ids++); auto graph_msg = utils::toMsg(graph, frame, time); - EXPECT_EQ(graph_msg.markers.size(), 34u); // 9 nodes and 8 edges (with text markers) + constexpr size_t expected_edge_markers = 1; + constexpr size_t expected_node_markers = 1; + constexpr size_t expected_edge_id_text_markers = 8; + constexpr size_t expected_node_id_text_markers = 9; + constexpr size_t expected_total_markers = + expected_edge_markers + expected_node_markers + + expected_edge_id_text_markers + expected_node_id_text_markers; + + EXPECT_EQ(graph_msg.markers.size(), expected_total_markers); for (auto & marker : graph_msg.markers) { if (marker.ns == "route_graph_ids") { EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::TEXT_VIEW_FACING); diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index 958047a22c2..b917cbffc33 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -1,8 +1,9 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_rviz_plugins) find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_lifecycle_manager REQUIRED) diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index 3a4cdd9e50f..44ad28d279e 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -35,9 +35,9 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include "nav2_util/geometry_utils.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/buffer.hpp" class QPushButton; diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 91de46fa84c..3a25157d1ce 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -12,6 +12,7 @@ qtbase5-dev ament_index_cpp + backward_ros geometry_msgs nav2_lifecycle_manager nav2_msgs diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index ea93fc61711..f25c7774293 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -41,7 +41,7 @@ from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy from rclpy.task import Future -from rclpy.type_support import GetResultServiceResponse +#from rclpy.type_support import GetResultServiceResponse # Task Result enum for the result of the task being executed @@ -291,7 +291,7 @@ def goThroughPoses(self, poses: Goals, behavior_tree: str = '') -> Optional[Runn goal_msg.poses = poses goal_msg.behavior_tree = behavior_tree - self.info(f'Navigating with {len(goal_msg.poses)} goals....') + self.info(f'Navigating with {len(goal_msg.poses.goals)} goals....') send_goal_future = self.nav_through_poses_client.send_goal_async( goal_msg, self._feedbackCallback ) diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index a344a41aa0f..08563b3df41 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -1,9 +1,10 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_smac_planner) find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) diff --git a/nav2_smac_planner/lattice_primitives/config.json b/nav2_smac_planner/lattice_primitives/config.json index 12a8470bdb3..531bbdccad1 100644 --- a/nav2_smac_planner/lattice_primitives/config.json +++ b/nav2_smac_planner/lattice_primitives/config.json @@ -1,6 +1,6 @@ { - "motion_model": "ackermann", - "turning_radius": 0.5, + "motion_model": "diff", + "turning_radius": 0.00, "grid_resolution": 0.05, "stopping_threshold": 5, "num_of_headings": 16 diff --git a/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py index bf9e64559ab..7e278a9313b 100644 --- a/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py +++ b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py @@ -21,9 +21,9 @@ from typing import Any, cast, Dict, List, TypedDict import matplotlib.pyplot as plt -from nav2_smac_planner.lattice_primitives import constants -from nav2_smac_planner.lattice_primitives.lattice_generator import ConfigDict, LatticeGenerator -from nav2_smac_planner.lattice_primitives.trajectory import Trajectory +from lattice_primitives import constants +from lattice_primitives.lattice_generator import ConfigDict, LatticeGenerator +from lattice_primitives.trajectory import Trajectory import numpy as np logging.basicConfig(level=logging.INFO) diff --git a/nav2_smac_planner/lattice_primitives/lattice_generator.py b/nav2_smac_planner/lattice_primitives/lattice_generator.py index 7aacd36e156..1d909366f95 100644 --- a/nav2_smac_planner/lattice_primitives/lattice_generator.py +++ b/nav2_smac_planner/lattice_primitives/lattice_generator.py @@ -16,10 +16,10 @@ from enum import Enum from typing import Any, Dict, List, Tuple, TypedDict -from nav2_smac_planner.lattice_primitives.helper import angle_difference, interpolate_yaws -from nav2_smac_planner.lattice_primitives.trajectory import (AnyFloat, FloatNDArray, Path, +from lattice_primitives.helper import angle_difference, interpolate_yaws +from lattice_primitives.trajectory import (AnyFloat, FloatNDArray, Path, Trajectory, TrajectoryParameters) -from nav2_smac_planner.lattice_primitives.trajectory_generator import TrajectoryGenerator +from lattice_primitives.trajectory_generator import TrajectoryGenerator import numpy as np from rtree import index diff --git a/nav2_smac_planner/lattice_primitives/output.json b/nav2_smac_planner/lattice_primitives/output.json new file mode 100644 index 00000000000..b5766a0c2df --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/output.json @@ -0,0 +1,3864 @@ +{ + "version": 1.0, + "date_generated": "2025-12-03", + "lattice_metadata": { + "motion_model": "diff", + "turning_radius": 0.08, + "grid_resolution": 0.05, + "stopping_threshold": 5, + "num_of_headings": 16, + "heading_angles": [ + 0.0, + 0.4636476090008061, + 0.7853981633974483, + 1.1071487177940904, + 1.5707963267948966, + 2.0344439357957027, + 2.356194490192345, + 2.677945044588987, + 3.141592653589793, + 3.6052402625905993, + 3.9269908169872414, + 4.2487413713838835, + 4.71238898038469, + 5.176036589385496, + 5.497787143782138, + 5.81953769817878 + ], + "number_of_trajectories": 136 + }, + "primitives": [ + { + "trajectory_id": 0, + "start_angle_index": 0, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.0809, + "trajectory_length": 0.15137, + "arc_length": 0.08957, + "straight_length": 0.0618, + "poses": [ + [ + 0.04725, + -0.01523, + 5.659486377865489 + ], + [ + 0.07743, + -0.05487, + 5.176036589385496 + ], + [ + 0.1, + -0.1, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 1, + "start_angle_index": 0, + "end_angle_index": 14, + "left_turn": false, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + 0.05558, + -0.01356, + 5.80469930592716 + ], + [ + 0.1, + -0.05, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 2, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 6.128636104179318 + ], + [ + 0.0, + 0.0, + 5.9740869011790485 + ], + [ + 0.0, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 3, + "start_angle_index": 0, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.05, + "arc_length": 0.0, + "straight_length": 0.05, + "poses": [ + [ + 0.05, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 4, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.1545492030002687 + ], + [ + 0.0, + 0.0, + 0.3090984060005374 + ], + [ + 0.0, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 5, + "start_angle_index": 0, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + 0.05558, + 0.01356, + 0.47848600125242624 + ], + [ + 0.1, + 0.05, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 6, + "start_angle_index": 0, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0809, + "trajectory_length": 0.15137, + "arc_length": 0.08957, + "straight_length": 0.0618, + "poses": [ + [ + 0.04725, + 0.01523, + 0.6236989293140972 + ], + [ + 0.07743, + 0.05487, + 1.1071487177940904 + ], + [ + 0.1, + 0.1, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 7, + "start_angle_index": 1, + "end_angle_index": 14, + "left_turn": false, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + 0.04916, + 0.02356, + 0.32653252396015897 + ], + [ + 0.10294, + 0.02617, + 6.053752487460941 + ], + [ + 0.15, + 0.0, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 8, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + 0.05, + 0.0118, + 0.0 + ], + [ + 0.1, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 9, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.22361, + "trajectory_length": 0.20735, + "arc_length": 0.20735, + "straight_length": 0.0, + "poses": [ + [ + 0.04862, + 0.01763, + 0.23182380450040305 + ], + [ + 0.1, + 0.02361, + 0.0 + ], + [ + 0.15138, + 0.01763, + 6.051361502679184 + ], + [ + 0.2, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 10, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.33541, + "trajectory_length": 0.31102, + "arc_length": 0.31102, + "straight_length": 0.0, + "poses": [ + [ + 0.04797, + 0.01951, + 0.3090984060005374 + ], + [ + 0.09837, + 0.03141, + 0.1545492030002687 + ], + [ + 0.15, + 0.03541, + 0.0 + ], + [ + 0.20163, + 0.03141, + 6.128636104179318 + ], + [ + 0.25203, + 0.01951, + 5.9740869011790485 + ], + [ + 0.3, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 11, + "start_angle_index": 1, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.4636476090008061 + ], + [ + 0.0, + 0.0, + 0.23182380450040305 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 12, + "start_angle_index": 1, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.1118, + "arc_length": 0.0, + "straight_length": 0.1118, + "poses": [ + [ + 0.05, + 0.025, + 0.4636476090008061 + ], + [ + 0.1, + 0.05, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 13, + "start_angle_index": 1, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.4636476090008061 + ], + [ + 0.0, + 0.0, + 0.6245228861991272 + ], + [ + 0.0, + 0.0, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 14, + "start_angle_index": 1, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.07194, + "arc_length": 0.07194, + "straight_length": 0.0, + "poses": [ + [ + 0.05, + 0.05, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 15, + "start_angle_index": 1, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.22361, + "trajectory_length": 0.14389, + "arc_length": 0.14389, + "straight_length": 0.0, + "poses": [ + [ + 0.04028, + 0.02587, + 0.6781479785985676 + ], + [ + 0.07413, + 0.05972, + 0.892648348196329 + ], + [ + 0.1, + 0.1, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 16, + "start_angle_index": 1, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.3618, + "trajectory_length": 0.42696, + "arc_length": 0.40057, + "straight_length": 0.02639, + "poses": [ + [ + 0.04092, + 0.02394, + 0.5947686722333784 + ], + [ + 0.07836, + 0.05302, + 0.7258897354659507 + ], + [ + 0.11168, + 0.08674, + 0.8570107986985229 + ], + [ + 0.1403, + 0.12453, + 0.9881318619310953 + ], + [ + 0.16374, + 0.16574, + 1.1192529251636676 + ], + [ + 0.18159, + 0.20965, + 1.25037398839624 + ], + [ + 0.19354, + 0.25553, + 1.3814950516288125 + ], + [ + 0.19939, + 0.30257, + 1.512616114861385 + ], + [ + 0.2, + 0.35, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 17, + "start_angle_index": 2, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + 0.04706, + 0.02617, + 0.2294328197186447 + ], + [ + 0.10084, + 0.02356, + 5.956652783219427 + ], + [ + 0.15, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 18, + "start_angle_index": 2, + "end_angle_index": 0, + "left_turn": false, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + 0.04442, + 0.03644, + 0.4784860012524262 + ], + [ + 0.1, + 0.05, + 0.0 + ] + ] + }, + { + "trajectory_id": 19, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.7853981633974483 + ], + [ + 0.0, + 0.0, + 0.6245228861991272 + ], + [ + 0.0, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 20, + "start_angle_index": 2, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.07071, + "arc_length": 0.0, + "straight_length": 0.07071, + "poses": [ + [ + 0.05, + 0.05, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 21, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.7853981633974483 + ], + [ + 0.0, + 0.0, + 0.9462734405957693 + ], + [ + 0.0, + 0.0, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 22, + "start_angle_index": 2, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + 0.03644, + 0.04442, + 1.0923103255424704 + ], + [ + 0.05, + 0.1, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 23, + "start_angle_index": 2, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + 0.02617, + 0.04706, + 1.341363507076252 + ], + [ + 0.02356, + 0.10084, + 1.8973288507550554 + ], + [ + 0.0, + 0.15, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 24, + "start_angle_index": 3, + "end_angle_index": 0, + "left_turn": false, + "trajectory_radius": 0.36181, + "trajectory_length": 0.42697, + "arc_length": 0.40058, + "straight_length": 0.02639, + "poses": [ + [ + 0.02393, + 0.04093, + 0.9760278785613808 + ], + [ + 0.05301, + 0.07837, + 0.8449070393286713 + ], + [ + 0.08673, + 0.11169, + 0.7137862000959616 + ], + [ + 0.12452, + 0.14031, + 0.582665360863252 + ], + [ + 0.16573, + 0.16375, + 0.4515445216305424 + ], + [ + 0.20965, + 0.18159, + 0.3204236823978327 + ], + [ + 0.25553, + 0.19355, + 0.18930284316512325 + ], + [ + 0.30257, + 0.1994, + 0.05818200393241346 + ], + [ + 0.35, + 0.2, + 0.0 + ] + ] + }, + { + "trajectory_id": 25, + "start_angle_index": 3, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.07194, + "arc_length": 0.07194, + "straight_length": 0.0, + "poses": [ + [ + 0.05, + 0.05, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 26, + "start_angle_index": 3, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.22361, + "trajectory_length": 0.14389, + "arc_length": 0.14389, + "straight_length": 0.0, + "poses": [ + [ + 0.02587, + 0.04028, + 0.892648348196329 + ], + [ + 0.05972, + 0.07413, + 0.6781479785985676 + ], + [ + 0.1, + 0.1, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 27, + "start_angle_index": 3, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.1071487177940904 + ], + [ + 0.0, + 0.0, + 0.9998985329952097 + ], + [ + 0.0, + 0.0, + 0.892648348196329 + ], + [ + 0.0, + 0.0, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 28, + "start_angle_index": 3, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.1118, + "arc_length": 0.0, + "straight_length": 0.1118, + "poses": [ + [ + 0.025, + 0.05, + 1.1071487177940904 + ], + [ + 0.05, + 0.1, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 29, + "start_angle_index": 3, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.1071487177940904 + ], + [ + 0.0, + 0.0, + 1.261697920794359 + ], + [ + 0.0, + 0.0, + 1.416247123794628 + ], + [ + 0.0, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 30, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + 0.0118, + 0.05, + 1.5707963267948966 + ], + [ + 0.0, + 0.1, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 31, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.22361, + "trajectory_length": 0.20735, + "arc_length": 0.20735, + "straight_length": 0.0, + "poses": [ + [ + 0.01763, + 0.04862, + 1.3389725222944935 + ], + [ + 0.02361, + 0.1, + 1.5707963267948966 + ], + [ + 0.01763, + 0.15138, + 1.8026201312952996 + ], + [ + 0.0, + 0.2, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 32, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.33541, + "trajectory_length": 0.31102, + "arc_length": 0.31102, + "straight_length": 0.0, + "poses": [ + [ + 0.01951, + 0.04797, + 1.261697920794359 + ], + [ + 0.03141, + 0.09837, + 1.416247123794628 + ], + [ + 0.03541, + 0.15, + 1.5707963267948966 + ], + [ + 0.03141, + 0.20163, + 1.7253455297951654 + ], + [ + 0.01951, + 0.25203, + 1.879894732795434 + ], + [ + 0.0, + 0.3, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 33, + "start_angle_index": 3, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + 0.02356, + 0.04916, + 1.2442638028347375 + ], + [ + 0.02617, + 0.10294, + 1.8002291465135412 + ], + [ + 0.0, + 0.15, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 34, + "start_angle_index": 4, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.0809, + "trajectory_length": 0.15137, + "arc_length": 0.08957, + "straight_length": 0.0618, + "poses": [ + [ + 0.01523, + 0.04725, + 0.9470973974807994 + ], + [ + 0.05487, + 0.07743, + 0.4636476090008061 + ], + [ + 0.1, + 0.1, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 35, + "start_angle_index": 4, + "end_angle_index": 2, + "left_turn": false, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + 0.01356, + 0.05558, + 1.0923103255424704 + ], + [ + 0.05, + 0.1, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 36, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.5707963267948966 + ], + [ + 0.0, + 0.0, + 1.416247123794628 + ], + [ + 0.0, + 0.0, + 1.261697920794359 + ], + [ + 0.0, + 0.0, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 37, + "start_angle_index": 4, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.05, + "arc_length": 0.0, + "straight_length": 0.05, + "poses": [ + [ + 0.0, + 0.05, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 38, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.5707963267948966 + ], + [ + 0.0, + 0.0, + 1.7253455297951652 + ], + [ + 0.0, + 0.0, + 1.879894732795434 + ], + [ + 0.0, + 0.0, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 39, + "start_angle_index": 4, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + -0.01356, + 0.05558, + 2.049282328047323 + ], + [ + -0.05, + 0.1, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 40, + "start_angle_index": 4, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0809, + "trajectory_length": 0.15137, + "arc_length": 0.08957, + "straight_length": 0.0618, + "poses": [ + [ + -0.01523, + 0.04725, + 2.194495256108994 + ], + [ + -0.05487, + 0.07743, + 2.677945044588987 + ], + [ + -0.1, + 0.1, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 41, + "start_angle_index": 5, + "end_angle_index": 2, + "left_turn": false, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + -0.02356, + 0.04916, + 1.8973288507550556 + ], + [ + -0.02617, + 0.10294, + 1.341363507076252 + ], + [ + 0.0, + 0.15, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 42, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + -0.0118, + 0.05, + 1.5707963267948966 + ], + [ + 0.0, + 0.1, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 43, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.22361, + "trajectory_length": 0.20735, + "arc_length": 0.20735, + "straight_length": 0.0, + "poses": [ + [ + -0.01763, + 0.04862, + 1.8026201312952996 + ], + [ + -0.02361, + 0.1, + 1.5707963267948966 + ], + [ + -0.01763, + 0.15138, + 1.3389725222944935 + ], + [ + 0.0, + 0.2, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 44, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.33541, + "trajectory_length": 0.31102, + "arc_length": 0.31102, + "straight_length": 0.0, + "poses": [ + [ + -0.01951, + 0.04797, + 1.879894732795434 + ], + [ + -0.03141, + 0.09837, + 1.7253455297951652 + ], + [ + -0.03541, + 0.15, + 1.5707963267948966 + ], + [ + -0.03141, + 0.20163, + 1.4162471237946277 + ], + [ + -0.01951, + 0.25203, + 1.261697920794359 + ], + [ + 0.0, + 0.3, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 45, + "start_angle_index": 5, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.0344439357957027 + ], + [ + 0.0, + 0.0, + 1.8026201312952996 + ], + [ + 0.0, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 46, + "start_angle_index": 5, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.1118, + "arc_length": 0.0, + "straight_length": 0.1118, + "poses": [ + [ + -0.025, + 0.05, + 2.0344439357957027 + ], + [ + -0.05, + 0.1, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 47, + "start_angle_index": 5, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.0344439357957027 + ], + [ + 0.0, + 0.0, + 2.1953192129940238 + ], + [ + 0.0, + 0.0, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 48, + "start_angle_index": 5, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.07194, + "arc_length": 0.07194, + "straight_length": 0.0, + "poses": [ + [ + -0.05, + 0.05, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 49, + "start_angle_index": 5, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.22361, + "trajectory_length": 0.14389, + "arc_length": 0.14389, + "straight_length": 0.0, + "poses": [ + [ + -0.02587, + 0.04028, + 2.248944305393464 + ], + [ + -0.05972, + 0.07413, + 2.4634446749912255 + ], + [ + -0.1, + 0.1, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 50, + "start_angle_index": 5, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.36181, + "trajectory_length": 0.42697, + "arc_length": 0.40058, + "straight_length": 0.02639, + "poses": [ + [ + -0.02393, + 0.04093, + 2.1655647750284124 + ], + [ + -0.05301, + 0.07837, + 2.296685614261122 + ], + [ + -0.08673, + 0.11169, + 2.4278064534938313 + ], + [ + -0.12452, + 0.14031, + 2.558927292726541 + ], + [ + -0.16573, + 0.16375, + 2.6900481319592506 + ], + [ + -0.20965, + 0.18159, + 2.8211689711919603 + ], + [ + -0.25553, + 0.19355, + 2.95228981042467 + ], + [ + -0.30257, + 0.1994, + 3.0834106496573797 + ], + [ + -0.35, + 0.2, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 51, + "start_angle_index": 6, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + -0.04706, + 0.02617, + 2.9121598338711485 + ], + [ + -0.10084, + 0.02356, + 3.468125177549952 + ], + [ + -0.15, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 52, + "start_angle_index": 6, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + -0.02617, + 0.04706, + 1.8002291465135412 + ], + [ + -0.02356, + 0.10084, + 1.2442638028347377 + ], + [ + 0.0, + 0.15, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 53, + "start_angle_index": 6, + "end_angle_index": 4, + "left_turn": false, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + -0.03644, + 0.04442, + 2.049282328047323 + ], + [ + -0.05, + 0.1, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 54, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.356194490192345 + ], + [ + 0.0, + 0.0, + 2.1953192129940238 + ], + [ + 0.0, + 0.0, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 55, + "start_angle_index": 6, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.07071, + "arc_length": 0.0, + "straight_length": 0.07071, + "poses": [ + [ + -0.05, + 0.05, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 56, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.356194490192345 + ], + [ + 0.0, + 0.0, + 2.517069767390666 + ], + [ + 0.0, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 57, + "start_angle_index": 6, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + -0.04442, + 0.03644, + 2.6631066523373668 + ], + [ + -0.1, + 0.05, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 58, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + -0.05, + 0.0118, + 3.141592653589793 + ], + [ + -0.1, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 59, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.22361, + "trajectory_length": 0.20735, + "arc_length": 0.20735, + "straight_length": 0.0, + "poses": [ + [ + -0.04862, + 0.01763, + 2.90976884908939 + ], + [ + -0.1, + 0.02361, + 3.141592653589793 + ], + [ + -0.15138, + 0.01763, + 3.373416458090196 + ], + [ + -0.2, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 60, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.33541, + "trajectory_length": 0.31102, + "arc_length": 0.31102, + "straight_length": 0.0, + "poses": [ + [ + -0.04797, + 0.01951, + 2.832494247589256 + ], + [ + -0.09837, + 0.03141, + 2.9870434505895243 + ], + [ + -0.15, + 0.03541, + 3.141592653589793 + ], + [ + -0.20163, + 0.03141, + 3.296141856590062 + ], + [ + -0.25203, + 0.01951, + 3.450691059590331 + ], + [ + -0.3, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 61, + "start_angle_index": 7, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + -0.04916, + 0.02356, + 2.815060129629634 + ], + [ + -0.10294, + 0.02617, + 3.3710254733084377 + ], + [ + -0.15, + 0.0, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 62, + "start_angle_index": 7, + "end_angle_index": 4, + "left_turn": false, + "trajectory_radius": 0.36181, + "trajectory_length": 0.42697, + "arc_length": 0.40058, + "straight_length": 0.02639, + "poses": [ + [ + -0.04093, + 0.02393, + 2.5468242053562773 + ], + [ + -0.07837, + 0.05301, + 2.4157033661235676 + ], + [ + -0.11169, + 0.08673, + 2.2845825268908584 + ], + [ + -0.14031, + 0.12452, + 2.1534616876581487 + ], + [ + -0.16375, + 0.16573, + 2.022340848425439 + ], + [ + -0.18159, + 0.20965, + 1.8912200091927294 + ], + [ + -0.19355, + 0.25553, + 1.7600991699600197 + ], + [ + -0.1994, + 0.30257, + 1.62897833072731 + ], + [ + -0.2, + 0.35, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 63, + "start_angle_index": 7, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.07194, + "arc_length": 0.07194, + "straight_length": 0.0, + "poses": [ + [ + -0.05, + 0.05, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 64, + "start_angle_index": 7, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.22361, + "trajectory_length": 0.14389, + "arc_length": 0.14389, + "straight_length": 0.0, + "poses": [ + [ + -0.04028, + 0.02587, + 2.4634446749912255 + ], + [ + -0.07413, + 0.05972, + 2.248944305393464 + ], + [ + -0.1, + 0.1, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 65, + "start_angle_index": 7, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.677945044588987 + ], + [ + 0.0, + 0.0, + 2.5706948597901063 + ], + [ + 0.0, + 0.0, + 2.4634446749912255 + ], + [ + 0.0, + 0.0, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 66, + "start_angle_index": 7, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.1118, + "arc_length": 0.0, + "straight_length": 0.1118, + "poses": [ + [ + -0.05, + 0.025, + 2.677945044588987 + ], + [ + -0.1, + 0.05, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 67, + "start_angle_index": 7, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.677945044588987 + ], + [ + 0.0, + 0.0, + 2.832494247589256 + ], + [ + 0.0, + 0.0, + 2.9870434505895243 + ], + [ + 0.0, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 68, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.141592653589793 + ], + [ + 0.0, + 0.0, + 3.296141856590062 + ], + [ + 0.0, + 0.0, + 3.4506910595903304 + ], + [ + 0.0, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 69, + "start_angle_index": 8, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + -0.05558, + -0.01356, + 3.6200786548422195 + ], + [ + -0.1, + -0.05, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 70, + "start_angle_index": 8, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0809, + "trajectory_length": 0.15137, + "arc_length": 0.08957, + "straight_length": 0.0618, + "poses": [ + [ + -0.04725, + -0.01523, + 3.7652915829038904 + ], + [ + -0.07743, + -0.05487, + 4.2487413713838835 + ], + [ + -0.1, + -0.1, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 71, + "start_angle_index": 8, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.0809, + "trajectory_length": 0.15137, + "arc_length": 0.08957, + "straight_length": 0.0618, + "poses": [ + [ + -0.04725, + 0.01523, + 2.517893724275696 + ], + [ + -0.07743, + 0.05487, + 2.0344439357957027 + ], + [ + -0.1, + 0.1, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 72, + "start_angle_index": 8, + "end_angle_index": 6, + "left_turn": false, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + -0.05558, + 0.01356, + 2.6631066523373668 + ], + [ + -0.1, + 0.05, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 73, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.141592653589793 + ], + [ + 0.0, + 0.0, + 2.9870434505895243 + ], + [ + 0.0, + 0.0, + 2.832494247589256 + ], + [ + 0.0, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 74, + "start_angle_index": 8, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.05, + "arc_length": 0.0, + "straight_length": 0.05, + "poses": [ + [ + -0.05, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 75, + "start_angle_index": 9, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.1118, + "arc_length": 0.0, + "straight_length": 0.1118, + "poses": [ + [ + -0.05, + -0.025, + 3.6052402625905993 + ], + [ + -0.1, + -0.05, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 76, + "start_angle_index": 9, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.6052402625905993 + ], + [ + 0.0, + 0.0, + 3.7661155397889203 + ], + [ + 0.0, + 0.0, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 77, + "start_angle_index": 9, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.07194, + "arc_length": 0.07194, + "straight_length": 0.0, + "poses": [ + [ + -0.05, + -0.05, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 78, + "start_angle_index": 9, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.22361, + "trajectory_length": 0.14389, + "arc_length": 0.14389, + "straight_length": 0.0, + "poses": [ + [ + -0.04028, + -0.02587, + 3.8197406321883607 + ], + [ + -0.07413, + -0.05972, + 4.034241001786122 + ], + [ + -0.1, + -0.1, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 79, + "start_angle_index": 9, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.36181, + "trajectory_length": 0.42697, + "arc_length": 0.40058, + "straight_length": 0.02639, + "poses": [ + [ + -0.04093, + -0.02393, + 3.736361101823309 + ], + [ + -0.07837, + -0.05301, + 3.8674819410560186 + ], + [ + -0.11169, + -0.08673, + 3.998602780288728 + ], + [ + -0.14031, + -0.12452, + 4.129723619521437 + ], + [ + -0.16375, + -0.16573, + 4.260844458754147 + ], + [ + -0.18159, + -0.20965, + 4.391965297986857 + ], + [ + -0.19355, + -0.25553, + 4.5230861372195665 + ], + [ + -0.1994, + -0.30257, + 4.654206976452276 + ], + [ + -0.2, + -0.35, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 80, + "start_angle_index": 9, + "end_angle_index": 6, + "left_turn": false, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + -0.04916, + -0.02356, + 3.468125177549952 + ], + [ + -0.10294, + -0.02617, + 2.9121598338711485 + ], + [ + -0.15, + 0.0, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 81, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + -0.05, + -0.0118, + 3.141592653589793 + ], + [ + -0.1, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 82, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.22361, + "trajectory_length": 0.20735, + "arc_length": 0.20735, + "straight_length": 0.0, + "poses": [ + [ + -0.04862, + -0.01763, + 3.373416458090196 + ], + [ + -0.1, + -0.02361, + 3.141592653589793 + ], + [ + -0.15138, + -0.01763, + 2.90976884908939 + ], + [ + -0.2, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 83, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.33541, + "trajectory_length": 0.31102, + "arc_length": 0.31102, + "straight_length": 0.0, + "poses": [ + [ + -0.04797, + -0.01951, + 3.4506910595903304 + ], + [ + -0.09837, + -0.03141, + 3.296141856590062 + ], + [ + -0.15, + -0.03541, + 3.141592653589793 + ], + [ + -0.20163, + -0.03141, + 2.9870434505895243 + ], + [ + -0.25203, + -0.01951, + 2.8324942475892554 + ], + [ + -0.3, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 84, + "start_angle_index": 9, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.6052402625905993 + ], + [ + 0.0, + 0.0, + 3.373416458090196 + ], + [ + 0.0, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 85, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.9269908169872414 + ], + [ + 0.0, + 0.0, + 3.7661155397889203 + ], + [ + 0.0, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 86, + "start_angle_index": 10, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.07071, + "arc_length": 0.0, + "straight_length": 0.07071, + "poses": [ + [ + -0.05, + -0.05, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 87, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.9269908169872414 + ], + [ + 0.0, + 0.0, + 4.0878660941855625 + ], + [ + 0.0, + 0.0, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 88, + "start_angle_index": 10, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + -0.03644, + -0.04442, + 4.233902979132264 + ], + [ + -0.05, + -0.1, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 89, + "start_angle_index": 10, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + -0.02617, + -0.04706, + 4.4829561606660455 + ], + [ + -0.02356, + -0.10084, + 5.038921504344849 + ], + [ + 0.0, + -0.15, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 90, + "start_angle_index": 10, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + -0.04706, + -0.02617, + 3.3710254733084377 + ], + [ + -0.10084, + -0.02356, + 2.815060129629634 + ], + [ + -0.15, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 91, + "start_angle_index": 10, + "end_angle_index": 8, + "left_turn": false, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + -0.04442, + -0.03644, + 3.6200786548422195 + ], + [ + -0.1, + -0.05, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 92, + "start_angle_index": 11, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.07194, + "arc_length": 0.07194, + "straight_length": 0.0, + "poses": [ + [ + -0.05, + -0.05, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 93, + "start_angle_index": 11, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.22361, + "trajectory_length": 0.14389, + "arc_length": 0.14389, + "straight_length": 0.0, + "poses": [ + [ + -0.02587, + -0.04028, + 4.034241001786122 + ], + [ + -0.05972, + -0.07413, + 3.8197406321883607 + ], + [ + -0.1, + -0.1, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 94, + "start_angle_index": 11, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.2487413713838835 + ], + [ + 0.0, + 0.0, + 4.141491186585003 + ], + [ + 0.0, + 0.0, + 4.034241001786122 + ], + [ + 0.0, + 0.0, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 95, + "start_angle_index": 11, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.1118, + "arc_length": 0.0, + "straight_length": 0.1118, + "poses": [ + [ + -0.025, + -0.05, + 4.2487413713838835 + ], + [ + -0.05, + -0.1, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 96, + "start_angle_index": 11, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.2487413713838835 + ], + [ + 0.0, + 0.0, + 4.403290574384152 + ], + [ + 0.0, + 0.0, + 4.557839777384421 + ], + [ + 0.0, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 97, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + -0.0118, + -0.05, + 4.71238898038469 + ], + [ + 0.0, + -0.1, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 98, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.22361, + "trajectory_length": 0.20735, + "arc_length": 0.20735, + "straight_length": 0.0, + "poses": [ + [ + -0.01763, + -0.04862, + 4.480565175884287 + ], + [ + -0.02361, + -0.1, + 4.71238898038469 + ], + [ + -0.01763, + -0.15138, + 4.944212784885092 + ], + [ + 0.0, + -0.2, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 99, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.33541, + "trajectory_length": 0.31102, + "arc_length": 0.31102, + "straight_length": 0.0, + "poses": [ + [ + -0.01951, + -0.04797, + 4.403290574384152 + ], + [ + -0.03141, + -0.09837, + 4.557839777384421 + ], + [ + -0.03541, + -0.15, + 4.71238898038469 + ], + [ + -0.03141, + -0.20163, + 4.866938183384958 + ], + [ + -0.01951, + -0.25203, + 5.021487386385227 + ], + [ + 0.0, + -0.3, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 100, + "start_angle_index": 11, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + -0.02356, + -0.04916, + 4.385856456424531 + ], + [ + -0.02617, + -0.10294, + 4.941821800103334 + ], + [ + 0.0, + -0.15, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 101, + "start_angle_index": 11, + "end_angle_index": 8, + "left_turn": false, + "trajectory_radius": 0.36181, + "trajectory_length": 0.42697, + "arc_length": 0.40058, + "straight_length": 0.02639, + "poses": [ + [ + -0.02393, + -0.04093, + 4.117620532151173 + ], + [ + -0.05301, + -0.07837, + 3.986499692918464 + ], + [ + -0.08673, + -0.11169, + 3.855378853685755 + ], + [ + -0.12452, + -0.14031, + 3.7242580144530453 + ], + [ + -0.16573, + -0.16375, + 3.5931371752203356 + ], + [ + -0.20965, + -0.18159, + 3.462016335987626 + ], + [ + -0.25553, + -0.19355, + 3.3308954967549163 + ], + [ + -0.30257, + -0.1994, + 3.1997746575222066 + ], + [ + -0.35, + -0.2, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 102, + "start_angle_index": 12, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.0809, + "trajectory_length": 0.15137, + "arc_length": 0.08957, + "straight_length": 0.0618, + "poses": [ + [ + -0.01523, + -0.04725, + 4.088690051070593 + ], + [ + -0.05487, + -0.07743, + 3.6052402625905993 + ], + [ + -0.1, + -0.1, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 103, + "start_angle_index": 12, + "end_angle_index": 10, + "left_turn": false, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + -0.01356, + -0.05558, + 4.233902979132264 + ], + [ + -0.05, + -0.1, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 104, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.71238898038469 + ], + [ + 0.0, + 0.0, + 4.557839777384421 + ], + [ + 0.0, + 0.0, + 4.403290574384152 + ], + [ + 0.0, + 0.0, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 105, + "start_angle_index": 12, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.05, + "arc_length": 0.0, + "straight_length": 0.05, + "poses": [ + [ + 0.0, + -0.05, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 106, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.71238898038469 + ], + [ + 0.0, + 0.0, + 4.866938183384958 + ], + [ + 0.0, + 0.0, + 5.021487386385227 + ], + [ + 0.0, + 0.0, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 107, + "start_angle_index": 12, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + 0.01356, + -0.05558, + 5.190874981637116 + ], + [ + 0.05, + -0.1, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 108, + "start_angle_index": 12, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0809, + "trajectory_length": 0.15137, + "arc_length": 0.08957, + "straight_length": 0.0618, + "poses": [ + [ + 0.01523, + -0.04725, + 5.3360879096987865 + ], + [ + 0.05487, + -0.07743, + 5.81953769817878 + ], + [ + 0.1, + -0.1, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 109, + "start_angle_index": 13, + "end_angle_index": 10, + "left_turn": false, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + 0.02356, + -0.04916, + 5.038921504344849 + ], + [ + 0.02617, + -0.10294, + 4.4829561606660455 + ], + [ + 0.0, + -0.15, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 110, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + 0.0118, + -0.05, + 4.71238898038469 + ], + [ + 0.0, + -0.1, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 111, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.22361, + "trajectory_length": 0.20735, + "arc_length": 0.20735, + "straight_length": 0.0, + "poses": [ + [ + 0.01763, + -0.04862, + 4.944212784885092 + ], + [ + 0.02361, + -0.1, + 4.71238898038469 + ], + [ + 0.01763, + -0.15138, + 4.480565175884287 + ], + [ + 0.0, + -0.2, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 112, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.33541, + "trajectory_length": 0.31102, + "arc_length": 0.31102, + "straight_length": 0.0, + "poses": [ + [ + 0.01951, + -0.04797, + 5.021487386385227 + ], + [ + 0.03141, + -0.09837, + 4.866938183384958 + ], + [ + 0.03541, + -0.15, + 4.71238898038469 + ], + [ + 0.03141, + -0.20163, + 4.557839777384421 + ], + [ + 0.01951, + -0.25203, + 4.403290574384152 + ], + [ + 0.0, + -0.3, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 113, + "start_angle_index": 13, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.176036589385496 + ], + [ + 0.0, + 0.0, + 4.944212784885092 + ], + [ + 0.0, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 114, + "start_angle_index": 13, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.1118, + "arc_length": 0.0, + "straight_length": 0.1118, + "poses": [ + [ + 0.025, + -0.05, + 5.176036589385496 + ], + [ + 0.05, + -0.1, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 115, + "start_angle_index": 13, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.176036589385496 + ], + [ + 0.0, + 0.0, + 5.336911866583817 + ], + [ + 0.0, + 0.0, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 116, + "start_angle_index": 13, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.07194, + "arc_length": 0.07194, + "straight_length": 0.0, + "poses": [ + [ + 0.05, + -0.05, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 117, + "start_angle_index": 13, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.22361, + "trajectory_length": 0.14389, + "arc_length": 0.14389, + "straight_length": 0.0, + "poses": [ + [ + 0.02587, + -0.04028, + 5.390536958983257 + ], + [ + 0.05972, + -0.07413, + 5.605037328581019 + ], + [ + 0.1, + -0.1, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 118, + "start_angle_index": 13, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.36181, + "trajectory_length": 0.42697, + "arc_length": 0.40058, + "straight_length": 0.02639, + "poses": [ + [ + 0.02393, + -0.04093, + 5.307157428618205 + ], + [ + 0.05301, + -0.07837, + 5.438278267850915 + ], + [ + 0.08673, + -0.11169, + 5.569399107083624 + ], + [ + 0.12452, + -0.14031, + 5.7005199463163345 + ], + [ + 0.16573, + -0.16375, + 5.831640785549044 + ], + [ + 0.20965, + -0.18159, + 5.962761624781754 + ], + [ + 0.25553, + -0.19355, + 6.093882464014463 + ], + [ + 0.30257, + -0.1994, + 6.225003303247172 + ], + [ + 0.35, + -0.2, + 0.0 + ] + ] + }, + { + "trajectory_id": 119, + "start_angle_index": 14, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + 0.02617, + -0.04706, + 4.941821800103334 + ], + [ + 0.02356, + -0.10084, + 4.385856456424531 + ], + [ + 0.0, + -0.15, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 120, + "start_angle_index": 14, + "end_angle_index": 12, + "left_turn": false, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + 0.03644, + -0.04442, + 5.190874981637116 + ], + [ + 0.05, + -0.1, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 121, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.497787143782138 + ], + [ + 0.0, + 0.0, + 5.336911866583817 + ], + [ + 0.0, + 0.0, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 122, + "start_angle_index": 14, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.07071, + "arc_length": 0.0, + "straight_length": 0.07071, + "poses": [ + [ + 0.05, + -0.05, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 123, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.497787143782138 + ], + [ + 0.0, + 0.0, + 5.658662420980459 + ], + [ + 0.0, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 124, + "start_angle_index": 14, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + 0.04442, + -0.03644, + 5.80469930592716 + ], + [ + 0.1, + -0.05, + 0.0 + ] + ] + }, + { + "trajectory_id": 125, + "start_angle_index": 14, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + 0.04706, + -0.02617, + 6.053752487460941 + ], + [ + 0.10084, + -0.02356, + 0.32653252396015886 + ], + [ + 0.15, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 126, + "start_angle_index": 15, + "end_angle_index": 12, + "left_turn": false, + "trajectory_radius": 0.3618, + "trajectory_length": 0.42696, + "arc_length": 0.40057, + "straight_length": 0.02639, + "poses": [ + [ + 0.04092, + -0.02394, + 5.688416634946208 + ], + [ + 0.07836, + -0.05302, + 5.557295571713635 + ], + [ + 0.11168, + -0.08674, + 5.426174508481063 + ], + [ + 0.1403, + -0.12453, + 5.295053445248491 + ], + [ + 0.16374, + -0.16574, + 5.163932382015918 + ], + [ + 0.18159, + -0.20965, + 5.032811318783346 + ], + [ + 0.19354, + -0.25553, + 4.901690255550774 + ], + [ + 0.19939, + -0.30257, + 4.770569192318201 + ], + [ + 0.2, + -0.35, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 127, + "start_angle_index": 15, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.07194, + "arc_length": 0.07194, + "straight_length": 0.0, + "poses": [ + [ + 0.05, + -0.05, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 128, + "start_angle_index": 15, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.22361, + "trajectory_length": 0.14389, + "arc_length": 0.14389, + "straight_length": 0.0, + "poses": [ + [ + 0.04028, + -0.02587, + 5.605037328581019 + ], + [ + 0.07413, + -0.05972, + 5.390536958983257 + ], + [ + 0.1, + -0.1, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 129, + "start_angle_index": 15, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.81953769817878 + ], + [ + 0.0, + 0.0, + 5.712287513379899 + ], + [ + 0.0, + 0.0, + 5.605037328581019 + ], + [ + 0.0, + 0.0, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 130, + "start_angle_index": 15, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.1118, + "arc_length": 0.0, + "straight_length": 0.1118, + "poses": [ + [ + 0.05, + -0.025, + 5.81953769817878 + ], + [ + 0.1, + -0.05, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 131, + "start_angle_index": 15, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.81953769817878 + ], + [ + 0.0, + 0.0, + 5.9740869011790485 + ], + [ + 0.0, + 0.0, + 6.128636104179318 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 132, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + 0.05, + -0.0118, + 0.0 + ], + [ + 0.1, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 133, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.22361, + "trajectory_length": 0.20735, + "arc_length": 0.20735, + "straight_length": 0.0, + "poses": [ + [ + 0.04862, + -0.01763, + 6.051361502679184 + ], + [ + 0.1, + -0.02361, + 0.0 + ], + [ + 0.15138, + -0.01763, + 0.23182380450040302 + ], + [ + 0.2, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 134, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.33541, + "trajectory_length": 0.31102, + "arc_length": 0.31102, + "straight_length": 0.0, + "poses": [ + [ + 0.04797, + -0.01951, + 5.9740869011790485 + ], + [ + 0.09837, + -0.03141, + 6.128636104179318 + ], + [ + 0.15, + -0.03541, + 0.0 + ], + [ + 0.20163, + -0.03141, + 0.1545492030002687 + ], + [ + 0.25203, + -0.01951, + 0.30909840600053734 + ], + [ + 0.3, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 135, + "start_angle_index": 15, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + 0.04916, + -0.02356, + 5.956652783219427 + ], + [ + 0.10294, + -0.02617, + 0.22943281971864465 + ], + [ + 0.15, + 0.0, + 0.7853981633974483 + ] + ] + } + ] +} \ No newline at end of file diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output1.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output1.json new file mode 100644 index 00000000000..dfff481543d --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output1.json @@ -0,0 +1,3864 @@ +{ + "version": 1.0, + "date_generated": "2025-10-30", + "lattice_metadata": { + "motion_model": "diff", + "turning_radius": 0.08, + "grid_resolution": 0.05, + "stopping_threshold": 5, + "num_of_headings": 16, + "heading_angles": [ + 0.0, + 0.4636476090008061, + 0.7853981633974483, + 1.1071487177940904, + 1.5707963267948966, + 2.0344439357957027, + 2.356194490192345, + 2.677945044588987, + 3.141592653589793, + 3.6052402625905993, + 3.9269908169872414, + 4.2487413713838835, + 4.71238898038469, + 5.176036589385496, + 5.497787143782138, + 5.81953769817878 + ], + "number_of_trajectories": 136 + }, + "primitives": [ + { + "trajectory_id": 0, + "start_angle_index": 0, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.0809, + "trajectory_length": 0.15137, + "arc_length": 0.08957, + "straight_length": 0.0618, + "poses": [ + [ + 0.04725, + -0.01523, + 5.659486377865489 + ], + [ + 0.07743, + -0.05487, + 5.176036589385496 + ], + [ + 0.1, + -0.1, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 1, + "start_angle_index": 0, + "end_angle_index": 14, + "left_turn": false, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + 0.05558, + -0.01356, + 5.80469930592716 + ], + [ + 0.1, + -0.05, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 2, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 6.128636104179318 + ], + [ + 0.0, + 0.0, + 5.9740869011790485 + ], + [ + 0.0, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 3, + "start_angle_index": 0, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.05, + "arc_length": 0.0, + "straight_length": 0.05, + "poses": [ + [ + 0.05, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 4, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.1545492030002687 + ], + [ + 0.0, + 0.0, + 0.3090984060005374 + ], + [ + 0.0, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 5, + "start_angle_index": 0, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + 0.05558, + 0.01356, + 0.47848600125242624 + ], + [ + 0.1, + 0.05, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 6, + "start_angle_index": 0, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0809, + "trajectory_length": 0.15137, + "arc_length": 0.08957, + "straight_length": 0.0618, + "poses": [ + [ + 0.04725, + 0.01523, + 0.6236989293140972 + ], + [ + 0.07743, + 0.05487, + 1.1071487177940904 + ], + [ + 0.1, + 0.1, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 7, + "start_angle_index": 1, + "end_angle_index": 14, + "left_turn": false, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + 0.04916, + 0.02356, + 0.32653252396015897 + ], + [ + 0.10294, + 0.02617, + 6.053752487460941 + ], + [ + 0.15, + 0.0, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 8, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + 0.05, + 0.0118, + 0.0 + ], + [ + 0.1, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 9, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.22361, + "trajectory_length": 0.20735, + "arc_length": 0.20735, + "straight_length": 0.0, + "poses": [ + [ + 0.04862, + 0.01763, + 0.23182380450040305 + ], + [ + 0.1, + 0.02361, + 0.0 + ], + [ + 0.15138, + 0.01763, + 6.051361502679184 + ], + [ + 0.2, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 10, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.33541, + "trajectory_length": 0.31102, + "arc_length": 0.31102, + "straight_length": 0.0, + "poses": [ + [ + 0.04797, + 0.01951, + 0.3090984060005374 + ], + [ + 0.09837, + 0.03141, + 0.1545492030002687 + ], + [ + 0.15, + 0.03541, + 0.0 + ], + [ + 0.20163, + 0.03141, + 6.128636104179318 + ], + [ + 0.25203, + 0.01951, + 5.9740869011790485 + ], + [ + 0.3, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 11, + "start_angle_index": 1, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.4636476090008061 + ], + [ + 0.0, + 0.0, + 0.23182380450040305 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 12, + "start_angle_index": 1, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.1118, + "arc_length": 0.0, + "straight_length": 0.1118, + "poses": [ + [ + 0.05, + 0.025, + 0.4636476090008061 + ], + [ + 0.1, + 0.05, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 13, + "start_angle_index": 1, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.4636476090008061 + ], + [ + 0.0, + 0.0, + 0.6245228861991272 + ], + [ + 0.0, + 0.0, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 14, + "start_angle_index": 1, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.07194, + "arc_length": 0.07194, + "straight_length": 0.0, + "poses": [ + [ + 0.05, + 0.05, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 15, + "start_angle_index": 1, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.22361, + "trajectory_length": 0.14389, + "arc_length": 0.14389, + "straight_length": 0.0, + "poses": [ + [ + 0.04028, + 0.02587, + 0.6781479785985676 + ], + [ + 0.07413, + 0.05972, + 0.892648348196329 + ], + [ + 0.1, + 0.1, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 16, + "start_angle_index": 1, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.3618, + "trajectory_length": 0.42696, + "arc_length": 0.40057, + "straight_length": 0.02639, + "poses": [ + [ + 0.04092, + 0.02394, + 0.5947686722333784 + ], + [ + 0.07836, + 0.05302, + 0.7258897354659507 + ], + [ + 0.11168, + 0.08674, + 0.8570107986985229 + ], + [ + 0.1403, + 0.12453, + 0.9881318619310953 + ], + [ + 0.16374, + 0.16574, + 1.1192529251636676 + ], + [ + 0.18159, + 0.20965, + 1.25037398839624 + ], + [ + 0.19354, + 0.25553, + 1.3814950516288125 + ], + [ + 0.19939, + 0.30257, + 1.512616114861385 + ], + [ + 0.2, + 0.35, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 17, + "start_angle_index": 2, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + 0.04706, + 0.02617, + 0.2294328197186447 + ], + [ + 0.10084, + 0.02356, + 5.956652783219427 + ], + [ + 0.15, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 18, + "start_angle_index": 2, + "end_angle_index": 0, + "left_turn": false, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + 0.04442, + 0.03644, + 0.4784860012524262 + ], + [ + 0.1, + 0.05, + 0.0 + ] + ] + }, + { + "trajectory_id": 19, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.7853981633974483 + ], + [ + 0.0, + 0.0, + 0.6245228861991272 + ], + [ + 0.0, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 20, + "start_angle_index": 2, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.07071, + "arc_length": 0.0, + "straight_length": 0.07071, + "poses": [ + [ + 0.05, + 0.05, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 21, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.7853981633974483 + ], + [ + 0.0, + 0.0, + 0.9462734405957693 + ], + [ + 0.0, + 0.0, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 22, + "start_angle_index": 2, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + 0.03644, + 0.04442, + 1.0923103255424704 + ], + [ + 0.05, + 0.1, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 23, + "start_angle_index": 2, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + 0.02617, + 0.04706, + 1.341363507076252 + ], + [ + 0.02356, + 0.10084, + 1.8973288507550554 + ], + [ + 0.0, + 0.15, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 24, + "start_angle_index": 3, + "end_angle_index": 0, + "left_turn": false, + "trajectory_radius": 0.36181, + "trajectory_length": 0.42697, + "arc_length": 0.40058, + "straight_length": 0.02639, + "poses": [ + [ + 0.02393, + 0.04093, + 0.9760278785613808 + ], + [ + 0.05301, + 0.07837, + 0.8449070393286713 + ], + [ + 0.08673, + 0.11169, + 0.7137862000959616 + ], + [ + 0.12452, + 0.14031, + 0.582665360863252 + ], + [ + 0.16573, + 0.16375, + 0.4515445216305424 + ], + [ + 0.20965, + 0.18159, + 0.3204236823978327 + ], + [ + 0.25553, + 0.19355, + 0.18930284316512325 + ], + [ + 0.30257, + 0.1994, + 0.05818200393241346 + ], + [ + 0.35, + 0.2, + 0.0 + ] + ] + }, + { + "trajectory_id": 25, + "start_angle_index": 3, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.07194, + "arc_length": 0.07194, + "straight_length": 0.0, + "poses": [ + [ + 0.05, + 0.05, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 26, + "start_angle_index": 3, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.22361, + "trajectory_length": 0.14389, + "arc_length": 0.14389, + "straight_length": 0.0, + "poses": [ + [ + 0.02587, + 0.04028, + 0.892648348196329 + ], + [ + 0.05972, + 0.07413, + 0.6781479785985676 + ], + [ + 0.1, + 0.1, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 27, + "start_angle_index": 3, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.1071487177940904 + ], + [ + 0.0, + 0.0, + 0.9998985329952097 + ], + [ + 0.0, + 0.0, + 0.892648348196329 + ], + [ + 0.0, + 0.0, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 28, + "start_angle_index": 3, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.1118, + "arc_length": 0.0, + "straight_length": 0.1118, + "poses": [ + [ + 0.025, + 0.05, + 1.1071487177940904 + ], + [ + 0.05, + 0.1, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 29, + "start_angle_index": 3, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.1071487177940904 + ], + [ + 0.0, + 0.0, + 1.261697920794359 + ], + [ + 0.0, + 0.0, + 1.416247123794628 + ], + [ + 0.0, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 30, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + 0.0118, + 0.05, + 1.5707963267948966 + ], + [ + 0.0, + 0.1, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 31, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.22361, + "trajectory_length": 0.20735, + "arc_length": 0.20735, + "straight_length": 0.0, + "poses": [ + [ + 0.01763, + 0.04862, + 1.3389725222944935 + ], + [ + 0.02361, + 0.1, + 1.5707963267948966 + ], + [ + 0.01763, + 0.15138, + 1.8026201312952996 + ], + [ + 0.0, + 0.2, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 32, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.33541, + "trajectory_length": 0.31102, + "arc_length": 0.31102, + "straight_length": 0.0, + "poses": [ + [ + 0.01951, + 0.04797, + 1.261697920794359 + ], + [ + 0.03141, + 0.09837, + 1.416247123794628 + ], + [ + 0.03541, + 0.15, + 1.5707963267948966 + ], + [ + 0.03141, + 0.20163, + 1.7253455297951654 + ], + [ + 0.01951, + 0.25203, + 1.879894732795434 + ], + [ + 0.0, + 0.3, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 33, + "start_angle_index": 3, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + 0.02356, + 0.04916, + 1.2442638028347375 + ], + [ + 0.02617, + 0.10294, + 1.8002291465135412 + ], + [ + 0.0, + 0.15, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 34, + "start_angle_index": 4, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.0809, + "trajectory_length": 0.15137, + "arc_length": 0.08957, + "straight_length": 0.0618, + "poses": [ + [ + 0.01523, + 0.04725, + 0.9470973974807994 + ], + [ + 0.05487, + 0.07743, + 0.4636476090008061 + ], + [ + 0.1, + 0.1, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 35, + "start_angle_index": 4, + "end_angle_index": 2, + "left_turn": false, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + 0.01356, + 0.05558, + 1.0923103255424704 + ], + [ + 0.05, + 0.1, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 36, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.5707963267948966 + ], + [ + 0.0, + 0.0, + 1.416247123794628 + ], + [ + 0.0, + 0.0, + 1.261697920794359 + ], + [ + 0.0, + 0.0, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 37, + "start_angle_index": 4, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.05, + "arc_length": 0.0, + "straight_length": 0.05, + "poses": [ + [ + 0.0, + 0.05, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 38, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.5707963267948966 + ], + [ + 0.0, + 0.0, + 1.7253455297951652 + ], + [ + 0.0, + 0.0, + 1.879894732795434 + ], + [ + 0.0, + 0.0, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 39, + "start_angle_index": 4, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + -0.01356, + 0.05558, + 2.049282328047323 + ], + [ + -0.05, + 0.1, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 40, + "start_angle_index": 4, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0809, + "trajectory_length": 0.15137, + "arc_length": 0.08957, + "straight_length": 0.0618, + "poses": [ + [ + -0.01523, + 0.04725, + 2.194495256108994 + ], + [ + -0.05487, + 0.07743, + 2.677945044588987 + ], + [ + -0.1, + 0.1, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 41, + "start_angle_index": 5, + "end_angle_index": 2, + "left_turn": false, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + -0.02356, + 0.04916, + 1.8973288507550556 + ], + [ + -0.02617, + 0.10294, + 1.341363507076252 + ], + [ + 0.0, + 0.15, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 42, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + -0.0118, + 0.05, + 1.5707963267948966 + ], + [ + 0.0, + 0.1, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 43, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.22361, + "trajectory_length": 0.20735, + "arc_length": 0.20735, + "straight_length": 0.0, + "poses": [ + [ + -0.01763, + 0.04862, + 1.8026201312952996 + ], + [ + -0.02361, + 0.1, + 1.5707963267948966 + ], + [ + -0.01763, + 0.15138, + 1.3389725222944935 + ], + [ + 0.0, + 0.2, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 44, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.33541, + "trajectory_length": 0.31102, + "arc_length": 0.31102, + "straight_length": 0.0, + "poses": [ + [ + -0.01951, + 0.04797, + 1.879894732795434 + ], + [ + -0.03141, + 0.09837, + 1.7253455297951652 + ], + [ + -0.03541, + 0.15, + 1.5707963267948966 + ], + [ + -0.03141, + 0.20163, + 1.4162471237946277 + ], + [ + -0.01951, + 0.25203, + 1.261697920794359 + ], + [ + 0.0, + 0.3, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 45, + "start_angle_index": 5, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.0344439357957027 + ], + [ + 0.0, + 0.0, + 1.8026201312952996 + ], + [ + 0.0, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 46, + "start_angle_index": 5, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.1118, + "arc_length": 0.0, + "straight_length": 0.1118, + "poses": [ + [ + -0.025, + 0.05, + 2.0344439357957027 + ], + [ + -0.05, + 0.1, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 47, + "start_angle_index": 5, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.0344439357957027 + ], + [ + 0.0, + 0.0, + 2.1953192129940238 + ], + [ + 0.0, + 0.0, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 48, + "start_angle_index": 5, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.07194, + "arc_length": 0.07194, + "straight_length": 0.0, + "poses": [ + [ + -0.05, + 0.05, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 49, + "start_angle_index": 5, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.22361, + "trajectory_length": 0.14389, + "arc_length": 0.14389, + "straight_length": 0.0, + "poses": [ + [ + -0.02587, + 0.04028, + 2.248944305393464 + ], + [ + -0.05972, + 0.07413, + 2.4634446749912255 + ], + [ + -0.1, + 0.1, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 50, + "start_angle_index": 5, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.36181, + "trajectory_length": 0.42697, + "arc_length": 0.40058, + "straight_length": 0.02639, + "poses": [ + [ + -0.02393, + 0.04093, + 2.1655647750284124 + ], + [ + -0.05301, + 0.07837, + 2.296685614261122 + ], + [ + -0.08673, + 0.11169, + 2.4278064534938313 + ], + [ + -0.12452, + 0.14031, + 2.558927292726541 + ], + [ + -0.16573, + 0.16375, + 2.6900481319592506 + ], + [ + -0.20965, + 0.18159, + 2.8211689711919603 + ], + [ + -0.25553, + 0.19355, + 2.95228981042467 + ], + [ + -0.30257, + 0.1994, + 3.0834106496573797 + ], + [ + -0.35, + 0.2, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 51, + "start_angle_index": 6, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + -0.04706, + 0.02617, + 2.9121598338711485 + ], + [ + -0.10084, + 0.02356, + 3.468125177549952 + ], + [ + -0.15, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 52, + "start_angle_index": 6, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + -0.02617, + 0.04706, + 1.8002291465135412 + ], + [ + -0.02356, + 0.10084, + 1.2442638028347377 + ], + [ + 0.0, + 0.15, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 53, + "start_angle_index": 6, + "end_angle_index": 4, + "left_turn": false, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + -0.03644, + 0.04442, + 2.049282328047323 + ], + [ + -0.05, + 0.1, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 54, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.356194490192345 + ], + [ + 0.0, + 0.0, + 2.1953192129940238 + ], + [ + 0.0, + 0.0, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 55, + "start_angle_index": 6, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.07071, + "arc_length": 0.0, + "straight_length": 0.07071, + "poses": [ + [ + -0.05, + 0.05, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 56, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.356194490192345 + ], + [ + 0.0, + 0.0, + 2.517069767390666 + ], + [ + 0.0, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 57, + "start_angle_index": 6, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + -0.04442, + 0.03644, + 2.6631066523373668 + ], + [ + -0.1, + 0.05, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 58, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + -0.05, + 0.0118, + 3.141592653589793 + ], + [ + -0.1, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 59, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.22361, + "trajectory_length": 0.20735, + "arc_length": 0.20735, + "straight_length": 0.0, + "poses": [ + [ + -0.04862, + 0.01763, + 2.90976884908939 + ], + [ + -0.1, + 0.02361, + 3.141592653589793 + ], + [ + -0.15138, + 0.01763, + 3.373416458090196 + ], + [ + -0.2, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 60, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.33541, + "trajectory_length": 0.31102, + "arc_length": 0.31102, + "straight_length": 0.0, + "poses": [ + [ + -0.04797, + 0.01951, + 2.832494247589256 + ], + [ + -0.09837, + 0.03141, + 2.9870434505895243 + ], + [ + -0.15, + 0.03541, + 3.141592653589793 + ], + [ + -0.20163, + 0.03141, + 3.296141856590062 + ], + [ + -0.25203, + 0.01951, + 3.450691059590331 + ], + [ + -0.3, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 61, + "start_angle_index": 7, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + -0.04916, + 0.02356, + 2.815060129629634 + ], + [ + -0.10294, + 0.02617, + 3.3710254733084377 + ], + [ + -0.15, + 0.0, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 62, + "start_angle_index": 7, + "end_angle_index": 4, + "left_turn": false, + "trajectory_radius": 0.36181, + "trajectory_length": 0.42697, + "arc_length": 0.40058, + "straight_length": 0.02639, + "poses": [ + [ + -0.04093, + 0.02393, + 2.5468242053562773 + ], + [ + -0.07837, + 0.05301, + 2.4157033661235676 + ], + [ + -0.11169, + 0.08673, + 2.2845825268908584 + ], + [ + -0.14031, + 0.12452, + 2.1534616876581487 + ], + [ + -0.16375, + 0.16573, + 2.022340848425439 + ], + [ + -0.18159, + 0.20965, + 1.8912200091927294 + ], + [ + -0.19355, + 0.25553, + 1.7600991699600197 + ], + [ + -0.1994, + 0.30257, + 1.62897833072731 + ], + [ + -0.2, + 0.35, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 63, + "start_angle_index": 7, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.07194, + "arc_length": 0.07194, + "straight_length": 0.0, + "poses": [ + [ + -0.05, + 0.05, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 64, + "start_angle_index": 7, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.22361, + "trajectory_length": 0.14389, + "arc_length": 0.14389, + "straight_length": 0.0, + "poses": [ + [ + -0.04028, + 0.02587, + 2.4634446749912255 + ], + [ + -0.07413, + 0.05972, + 2.248944305393464 + ], + [ + -0.1, + 0.1, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 65, + "start_angle_index": 7, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.677945044588987 + ], + [ + 0.0, + 0.0, + 2.5706948597901063 + ], + [ + 0.0, + 0.0, + 2.4634446749912255 + ], + [ + 0.0, + 0.0, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 66, + "start_angle_index": 7, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.1118, + "arc_length": 0.0, + "straight_length": 0.1118, + "poses": [ + [ + -0.05, + 0.025, + 2.677945044588987 + ], + [ + -0.1, + 0.05, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 67, + "start_angle_index": 7, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.677945044588987 + ], + [ + 0.0, + 0.0, + 2.832494247589256 + ], + [ + 0.0, + 0.0, + 2.9870434505895243 + ], + [ + 0.0, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 68, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.141592653589793 + ], + [ + 0.0, + 0.0, + 3.296141856590062 + ], + [ + 0.0, + 0.0, + 3.4506910595903304 + ], + [ + 0.0, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 69, + "start_angle_index": 8, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + -0.05558, + -0.01356, + 3.6200786548422195 + ], + [ + -0.1, + -0.05, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 70, + "start_angle_index": 8, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0809, + "trajectory_length": 0.15137, + "arc_length": 0.08957, + "straight_length": 0.0618, + "poses": [ + [ + -0.04725, + -0.01523, + 3.7652915829038904 + ], + [ + -0.07743, + -0.05487, + 4.2487413713838835 + ], + [ + -0.1, + -0.1, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 71, + "start_angle_index": 8, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.0809, + "trajectory_length": 0.15137, + "arc_length": 0.08957, + "straight_length": 0.0618, + "poses": [ + [ + -0.04725, + 0.01523, + 2.517893724275696 + ], + [ + -0.07743, + 0.05487, + 2.0344439357957027 + ], + [ + -0.1, + 0.1, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 72, + "start_angle_index": 8, + "end_angle_index": 6, + "left_turn": false, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + -0.05558, + 0.01356, + 2.6631066523373668 + ], + [ + -0.1, + 0.05, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 73, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.141592653589793 + ], + [ + 0.0, + 0.0, + 2.9870434505895243 + ], + [ + 0.0, + 0.0, + 2.832494247589256 + ], + [ + 0.0, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 74, + "start_angle_index": 8, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.05, + "arc_length": 0.0, + "straight_length": 0.05, + "poses": [ + [ + -0.05, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 75, + "start_angle_index": 9, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.1118, + "arc_length": 0.0, + "straight_length": 0.1118, + "poses": [ + [ + -0.05, + -0.025, + 3.6052402625905993 + ], + [ + -0.1, + -0.05, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 76, + "start_angle_index": 9, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.6052402625905993 + ], + [ + 0.0, + 0.0, + 3.7661155397889203 + ], + [ + 0.0, + 0.0, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 77, + "start_angle_index": 9, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.07194, + "arc_length": 0.07194, + "straight_length": 0.0, + "poses": [ + [ + -0.05, + -0.05, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 78, + "start_angle_index": 9, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.22361, + "trajectory_length": 0.14389, + "arc_length": 0.14389, + "straight_length": 0.0, + "poses": [ + [ + -0.04028, + -0.02587, + 3.8197406321883607 + ], + [ + -0.07413, + -0.05972, + 4.034241001786122 + ], + [ + -0.1, + -0.1, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 79, + "start_angle_index": 9, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.36181, + "trajectory_length": 0.42697, + "arc_length": 0.40058, + "straight_length": 0.02639, + "poses": [ + [ + -0.04093, + -0.02393, + 3.736361101823309 + ], + [ + -0.07837, + -0.05301, + 3.8674819410560186 + ], + [ + -0.11169, + -0.08673, + 3.998602780288728 + ], + [ + -0.14031, + -0.12452, + 4.129723619521437 + ], + [ + -0.16375, + -0.16573, + 4.260844458754147 + ], + [ + -0.18159, + -0.20965, + 4.391965297986857 + ], + [ + -0.19355, + -0.25553, + 4.5230861372195665 + ], + [ + -0.1994, + -0.30257, + 4.654206976452276 + ], + [ + -0.2, + -0.35, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 80, + "start_angle_index": 9, + "end_angle_index": 6, + "left_turn": false, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + -0.04916, + -0.02356, + 3.468125177549952 + ], + [ + -0.10294, + -0.02617, + 2.9121598338711485 + ], + [ + -0.15, + 0.0, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 81, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + -0.05, + -0.0118, + 3.141592653589793 + ], + [ + -0.1, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 82, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.22361, + "trajectory_length": 0.20735, + "arc_length": 0.20735, + "straight_length": 0.0, + "poses": [ + [ + -0.04862, + -0.01763, + 3.373416458090196 + ], + [ + -0.1, + -0.02361, + 3.141592653589793 + ], + [ + -0.15138, + -0.01763, + 2.90976884908939 + ], + [ + -0.2, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 83, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.33541, + "trajectory_length": 0.31102, + "arc_length": 0.31102, + "straight_length": 0.0, + "poses": [ + [ + -0.04797, + -0.01951, + 3.4506910595903304 + ], + [ + -0.09837, + -0.03141, + 3.296141856590062 + ], + [ + -0.15, + -0.03541, + 3.141592653589793 + ], + [ + -0.20163, + -0.03141, + 2.9870434505895243 + ], + [ + -0.25203, + -0.01951, + 2.8324942475892554 + ], + [ + -0.3, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 84, + "start_angle_index": 9, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.6052402625905993 + ], + [ + 0.0, + 0.0, + 3.373416458090196 + ], + [ + 0.0, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 85, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.9269908169872414 + ], + [ + 0.0, + 0.0, + 3.7661155397889203 + ], + [ + 0.0, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 86, + "start_angle_index": 10, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.07071, + "arc_length": 0.0, + "straight_length": 0.07071, + "poses": [ + [ + -0.05, + -0.05, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 87, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.9269908169872414 + ], + [ + 0.0, + 0.0, + 4.0878660941855625 + ], + [ + 0.0, + 0.0, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 88, + "start_angle_index": 10, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + -0.03644, + -0.04442, + 4.233902979132264 + ], + [ + -0.05, + -0.1, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 89, + "start_angle_index": 10, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + -0.02617, + -0.04706, + 4.4829561606660455 + ], + [ + -0.02356, + -0.10084, + 5.038921504344849 + ], + [ + 0.0, + -0.15, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 90, + "start_angle_index": 10, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + -0.04706, + -0.02617, + 3.3710254733084377 + ], + [ + -0.10084, + -0.02356, + 2.815060129629634 + ], + [ + -0.15, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 91, + "start_angle_index": 10, + "end_angle_index": 8, + "left_turn": false, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + -0.04442, + -0.03644, + 3.6200786548422195 + ], + [ + -0.1, + -0.05, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 92, + "start_angle_index": 11, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.07194, + "arc_length": 0.07194, + "straight_length": 0.0, + "poses": [ + [ + -0.05, + -0.05, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 93, + "start_angle_index": 11, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.22361, + "trajectory_length": 0.14389, + "arc_length": 0.14389, + "straight_length": 0.0, + "poses": [ + [ + -0.02587, + -0.04028, + 4.034241001786122 + ], + [ + -0.05972, + -0.07413, + 3.8197406321883607 + ], + [ + -0.1, + -0.1, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 94, + "start_angle_index": 11, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.2487413713838835 + ], + [ + 0.0, + 0.0, + 4.141491186585003 + ], + [ + 0.0, + 0.0, + 4.034241001786122 + ], + [ + 0.0, + 0.0, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 95, + "start_angle_index": 11, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.1118, + "arc_length": 0.0, + "straight_length": 0.1118, + "poses": [ + [ + -0.025, + -0.05, + 4.2487413713838835 + ], + [ + -0.05, + -0.1, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 96, + "start_angle_index": 11, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.2487413713838835 + ], + [ + 0.0, + 0.0, + 4.403290574384152 + ], + [ + 0.0, + 0.0, + 4.557839777384421 + ], + [ + 0.0, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 97, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + -0.0118, + -0.05, + 4.71238898038469 + ], + [ + 0.0, + -0.1, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 98, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.22361, + "trajectory_length": 0.20735, + "arc_length": 0.20735, + "straight_length": 0.0, + "poses": [ + [ + -0.01763, + -0.04862, + 4.480565175884287 + ], + [ + -0.02361, + -0.1, + 4.71238898038469 + ], + [ + -0.01763, + -0.15138, + 4.944212784885092 + ], + [ + 0.0, + -0.2, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 99, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.33541, + "trajectory_length": 0.31102, + "arc_length": 0.31102, + "straight_length": 0.0, + "poses": [ + [ + -0.01951, + -0.04797, + 4.403290574384152 + ], + [ + -0.03141, + -0.09837, + 4.557839777384421 + ], + [ + -0.03541, + -0.15, + 4.71238898038469 + ], + [ + -0.03141, + -0.20163, + 4.866938183384958 + ], + [ + -0.01951, + -0.25203, + 5.021487386385227 + ], + [ + 0.0, + -0.3, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 100, + "start_angle_index": 11, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + -0.02356, + -0.04916, + 4.385856456424531 + ], + [ + -0.02617, + -0.10294, + 4.941821800103334 + ], + [ + 0.0, + -0.15, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 101, + "start_angle_index": 11, + "end_angle_index": 8, + "left_turn": false, + "trajectory_radius": 0.36181, + "trajectory_length": 0.42697, + "arc_length": 0.40058, + "straight_length": 0.02639, + "poses": [ + [ + -0.02393, + -0.04093, + 4.117620532151173 + ], + [ + -0.05301, + -0.07837, + 3.986499692918464 + ], + [ + -0.08673, + -0.11169, + 3.855378853685755 + ], + [ + -0.12452, + -0.14031, + 3.7242580144530453 + ], + [ + -0.16573, + -0.16375, + 3.5931371752203356 + ], + [ + -0.20965, + -0.18159, + 3.462016335987626 + ], + [ + -0.25553, + -0.19355, + 3.3308954967549163 + ], + [ + -0.30257, + -0.1994, + 3.1997746575222066 + ], + [ + -0.35, + -0.2, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 102, + "start_angle_index": 12, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.0809, + "trajectory_length": 0.15137, + "arc_length": 0.08957, + "straight_length": 0.0618, + "poses": [ + [ + -0.01523, + -0.04725, + 4.088690051070593 + ], + [ + -0.05487, + -0.07743, + 3.6052402625905993 + ], + [ + -0.1, + -0.1, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 103, + "start_angle_index": 12, + "end_angle_index": 10, + "left_turn": false, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + -0.01356, + -0.05558, + 4.233902979132264 + ], + [ + -0.05, + -0.1, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 104, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.71238898038469 + ], + [ + 0.0, + 0.0, + 4.557839777384421 + ], + [ + 0.0, + 0.0, + 4.403290574384152 + ], + [ + 0.0, + 0.0, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 105, + "start_angle_index": 12, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.05, + "arc_length": 0.0, + "straight_length": 0.05, + "poses": [ + [ + 0.0, + -0.05, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 106, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.71238898038469 + ], + [ + 0.0, + 0.0, + 4.866938183384958 + ], + [ + 0.0, + 0.0, + 5.021487386385227 + ], + [ + 0.0, + 0.0, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 107, + "start_angle_index": 12, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + 0.01356, + -0.05558, + 5.190874981637116 + ], + [ + 0.05, + -0.1, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 108, + "start_angle_index": 12, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0809, + "trajectory_length": 0.15137, + "arc_length": 0.08957, + "straight_length": 0.0618, + "poses": [ + [ + 0.01523, + -0.04725, + 5.3360879096987865 + ], + [ + 0.05487, + -0.07743, + 5.81953769817878 + ], + [ + 0.1, + -0.1, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 109, + "start_angle_index": 13, + "end_angle_index": 10, + "left_turn": false, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + 0.02356, + -0.04916, + 5.038921504344849 + ], + [ + 0.02617, + -0.10294, + 4.4829561606660455 + ], + [ + 0.0, + -0.15, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 110, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + 0.0118, + -0.05, + 4.71238898038469 + ], + [ + 0.0, + -0.1, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 111, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.22361, + "trajectory_length": 0.20735, + "arc_length": 0.20735, + "straight_length": 0.0, + "poses": [ + [ + 0.01763, + -0.04862, + 4.944212784885092 + ], + [ + 0.02361, + -0.1, + 4.71238898038469 + ], + [ + 0.01763, + -0.15138, + 4.480565175884287 + ], + [ + 0.0, + -0.2, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 112, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.33541, + "trajectory_length": 0.31102, + "arc_length": 0.31102, + "straight_length": 0.0, + "poses": [ + [ + 0.01951, + -0.04797, + 5.021487386385227 + ], + [ + 0.03141, + -0.09837, + 4.866938183384958 + ], + [ + 0.03541, + -0.15, + 4.71238898038469 + ], + [ + 0.03141, + -0.20163, + 4.557839777384421 + ], + [ + 0.01951, + -0.25203, + 4.403290574384152 + ], + [ + 0.0, + -0.3, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 113, + "start_angle_index": 13, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.176036589385496 + ], + [ + 0.0, + 0.0, + 4.944212784885092 + ], + [ + 0.0, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 114, + "start_angle_index": 13, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.1118, + "arc_length": 0.0, + "straight_length": 0.1118, + "poses": [ + [ + 0.025, + -0.05, + 5.176036589385496 + ], + [ + 0.05, + -0.1, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 115, + "start_angle_index": 13, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.176036589385496 + ], + [ + 0.0, + 0.0, + 5.336911866583817 + ], + [ + 0.0, + 0.0, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 116, + "start_angle_index": 13, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.07194, + "arc_length": 0.07194, + "straight_length": 0.0, + "poses": [ + [ + 0.05, + -0.05, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 117, + "start_angle_index": 13, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.22361, + "trajectory_length": 0.14389, + "arc_length": 0.14389, + "straight_length": 0.0, + "poses": [ + [ + 0.02587, + -0.04028, + 5.390536958983257 + ], + [ + 0.05972, + -0.07413, + 5.605037328581019 + ], + [ + 0.1, + -0.1, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 118, + "start_angle_index": 13, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.36181, + "trajectory_length": 0.42697, + "arc_length": 0.40058, + "straight_length": 0.02639, + "poses": [ + [ + 0.02393, + -0.04093, + 5.307157428618205 + ], + [ + 0.05301, + -0.07837, + 5.438278267850915 + ], + [ + 0.08673, + -0.11169, + 5.569399107083624 + ], + [ + 0.12452, + -0.14031, + 5.7005199463163345 + ], + [ + 0.16573, + -0.16375, + 5.831640785549044 + ], + [ + 0.20965, + -0.18159, + 5.962761624781754 + ], + [ + 0.25553, + -0.19355, + 6.093882464014463 + ], + [ + 0.30257, + -0.1994, + 6.225003303247172 + ], + [ + 0.35, + -0.2, + 0.0 + ] + ] + }, + { + "trajectory_id": 119, + "start_angle_index": 14, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + 0.02617, + -0.04706, + 4.941821800103334 + ], + [ + 0.02356, + -0.10084, + 4.385856456424531 + ], + [ + 0.0, + -0.15, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 120, + "start_angle_index": 14, + "end_angle_index": 12, + "left_turn": false, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + 0.03644, + -0.04442, + 5.190874981637116 + ], + [ + 0.05, + -0.1, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 121, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.497787143782138 + ], + [ + 0.0, + 0.0, + 5.336911866583817 + ], + [ + 0.0, + 0.0, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 122, + "start_angle_index": 14, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.07071, + "arc_length": 0.0, + "straight_length": 0.07071, + "poses": [ + [ + 0.05, + -0.05, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 123, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.497787143782138 + ], + [ + 0.0, + 0.0, + 5.658662420980459 + ], + [ + 0.0, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 124, + "start_angle_index": 14, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.12071, + "trajectory_length": 0.11552, + "arc_length": 0.09481, + "straight_length": 0.02071, + "poses": [ + [ + 0.04442, + -0.03644, + 5.80469930592716 + ], + [ + 0.1, + -0.05, + 0.0 + ] + ] + }, + { + "trajectory_id": 125, + "start_angle_index": 14, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + 0.04706, + -0.02617, + 6.053752487460941 + ], + [ + 0.10084, + -0.02356, + 0.32653252396015886 + ], + [ + 0.15, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 126, + "start_angle_index": 15, + "end_angle_index": 12, + "left_turn": false, + "trajectory_radius": 0.3618, + "trajectory_length": 0.42696, + "arc_length": 0.40057, + "straight_length": 0.02639, + "poses": [ + [ + 0.04092, + -0.02394, + 5.688416634946208 + ], + [ + 0.07836, + -0.05302, + 5.557295571713635 + ], + [ + 0.11168, + -0.08674, + 5.426174508481063 + ], + [ + 0.1403, + -0.12453, + 5.295053445248491 + ], + [ + 0.16374, + -0.16574, + 5.163932382015918 + ], + [ + 0.18159, + -0.20965, + 5.032811318783346 + ], + [ + 0.19354, + -0.25553, + 4.901690255550774 + ], + [ + 0.19939, + -0.30257, + 4.770569192318201 + ], + [ + 0.2, + -0.35, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 127, + "start_angle_index": 15, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.07194, + "arc_length": 0.07194, + "straight_length": 0.0, + "poses": [ + [ + 0.05, + -0.05, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 128, + "start_angle_index": 15, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.22361, + "trajectory_length": 0.14389, + "arc_length": 0.14389, + "straight_length": 0.0, + "poses": [ + [ + 0.04028, + -0.02587, + 5.605037328581019 + ], + [ + 0.07413, + -0.05972, + 5.390536958983257 + ], + [ + 0.1, + -0.1, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 129, + "start_angle_index": 15, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.81953769817878 + ], + [ + 0.0, + 0.0, + 5.712287513379899 + ], + [ + 0.0, + 0.0, + 5.605037328581019 + ], + [ + 0.0, + 0.0, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 130, + "start_angle_index": 15, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.1118, + "arc_length": 0.0, + "straight_length": 0.1118, + "poses": [ + [ + 0.05, + -0.025, + 5.81953769817878 + ], + [ + 0.1, + -0.05, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 131, + "start_angle_index": 15, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.81953769817878 + ], + [ + 0.0, + 0.0, + 5.9740869011790485 + ], + [ + 0.0, + 0.0, + 6.128636104179318 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 132, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + 0.05, + -0.0118, + 0.0 + ], + [ + 0.1, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 133, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.22361, + "trajectory_length": 0.20735, + "arc_length": 0.20735, + "straight_length": 0.0, + "poses": [ + [ + 0.04862, + -0.01763, + 6.051361502679184 + ], + [ + 0.1, + -0.02361, + 0.0 + ], + [ + 0.15138, + -0.01763, + 0.23182380450040302 + ], + [ + 0.2, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 134, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.33541, + "trajectory_length": 0.31102, + "arc_length": 0.31102, + "straight_length": 0.0, + "poses": [ + [ + 0.04797, + -0.01951, + 5.9740869011790485 + ], + [ + 0.09837, + -0.03141, + 6.128636104179318 + ], + [ + 0.15, + -0.03541, + 0.0 + ], + [ + 0.20163, + -0.03141, + 0.1545492030002687 + ], + [ + 0.25203, + -0.01951, + 0.30909840600053734 + ], + [ + 0.3, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 135, + "start_angle_index": 15, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.09811, + "trajectory_length": 0.16364, + "arc_length": 0.12254, + "straight_length": 0.04109, + "poses": [ + [ + 0.04916, + -0.02356, + 5.956652783219427 + ], + [ + 0.10294, + -0.02617, + 0.22943281971864465 + ], + [ + 0.15, + 0.0, + 0.7853981633974483 + ] + ] + } + ] +} \ No newline at end of file diff --git a/nav2_smac_planner/lattice_primitives/trajectory.py b/nav2_smac_planner/lattice_primitives/trajectory.py index 0ba9141411c..637e2d993e1 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory.py +++ b/nav2_smac_planner/lattice_primitives/trajectory.py @@ -15,7 +15,7 @@ from dataclasses import dataclass from typing import Any, Union -from nav2_smac_planner.lattice_primitives.helper import angle_difference, normalize_angle +from lattice_primitives.helper import angle_difference, normalize_angle import numpy as np from numpy.typing import NDArray diff --git a/nav2_smac_planner/lattice_primitives/trajectory_generator.py b/nav2_smac_planner/lattice_primitives/trajectory_generator.py index f576b14c188..5bf36a7efe2 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory_generator.py +++ b/nav2_smac_planner/lattice_primitives/trajectory_generator.py @@ -15,7 +15,7 @@ import logging from typing import Any, List, Tuple, TypedDict, Union -from nav2_smac_planner.lattice_primitives.trajectory import (FloatNDArray, Path, Trajectory, +from lattice_primitives.trajectory import (FloatNDArray, Path, Trajectory, TrajectoryFloat, TrajectoryParameters) import numpy as np diff --git a/nav2_smac_planner/lattice_primitives/visualizations/0.0.png b/nav2_smac_planner/lattice_primitives/visualizations/0.0.png new file mode 100644 index 00000000000..a0cb1b53329 Binary files /dev/null and b/nav2_smac_planner/lattice_primitives/visualizations/0.0.png differ diff --git a/nav2_smac_planner/lattice_primitives/visualizations/26.56505117707799.png b/nav2_smac_planner/lattice_primitives/visualizations/26.56505117707799.png new file mode 100644 index 00000000000..f66c187cb6a Binary files /dev/null and b/nav2_smac_planner/lattice_primitives/visualizations/26.56505117707799.png differ diff --git a/nav2_smac_planner/lattice_primitives/visualizations/45.0.png b/nav2_smac_planner/lattice_primitives/visualizations/45.0.png new file mode 100644 index 00000000000..2d6cc75e110 Binary files /dev/null and b/nav2_smac_planner/lattice_primitives/visualizations/45.0.png differ diff --git a/nav2_smac_planner/lattice_primitives/visualizations/63.43494882292201.png b/nav2_smac_planner/lattice_primitives/visualizations/63.43494882292201.png new file mode 100644 index 00000000000..a8a7428be77 Binary files /dev/null and b/nav2_smac_planner/lattice_primitives/visualizations/63.43494882292201.png differ diff --git a/nav2_smac_planner/lattice_primitives/visualizations/90.0.png b/nav2_smac_planner/lattice_primitives/visualizations/90.0.png new file mode 100644 index 00000000000..29353676faa Binary files /dev/null and b/nav2_smac_planner/lattice_primitives/visualizations/90.0.png differ diff --git a/nav2_smac_planner/lattice_primitives/visualizations/all_trajectories.png b/nav2_smac_planner/lattice_primitives/visualizations/all_trajectories.png new file mode 100644 index 00000000000..6c356ad7a52 Binary files /dev/null and b/nav2_smac_planner/lattice_primitives/visualizations/all_trajectories.png differ diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index f8acf8bd6a6..9400ea3b37b 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -12,6 +12,7 @@ ament_index_cpp angles + backward_ros eigen geometry_msgs nav2_core diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index d9a7c8ebfa5..56dcbfdc271 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -50,23 +50,21 @@ void LatticeMotionTable::initMotionModel( unsigned int & size_x_in, SearchInfo & search_info) { - size_x = size_x_in; - - if (current_lattice_filepath == search_info.lattice_filepath) { - return; - } - size_x = size_x_in; change_penalty = search_info.change_penalty; non_straight_penalty = search_info.non_straight_penalty; cost_penalty = search_info.cost_penalty; reverse_penalty = search_info.reverse_penalty; travel_distance_reward = 1.0f - search_info.retrospective_penalty; - current_lattice_filepath = search_info.lattice_filepath; allow_reverse_expansion = search_info.allow_reverse_expansion; rotation_penalty = search_info.rotation_penalty; min_turning_radius = search_info.minimum_turning_radius; + if (current_lattice_filepath == search_info.lattice_filepath) { + return; + } + current_lattice_filepath = search_info.lattice_filepath; + // Get the metadata about this minimum control set lattice_metadata = getLatticeMetadata(current_lattice_filepath); std::ifstream latticeFile(current_lattice_filepath); @@ -232,7 +230,8 @@ bool NodeLattice::isNodeValid( // Check primitive end pose // Convert grid quantization of primitives to radians, then collision checker quantization static const double bin_size = 2.0 * M_PI / collision_checker->getPrecomputedAngles().size(); - const double & angle = motion_table.getAngleFromBin(this->pose.theta) / bin_size; + const double angle = std::fmod(motion_table.getAngleFromBin(this->pose.theta), + 2.0 * M_PI) / bin_size; if (collision_checker->inCollision( this->pose.x, this->pose.y, angle /*bin in collision checker*/, traverse_unknown)) { @@ -271,7 +270,7 @@ bool NodeLattice::isNodeValid( if (is_backwards) { prim_pose._theta = std::fmod(it->_theta + M_PI, 2.0 * M_PI); } else { - prim_pose._theta = it->_theta; + prim_pose._theta = std::fmod(it->_theta, 2.0 * M_PI); } if (collision_checker->inCollision( prim_pose._x, diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index ad1ae0647c9..07a30839374 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -273,9 +273,9 @@ void SmacPlannerHybrid::configure( _angle_quantizations); // Initialize path smoother + SmootherParams params; + params.get(node, name); if (smooth_path) { - SmootherParams params; - params.get(node, name); _smoother = std::make_unique(params); _smoother->initialize(_minimum_turning_radius_global_coords); } @@ -414,15 +414,17 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( std::to_string(start.pose.position.y) + ") was outside bounds"); } - double orientation_bin = std::round(tf2::getYaw(start.pose.orientation) / _angle_bin_size); - while (orientation_bin < 0.0) { - orientation_bin += static_cast(_angle_quantizations); + double start_orientation_bin = std::round(tf2::getYaw(start.pose.orientation) / _angle_bin_size); + while (start_orientation_bin < 0.0) { + start_orientation_bin += static_cast(_angle_quantizations); } // This is needed to handle precision issues - if (orientation_bin >= static_cast(_angle_quantizations)) { - orientation_bin -= static_cast(_angle_quantizations); + if (start_orientation_bin >= static_cast(_angle_quantizations)) { + start_orientation_bin -= static_cast(_angle_quantizations); } - _a_star->setStart(mx_start, my_start, static_cast(orientation_bin)); + unsigned int start_orientation_bin_int = + static_cast(start_orientation_bin); + _a_star->setStart(mx_start, my_start, start_orientation_bin_int); // Set goal point, in A* bin search coordinates if (!costmap->worldToMapContinuous( @@ -435,15 +437,17 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + std::to_string(goal.pose.position.y) + ") was outside bounds"); } - orientation_bin = std::round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size); - while (orientation_bin < 0.0) { - orientation_bin += static_cast(_angle_quantizations); + double goal_orientation_bin = std::round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size); + while (goal_orientation_bin < 0.0) { + goal_orientation_bin += static_cast(_angle_quantizations); } // This is needed to handle precision issues - if (orientation_bin >= static_cast(_angle_quantizations)) { - orientation_bin -= static_cast(_angle_quantizations); + if (goal_orientation_bin >= static_cast(_angle_quantizations)) { + goal_orientation_bin -= static_cast(_angle_quantizations); } - _a_star->setGoal(mx_goal, my_goal, static_cast(orientation_bin), + unsigned int goal_orientation_bin_int = + static_cast(goal_orientation_bin); + _a_star->setGoal(mx_goal, my_goal, static_cast(goal_orientation_bin_int), _goal_heading_mode, _coarse_search_resolution); // Setup message @@ -460,7 +464,8 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( // Corner case of start and goal being on the same cell if (std::floor(mx_start) == std::floor(mx_goal) && - std::floor(my_start) == std::floor(my_goal)) + std::floor(my_start) == std::floor(my_goal) && + start_orientation_bin_int == goal_orientation_bin_int) { pose.pose = start.pose; pose.pose.orientation = goal.pose.orientation; diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index d689507e8f4..c6e110cbfd4 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -230,9 +230,9 @@ void SmacPlannerLattice::configure( _metadata.number_of_headings); // Initialize path smoother + SmootherParams params; + params.get(node, name); if (smooth_path) { - SmootherParams params; - params.get(node, name); _smoother = std::make_unique(params); _smoother->initialize(_metadata.min_turning_radius); } @@ -338,9 +338,9 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + std::to_string(start.pose.position.y) + ") was outside bounds"); } - _a_star->setStart( - mx_start, my_start, - NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation))); + unsigned int start_bin = + NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation)); + _a_star->setStart(mx_start, my_start, start_bin); // Set goal point, in A* bin search coordinates if (!_costmap->worldToMapContinuous( @@ -353,9 +353,10 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + std::to_string(goal.pose.position.y) + ") was outside bounds"); } + unsigned int goal_bin = + NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)); _a_star->setGoal( - mx_goal, my_goal, - NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)), + mx_goal, my_goal, goal_bin, _goal_heading_mode, _coarse_search_resolution); // Setup message @@ -372,7 +373,8 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( // Corner case of start and goal being on the same cell if (std::floor(mx_start) == std::floor(mx_goal) && - std::floor(my_start) == std::floor(my_goal)) + std::floor(my_start) == std::floor(my_goal) && + start_bin == goal_bin) { pose.pose = start.pose; pose.pose.orientation = goal.pose.orientation; diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt index b2e3555ab2a..a48a7166d45 100644 --- a/nav2_smoother/CMakeLists.txt +++ b/nav2_smoother/CMakeLists.txt @@ -1,14 +1,14 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_smoother) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(backward_ros REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) -find_package(nav_2d_utils REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) @@ -16,6 +16,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(nav2_ros_common REQUIRED) +find_package(Eigen3 REQUIRED) nav2_package() @@ -42,10 +43,9 @@ target_link_libraries(${library_name} PUBLIC rclcpp_lifecycle::rclcpp_lifecycle tf2_ros::tf2_ros nav2_ros_common::nav2_ros_common + Eigen3::Eigen ) target_link_libraries(${library_name} PRIVATE - nav_2d_utils::conversions - nav_2d_utils::tf_help rclcpp_components::component tf2::tf2 ) @@ -159,6 +159,7 @@ ament_export_dependencies( rclcpp_lifecycle tf2 tf2_ros + Eigen3 ) ament_export_targets(${library_name}) ament_package() diff --git a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp index e18c0b823e5..1f56e8440e5 100644 --- a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp @@ -15,6 +15,7 @@ #ifndef NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_ #define NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_ +#include #include #include #include @@ -85,6 +86,11 @@ class SavitzkyGolaySmoother : public nav2_core::Smoother nav_msgs::msg::Path & path, const rclcpp::Duration & max_time) override; + /** + * @brief Method to calculate SavitzkyGolay Coefficients + */ + void calculateCoefficients(); + protected: /** * @brief Smoother method - does the smoothing on a segment @@ -99,7 +105,8 @@ class SavitzkyGolaySmoother : public nav2_core::Smoother bool & reversing_segment); bool do_refinement_, enforce_path_inversion_; - int refinement_num_; + int refinement_num_, window_size_, half_window_size_, poly_order_; + Eigen::VectorXd sg_coeffs_; rclcpp::Logger logger_{rclcpp::get_logger("SGSmoother")}; }; diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index e7d4708d790..f193809becc 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -12,11 +12,11 @@ nav2_common angles + backward_ros nav2_core nav2_costmap_2d nav2_msgs nav2_util - nav_2d_utils pluginlib rclcpp rclcpp_components @@ -24,6 +24,8 @@ tf2 tf2_ros nav2_ros_common + eigen3_cmake_module + eigen ament_cmake_gtest ament_index_cpp diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 4408b5a43e0..74714bafb81 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -23,9 +23,7 @@ #include "nav2_core/smoother_exceptions.hpp" #include "nav2_smoother/nav2_smoother.hpp" #include "nav2_ros_common/node_utils.hpp" -#include "nav_2d_utils/conversions.hpp" -#include "nav_2d_utils/tf_help.hpp" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/create_timer_ros.hpp" using namespace std::chrono_literals; @@ -80,7 +78,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State & state) auto timer_interface = std::make_shared( get_node_base_interface(), get_node_timers_interface()); tf_->setCreateTimerInterface(timer_interface); - transform_listener_ = std::make_shared(*tf_); + transform_listener_ = std::make_shared(*tf_, this, true); std::string costmap_topic, footprint_topic, robot_base_frame; double transform_tolerance = 0.1; diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp index af23cf30995..90180734e5c 100644 --- a/nav2_smoother/src/savitzky_golay_smoother.cpp +++ b/nav2_smoother/src/savitzky_golay_smoother.cpp @@ -40,9 +40,39 @@ void SavitzkyGolaySmoother::configure( node, name + ".refinement_num", rclcpp::ParameterValue(2)); declare_parameter_if_not_declared( node, name + ".enforce_path_inversion", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, name + ".window_size", rclcpp::ParameterValue(7)); + declare_parameter_if_not_declared( + node, name + ".poly_order", rclcpp::ParameterValue(3)); node->get_parameter(name + ".do_refinement", do_refinement_); node->get_parameter(name + ".refinement_num", refinement_num_); node->get_parameter(name + ".enforce_path_inversion", enforce_path_inversion_); + node->get_parameter(name + ".window_size", window_size_); + node->get_parameter(name + ".poly_order", poly_order_); + if (window_size_ % 2 == 0 || window_size_ <= 2) { + throw nav2_core::SmootherException( + "Savitzky-Golay Smoother requires an odd window size of 3 or greater"); + } + half_window_size_ = (window_size_ - 1) / 2; + calculateCoefficients(); +} + +// For more details on calculating Savitzky–Golay filter coefficients, +// see: https://www.colmryan.org/posts/savitsky_golay/ +void SavitzkyGolaySmoother::calculateCoefficients() +{ + // We construct the Vandermonde matrix here + Eigen::VectorXd v = Eigen::VectorXd::LinSpaced(window_size_, -half_window_size_, + half_window_size_); + Eigen::MatrixXd x = Eigen::MatrixXd::Ones(window_size_, poly_order_ + 1); + for(int i = 1; i <= poly_order_; i++) { + x.col(i) = (x.col(i - 1).array() * v.array()).matrix(); + } + // Compute the pseudoinverse of X, (X^T * X)^-1 * X^T + Eigen::MatrixXd coeff_mat = (x.transpose() * x).inverse() * x.transpose(); + + // Extract the smoothing coefficients + sg_coeffs_ = coeff_mat.row(0).transpose(); } bool SavitzkyGolaySmoother::smooth( @@ -62,8 +92,10 @@ bool SavitzkyGolaySmoother::smooth( path_segments = findDirectionalPathSegments(path); } + // Minimum point size to smooth is SG filter size + start + end + unsigned int minimum_points = window_size_ + 2; for (unsigned int i = 0; i != path_segments.size(); i++) { - if (path_segments[i].end - path_segments[i].start > 9) { + if (path_segments[i].end - path_segments[i].start > minimum_points) { // Populate path segment curr_path_segment.poses.clear(); std::copy( @@ -100,68 +132,41 @@ bool SavitzkyGolaySmoother::smoothImpl( nav_msgs::msg::Path & path, bool & reversing_segment) { - // Must be at least 10 in length to enter function const unsigned int & path_size = path.poses.size(); - // 7-point SG filter - const std::array filter = { - -2.0 / 21.0, - 3.0 / 21.0, - 6.0 / 21.0, - 7.0 / 21.0, - 6.0 / 21.0, - 3.0 / 21.0, - -2.0 / 21.0}; - - auto applyFilter = [&](const std::vector & data) - -> geometry_msgs::msg::Point - { - geometry_msgs::msg::Point val; - for (unsigned int i = 0; i != filter.size(); i++) { - val.x += filter[i] * data[i].x; - val.y += filter[i] * data[i].y; - } - return val; + // Convert PoseStamped to Eigen + auto toEigenVec = [](const geometry_msgs::msg::PoseStamped & pose) -> Eigen::Vector2d { + return {pose.pose.position.x, pose.pose.position.y}; }; auto applyFilterOverAxes = [&](std::vector & plan_pts, - const std::vector & init_plan_pts) -> void + const std::vector & init_plan_pts) -> void { - auto pt_m3 = init_plan_pts[0].pose.position; - auto pt_m2 = init_plan_pts[0].pose.position; - auto pt_m1 = init_plan_pts[0].pose.position; - auto pt = init_plan_pts[1].pose.position; - auto pt_p1 = init_plan_pts[2].pose.position; - auto pt_p2 = init_plan_pts[3].pose.position; - auto pt_p3 = init_plan_pts[4].pose.position; - // First point is fixed for (unsigned int idx = 1; idx != path_size - 1; idx++) { - plan_pts[idx].pose.position = applyFilter({pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3}); - pt_m3 = pt_m2; - pt_m2 = pt_m1; - pt_m1 = pt; - pt = pt_p1; - pt_p1 = pt_p2; - pt_p2 = pt_p3; - - if (idx + 4 < path_size - 1) { - pt_p3 = init_plan_pts[idx + 4].pose.position; - } else { - // Return the last point - pt_p3 = init_plan_pts[path_size - 1].pose.position; + Eigen::Vector2d accum(0.0, 0.0); + + for(int j = -half_window_size_; j <= half_window_size_; j++) { + int path_idx = std::clamp(idx + j, 0, path_size - 1); + accum += sg_coeffs_(j + half_window_size_) * init_plan_pts[path_idx]; } + plan_pts[idx].pose.position.x = accum.x(); + plan_pts[idx].pose.position.y = accum.y(); } }; - const auto initial_path_poses = path.poses; + std::vector initial_path_poses(path.poses.size()); + std::transform(path.poses.begin(), path.poses.end(), + initial_path_poses.begin(), toEigenVec); applyFilterOverAxes(path.poses, initial_path_poses); // Let's do additional refinement, it shouldn't take more than a couple milliseconds if (do_refinement_) { for (int i = 0; i < refinement_num_; i++) { - const auto reined_initial_path_poses = path.poses; + std::vector reined_initial_path_poses(path.poses.size()); + std::transform(path.poses.begin(), path.poses.end(), + reined_initial_path_poses.begin(), toEigenVec); applyFilterOverAxes(path.poses, reined_initial_path_poses); } } diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index db9f323e8a1..5179784f83a 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -81,7 +81,7 @@ bool SimpleSmoother::smooth( std::lock_guard lock(*(costmap->getMutex())); for (unsigned int i = 0; i != path_segments.size(); i++) { - if (path_segments[i].end - path_segments[i].start > 9) { + if (path_segments[i].end - path_segments[i].start > 3) { // Populate path segment curr_path_segment.poses.clear(); std::copy( diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 88922fc900f..9817f2f3563 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_system_tests) find_package(ament_cmake REQUIRED) diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 2f62894abb7..0c23468dc45 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -26,9 +26,9 @@ #include "behaviortree_cpp/bt_factory.h" #include "behaviortree_cpp/utils/shared_library.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/create_timer_ros.hpp" #include "nav2_util/odometry_utils.hpp" #include "nav2_util/string_utils.hpp" @@ -71,55 +71,80 @@ class BehaviorTreeHandler } } - bool loadBehaviorTree(const std::string & filename) + BT::Blackboard::Ptr setBlackboardVariables() { - // Read the input BT XML from the specified file into a string - std::ifstream xml_file(filename); + // Create and populate the blackboard + blackboard = BT::Blackboard::create(); + blackboard->set("node", node_); + blackboard->set("server_timeout", std::chrono::milliseconds(20)); + blackboard->set("bt_loop_duration", std::chrono::milliseconds(10)); + blackboard->set("wait_for_service_timeout", + std::chrono::milliseconds(1000)); + blackboard->set("tf_buffer", tf_); + blackboard->set("initial_pose_received", false); + blackboard->set("number_recoveries", 0); + blackboard->set("odom_smoother", odom_smoother_); + + // Create dummy goal + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.header.frame_id = "map"; + blackboard->set("goal", goal); + return blackboard; + } + bool behaviorTreeFileValidation( + const std::string & filename) + { + std::ifstream xml_file(filename); if (!xml_file.good()) { - RCLCPP_ERROR(node_->get_logger(), "Couldn't open input XML file: %s", filename.c_str()); + RCLCPP_ERROR(node_->get_logger(), + "Couldn't open BT XML file: %s", filename.c_str()); return false; } + return true; + } - std::stringstream buffer; - buffer << xml_file.rdbuf(); - xml_file.close(); - std::string xml_string = buffer.str(); - // Create the blackboard that will be shared by all of the nodes in the tree - blackboard = BT::Blackboard::create(); - // Put items on the blackboard - blackboard->set("node", node_); // NOLINT - blackboard->set( - "server_timeout", std::chrono::milliseconds(20)); // NOLINT - blackboard->set( - "bt_loop_duration", std::chrono::milliseconds(10)); // NOLINT - blackboard->set( - "wait_for_service_timeout", std::chrono::milliseconds(1000)); // NOLINT - blackboard->set("tf_buffer", tf_); // NOLINT - blackboard->set("initial_pose_received", false); // NOLINT - blackboard->set("number_recoveries", 0); // NOLINT - blackboard->set("odom_smoother", odom_smoother_); // NOLINT - - // set dummy goal on blackboard - geometry_msgs::msg::PoseStamped goal; - goal.header.stamp = node_->now(); - goal.header.frame_id = "map"; - goal.pose.position.x = 0.0; - goal.pose.position.y = 0.0; - goal.pose.position.z = 0.0; - goal.pose.orientation.x = 0.0; - goal.pose.orientation.y = 0.0; - goal.pose.orientation.z = 0.0; - goal.pose.orientation.w = 1.0; + bool loadBehaviorTree( + const std::string & filename, + const std::vector & search_directories) + { + if (!behaviorTreeFileValidation(filename)) { + return false; + } + + namespace fs = std::filesystem; + const auto canonical_main_bt = fs::canonical(filename); + + // Register all XML behavior Subtrees found in the given directories + for (const auto & directory : search_directories) { + try { + for (const auto & entry : fs::directory_iterator(directory)) { + if (entry.path().extension() == ".xml") { + // Skip registering the main tree file + if (fs::equivalent(fs::canonical(entry.path()), canonical_main_bt)) { + continue; + } + factory_.registerBehaviorTreeFromFile(entry.path().string()); + } + } + } catch (const std::exception & e) { + RCLCPP_ERROR(node_->get_logger(), + "Exception reading behavior tree directory: %s", e.what()); + return false; + } + } - blackboard->set("goal", goal); // NOLINT + // Create and populate the blackboard + blackboard = setBlackboardVariables(); - // Create the Behavior Tree from the XML input + // Build the tree from the XML string try { - tree = factory_.createTreeFromText(xml_string, blackboard); + tree = factory_.createTreeFromFile(filename, blackboard); } catch (BT::RuntimeError & exp) { - RCLCPP_ERROR(node_->get_logger(), "%s: %s", filename.c_str(), exp.what()); + RCLCPP_ERROR(node_->get_logger(), "Failed to create BT from %s: %s", filename.c_str(), + exp.what()); return false; } @@ -196,19 +221,75 @@ std::shared_ptr BehaviorTreeTestFixture::bt_handler = nullp TEST_F(BehaviorTreeTestFixture, TestBTXMLFiles) { - std::filesystem::path root = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - root /= "behavior_trees/"; - - if (std::filesystem::exists(root) && std::filesystem::is_directory(root)) { - for (auto const & entry : std::filesystem::recursive_directory_iterator(root)) { - if (std::filesystem::is_regular_file(entry) && entry.path().extension() == ".xml") { - std::cout << entry.path().string() << std::endl; - EXPECT_EQ(bt_handler->loadBehaviorTree(entry.path().string()), true); - } + // Get the BT root directory + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + + ASSERT_TRUE(std::filesystem::exists(root_dir)); + ASSERT_TRUE(std::filesystem::is_directory(root_dir)); + + std::vector search_directories = {root_dir.string()}; + + for (auto const & entry : std::filesystem::recursive_directory_iterator(root_dir)) { + if (entry.is_regular_file() && entry.path().extension() == ".xml") { + std::string main_bt = entry.path().string(); + std::cout << "Testing BT file: " << main_bt << std::endl; + + EXPECT_TRUE(bt_handler->loadBehaviorTree(main_bt, search_directories)) + << "Failed to load: " << main_bt; } } } +TEST_F(BehaviorTreeTestFixture, TestWrongBTFormatXML) +{ + auto write_file = [](const std::string & path, const std::string & content) { + std::ofstream ofs(path); + ofs << content; + }; + + // File paths + std::string valid_subtree = "/tmp/valid_subtree.xml"; + std::string invalid_subtree = "/tmp/invalid_subtree.xml"; + std::string main_file = "/tmp/test_main_tree.xml"; + std::string malformed_main = "/tmp/malformed_main.xml"; + + // Valid subtree + write_file(valid_subtree, + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"); + + // Invalid subtree (malformed XML) + write_file(invalid_subtree, ""); + + // Main tree referencing the valid subtree + write_file(main_file, + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"); + + // Malformed main tree + write_file(malformed_main, ""); + + std::vector search_directories = {"/tmp"}; + + EXPECT_FALSE(bt_handler->loadBehaviorTree(main_file, search_directories)); + EXPECT_FALSE(bt_handler->loadBehaviorTree(malformed_main, search_directories)); + + std::remove(valid_subtree.c_str()); + std::remove(main_file.c_str()); + std::remove(invalid_subtree.c_str()); + std::remove(malformed_main.c_str()); +} + /** * Test scenario: * @@ -218,10 +299,14 @@ TEST_F(BehaviorTreeTestFixture, TestBTXMLFiles) TEST_F(BehaviorTreeTestFixture, TestAllSuccess) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); BT::NodeStatus result = BT::NodeStatus::RUNNING; @@ -265,10 +350,14 @@ TEST_F(BehaviorTreeTestFixture, TestAllSuccess) TEST_F(BehaviorTreeTestFixture, TestAllFailure) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); // Set all action server to fail the first 100 times Ranges failureRange; @@ -321,10 +410,14 @@ TEST_F(BehaviorTreeTestFixture, TestAllFailure) TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); // Set ComputePathToPose and FollowPath action servers to fail for the first action Ranges failureRange; @@ -380,10 +473,14 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries) TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); // Set ComputePathToPose action server to fail for the first action Ranges plannerFailureRange; @@ -478,10 +575,14 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple) TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); // Set FollowPath action server to fail for the first 2 actions Ranges controllerFailureRange; @@ -546,10 +647,14 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex) TEST_F(BehaviorTreeTestFixture, TestRecoverySubtreeGoalUpdated) { // Load behavior tree from file - std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); - bt_file /= "behavior_trees/"; - bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; - EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml"; + + std::vector search_directories = {root_dir.string()}; + + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true); // Set FollowPath action server to fail for the first 2 actions Ranges controllerFailureRange; diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp index bb3f3b18159..40e2aa9a754 100644 --- a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp +++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp @@ -35,8 +35,8 @@ #include "std_msgs/msg/empty.hpp" #include "tf2/utils.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" namespace nav2_system_tests { diff --git a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp index 5a7313c0cd5..bea4506e68e 100644 --- a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp +++ b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp @@ -33,8 +33,8 @@ #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "tf2/utils.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" namespace nav2_system_tests { diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index b81cef7ce56..01fe9656d5b 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -76,6 +76,7 @@ controller_server: temperature: 0.3 gamma: 0.015 motion_model: "DiffDrive" + publish_critics_stats: true visualize: true regenerate_noises: true TrajectoryVisualizer: diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index d878bc49f22..3c14f795d4f 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -32,7 +32,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "nav2_planner/planner_server.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" namespace nav2_system_tests { diff --git a/nav2_theta_star_planner/CMakeLists.txt b/nav2_theta_star_planner/CMakeLists.txt index 87da1fa9cbc..09813efc4d7 100644 --- a/nav2_theta_star_planner/CMakeLists.txt +++ b/nav2_theta_star_planner/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_theta_star_planner) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) @@ -86,7 +87,8 @@ ament_export_libraries(${library_name}) ament_export_dependencies( geometry_msgs nav2_core - nav2_costmap_2dnav2_util + nav2_costmap_2d + nav2_util nav_msgs rclcpp rclcpp_lifecycle diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 0279b52fe7e..414de9f3066 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -11,6 +11,7 @@ ament_cmake nav2_common + backward_ros geometry_msgs nav2_core nav2_costmap_2d diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index 08a7908f3b3..2a2f4061818 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_util) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(bond REQUIRED) find_package(bondcpp REQUIRED) find_package(geometry_msgs REQUIRED) diff --git a/nav2_util/include/nav2_util/controller_utils.hpp b/nav2_util/include/nav2_util/controller_utils.hpp new file mode 100644 index 00000000000..02afd8ea575 --- /dev/null +++ b/nav2_util/include/nav2_util/controller_utils.hpp @@ -0,0 +1,69 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 Samsung Research America +// +// 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_UTIL__CONTROLLER_UTILS_HPP_ +#define NAV2_UTIL__CONTROLLER_UTILS_HPP_ + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "nav_msgs/msg/path.hpp" + +namespace nav2_util +{ +/** +* @brief Find the intersection a circle and a line segment. +* This assumes the circle is centered at the origin. +* If no intersection is found, a floating point error will occur. +* @param p1 first endpoint of line segment +* @param p2 second endpoint of line segment +* @param r radius of circle +* @return point of intersection +*/ +geometry_msgs::msg::Point circleSegmentIntersection( + const geometry_msgs::msg::Point & p1, + const geometry_msgs::msg::Point & p2, + double r); + +/** +* @brief Get lookahead point +* @param lookahead_dist Optimal lookahead distance +* @param path Current global path +* @param interpolate_after_goal If true, interpolate the lookahead point after the goal based +* on the orientation given by the position of the last two pose of the path +* @return Lookahead point +*/ +geometry_msgs::msg::PoseStamped getLookAheadPoint( + double &, const nav_msgs::msg::Path &, + const bool interpolate_after_goal = false); + + +/** +* @brief Find the linear interpolation between two points +* at a given distance starting from first endpoint. +* @param p1 first endpoint of line segment +* @param p2 second endpoint of line segment +* @param target_dist interpolation distance from first endpoint of line segment +* @return point of intersection +*/ +geometry_msgs::msg::Point linearInterpolation( + const geometry_msgs::msg::Point & p1, + const geometry_msgs::msg::Point & p2, + const double target_dist); + + +} // namespace nav2_util + +#endif // NAV2_UTIL__CONTROLLER_UTILS_HPP_ diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 3a56cb5feea..d67f17e3c7c 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -188,6 +188,49 @@ inline int find_next_matching_goal_in_waypoint_statuses( return itr - waypoint_statuses.begin(); } +/** + * @brief Checks if point is inside the polygon + * @param px X-coordinate of the given point to check + * @param py Y-coordinate of the given point to check + * @param polygon Polygon to check if the point is inside + * @return True if given point is inside polygon, otherwise false + */ +template +inline bool isPointInsidePolygon( + const double px, const double py, const std::vector & polygon) +{ + // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon." + // Communications of the ACM 5.8 (1962): 434. + // Implementation of ray crossings algorithm for point in polygon task solving. + // Y coordinate is fixed. Moving the ray on X+ axis starting from given point. + // Odd number of intersections with polygon boundaries means the point is inside polygon. + const int points_num = polygon.size(); + int i, j; // Polygon vertex iterators + bool res = false; // Final result, initialized with already inverted value + + // Starting from the edge where the last point of polygon is connected to the first + i = points_num - 1; + for (j = 0; j < points_num; j++) { + // Checking the edge only if given point is between edge boundaries by Y coordinates. + // One of the condition should contain equality in order to exclude the edges + // parallel to X+ ray. + if ((py <= polygon[i].y) == (py > polygon[j].y)) { + // Calculating the intersection coordinate of X+ ray + const double x_inter = polygon[i].x + + (py - polygon[i].y) * + (polygon[j].x - polygon[i].x) / + (polygon[j].y - polygon[i].y); + // If intersection with checked edge is greater than point x coordinate, + // inverting the result + if (x_inter > px) { + res = !res; + } + } + i = j; + } + return res; +} + } // namespace geometry_utils } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/lifecycle_service_client.hpp b/nav2_util/include/nav2_util/lifecycle_service_client.hpp index 64860f47e87..56eea6b5ccd 100644 --- a/nav2_util/include/nav2_util/lifecycle_service_client.hpp +++ b/nav2_util/include/nav2_util/lifecycle_service_client.hpp @@ -57,6 +57,12 @@ class LifecycleServiceClient } } + ~LifecycleServiceClient() + { + change_state_.stop(); + get_state_.stop(); + } + /// Trigger a state change /** * Throws std::runtime_error on failure diff --git a/nav2_util/include/nav2_util/occ_grid_utils.hpp b/nav2_util/include/nav2_util/occ_grid_utils.hpp new file mode 100644 index 00000000000..26bb8740f2b --- /dev/null +++ b/nav2_util/include/nav2_util/occ_grid_utils.hpp @@ -0,0 +1,100 @@ +// Copyright (c) 2008, 2013, Willow Garage, Inc. +// Copyright (c) 2023 Samsung R&D Institute Russia +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Eitan Marder-Eppstein +// David V. Lu!! +// Alexey Merzlyakov + +#ifndef NAV2_UTIL__OCC_GRID_UTILS_HPP_ +#define NAV2_UTIL__OCC_GRID_UTILS_HPP_ + +#include "nav_msgs/msg/occupancy_grid.hpp" + +namespace nav2_util +{ + +/** + * @brief: Convert from world coordinates to map coordinates. + Similar to Costmap2D::worldToMap() method but works directly with OccupancyGrid-s. + * @param map OccupancyGrid map on which to convert + * @param wx The x world coordinate + * @param wy The y world coordinate + * @param mx Will be set to the associated map x coordinate + * @param my Will be set to the associated map y coordinate + * @return True if the conversion was successful (legal bounds) false otherwise + */ +inline bool worldToMap( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + const double wx, const double wy, unsigned int & mx, unsigned int & my) +{ + const double origin_x = map->info.origin.position.x; + const double origin_y = map->info.origin.position.y; + const double resolution = map->info.resolution; + const unsigned int size_x = map->info.width; + const unsigned int size_y = map->info.height; + + if (wx < origin_x || wy < origin_y) { + return false; + } + + mx = static_cast((wx - origin_x) / resolution); + my = static_cast((wy - origin_y) / resolution); + if (mx >= size_x || my >= size_y) { + return false; + } + + return true; +} + +/** + * @brief Convert from map coordinates to world coordinates + * @param mx The x map coordinate + * @param my The y map coordinate + * @param wx Will be set to the associated world x coordinate + * @param wy Will be set to the associated world y coordinate + */ +inline void mapToWorld( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + const unsigned int mx, const unsigned int my, double & wx, double & wy) +{ + const double origin_x = map->info.origin.position.x; + const double origin_y = map->info.origin.position.y; + const double resolution = map->info.resolution; + + wx = origin_x + (mx + 0.5) * resolution; + wy = origin_y + (my + 0.5) * resolution; +} + +} // namespace nav2_util + +#endif // NAV2_UTIL__OCC_GRID_UTILS_HPP_ diff --git a/nav2_util/include/nav2_util/raytrace_line_2d.hpp b/nav2_util/include/nav2_util/raytrace_line_2d.hpp new file mode 100644 index 00000000000..b44d1e79a8d --- /dev/null +++ b/nav2_util/include/nav2_util/raytrace_line_2d.hpp @@ -0,0 +1,146 @@ +// Copyright (c) 2008, 2013, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Eitan Marder-Eppstein +// David V. Lu!! + +#ifndef NAV2_UTIL__RAYTRACE_LINE_2D_HPP_ +#define NAV2_UTIL__RAYTRACE_LINE_2D_HPP_ + +#include +#include +#include + +namespace nav2_util +{ + +/** + * @brief get the sign of an int + */ +inline int sign(int x) +{ + return x > 0 ? 1 : -1; +} + +/** + * @brief A 2D implementation of Bresenham's raytracing algorithm... + * applies an action at each step + */ +template +inline void bresenham2D( + ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, + int offset_a, int offset_b, unsigned int offset, + unsigned int max_length) +{ + unsigned int end = std::min(max_length, abs_da); + for (unsigned int i = 0; i < end; ++i) { + at(offset); + offset += offset_a; + error_b += abs_db; + if ((unsigned int)error_b >= abs_da) { + offset += offset_b; + error_b -= abs_da; + } + } + at(offset); +} + +/** + * @brief Raytrace a line and apply some action at each step + * @param at The action to take... a functor + * @param x0 The starting x coordinate + * @param y0 The starting y coordinate + * @param x1 The ending x coordinate + * @param y1 The ending y coordinate + * @param step_x OX-step on map + * @param max_length The maximum desired length of the segment... + * allows you to not go all the way to the endpoint + * @param min_length The minimum desired length of the segment + */ +template +inline void raytraceLine( + ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, + unsigned int y1, unsigned int step_x, + unsigned int max_length = UINT_MAX, unsigned int min_length = 0) +{ + int dx_full = x1 - x0; + int dy_full = y1 - y0; + + // we need to chose how much to scale our dominant dimension, + // based on the maximum length of the line + double dist = std::hypot(dx_full, dy_full); + if (dist < min_length) { + return; + } + + unsigned int min_x0, min_y0; + if (dist > 0.0) { + // Adjust starting point and offset to start from min_length distance + min_x0 = (unsigned int)(x0 + dx_full / dist * min_length); + min_y0 = (unsigned int)(y0 + dy_full / dist * min_length); + } else { + // dist can be 0 if [x0, y0]==[x1, y1]. + // In this case only this cell should be processed. + min_x0 = x0; + min_y0 = y0; + } + unsigned int offset = min_y0 * step_x + min_x0; + + int dx = x1 - min_x0; + int dy = y1 - min_y0; + + unsigned int abs_dx = abs(dx); + unsigned int abs_dy = abs(dy); + + int offset_dx = sign(dx); + int offset_dy = sign(dy) * step_x; + + double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist); + // if x is dominant + if (abs_dx >= abs_dy) { + int error_y = abs_dx / 2; + + bresenham2D( + at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx)); + return; + } + + // otherwise y is dominant + int error_x = abs_dy / 2; + + bresenham2D( + at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy)); +} + +} // namespace nav2_util + +#endif // NAV2_UTIL__RAYTRACE_LINE_2D_HPP_ diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index cffbe80a26b..faca6d301dc 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -26,7 +26,7 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2/time.hpp" #include "tf2/transform_datatypes.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 0acd0f1a37b..334879c3981 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -13,6 +13,7 @@ nav2_common + backward_ros bond bondcpp builtin_interfaces diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index 563742e8b72..99d24ee6348 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -3,6 +3,7 @@ add_library(${library_name} SHARED lifecycle_service_client.cpp string_utils.cpp robot_utils.cpp + controller_utils.cpp odometry_utils.cpp array_parser.cpp ) diff --git a/nav2_util/src/base_footprint_publisher.hpp b/nav2_util/src/base_footprint_publisher.hpp index 67f50625101..15b16037bed 100644 --- a/nav2_util/src/base_footprint_publisher.hpp +++ b/nav2_util/src/base_footprint_publisher.hpp @@ -21,10 +21,10 @@ #include "rclcpp/rclcpp.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/buffer.h" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/buffer.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/utils.hpp" diff --git a/nav2_util/src/controller_utils.cpp b/nav2_util/src/controller_utils.cpp new file mode 100644 index 00000000000..52b484d1d91 --- /dev/null +++ b/nav2_util/src/controller_utils.cpp @@ -0,0 +1,165 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 Samsung Research America +// +// 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 "nav2_util/controller_utils.hpp" +#include "nav2_util/geometry_utils.hpp" + +namespace nav2_util +{ +geometry_msgs::msg::Point circleSegmentIntersection( + const geometry_msgs::msg::Point & p1, + const geometry_msgs::msg::Point & p2, + double r) +{ + // Formula for intersection of a line with a circle centered at the origin, + // modified to always return the point that is on the segment between the two points. + // https://mathworld.wolfram.com/Circle-LineIntersection.html + // This works because the poses are transformed into the robot frame. + // This can be derived from solving the system of equations of a line and a circle + // which results in something that is just a reformulation of the quadratic formula. + // Interactive illustration in doc/circle-segment-intersection.ipynb as well as at + // https://www.desmos.com/calculator/td5cwbuocd + double x1 = p1.x; + double x2 = p2.x; + double y1 = p1.y; + double y2 = p2.y; + + double dx = x2 - x1; + double dy = y2 - y1; + double dr2 = dx * dx + dy * dy; + double D = x1 * y2 - x2 * y1; + + // Augmentation to only return point within segment + double d1 = x1 * x1 + y1 * y1; + double d2 = x2 * x2 + y2 * y2; + double dd = d2 - d1; + + geometry_msgs::msg::Point p; + double sqrt_term = std::sqrt(r * r * dr2 - D * D); + p.x = (D * dy + std::copysign(1.0, dd) * dx * sqrt_term) / dr2; + p.y = (-D * dx + std::copysign(1.0, dd) * dy * sqrt_term) / dr2; + return p; +} + +geometry_msgs::msg::Point linearInterpolation( + const geometry_msgs::msg::Point & p1, + const geometry_msgs::msg::Point & p2, + const double target_dist) +{ + geometry_msgs::msg::Point result; + + double dx = p2.x - p1.x; + double dy = p2.y - p1.y; + double d_dist = std::hypot(dx, dy); + + double target_ratio = target_dist / d_dist; + + result.x = p1.x + target_ratio * dx; + result.y = p1.y + target_ratio * dy; + return result; +} + + +geometry_msgs::msg::PoseStamped getLookAheadPoint( + double & lookahead_dist, + const nav_msgs::msg::Path & transformed_plan, + const bool interpolate_after_goal) +{ + // Find the first pose which is at a distance greater than the lookahead distance + // Using distance along the path + const auto & poses = transformed_plan.poses; + auto goal_pose_it = poses.begin(); + double path_dist = 0.0; + double interpolation_dist = 0.0; + + bool pose_found = false; + for (size_t i = 1; i < poses.size(); i++) { + const auto & prev_pose = poses[i - 1].pose.position; + const auto & curr_pose = poses[i].pose.position; + + const double d = std::hypot(curr_pose.x - prev_pose.x, curr_pose.y - prev_pose.y); + if (path_dist + d >= lookahead_dist) { + goal_pose_it = poses.begin() + i; + pose_found = true; + break; + } + + path_dist += d; + } + + interpolation_dist = lookahead_dist - path_dist; + if (!pose_found) { + goal_pose_it = poses.end(); + } + + // If the no pose is not far enough, take the last pose + if (goal_pose_it == transformed_plan.poses.end()) { + if (interpolate_after_goal) { + auto last_pose_it = std::prev(transformed_plan.poses.end()); + auto prev_last_pose_it = std::prev(last_pose_it); + + double end_path_orientation = atan2( + last_pose_it->pose.position.y - prev_last_pose_it->pose.position.y, + last_pose_it->pose.position.x - prev_last_pose_it->pose.position.x); + + // Project the last segment out to guarantee it is beyond the look ahead + // distance + auto projected_position = last_pose_it->pose.position; + projected_position.x += cos(end_path_orientation) * lookahead_dist; + projected_position.y += sin(end_path_orientation) * lookahead_dist; + + // Use the linear interpolation to find the position at the correct look + // ahead distance + const auto interpolated_position = linearInterpolation( + last_pose_it->pose.position, projected_position, interpolation_dist); + + geometry_msgs::msg::PoseStamped interpolated_pose; + interpolated_pose.header = last_pose_it->header; + interpolated_pose.pose.position = interpolated_position; + + return interpolated_pose; + } else { + lookahead_dist = path_dist; // Updating lookahead distance since using the final point + goal_pose_it = std::prev(transformed_plan.poses.end()); + } + } else if (goal_pose_it != transformed_plan.poses.begin()) { + // Find the point on the robot path + // that is exactly the lookahead distance away from the robot pose (the origin) + // This can be found with a linear interpolation between the prev_pose and + // the goal_pose, moving interpolation_dist starting from prev_pose in the + // direction of goal_pose. + auto prev_pose_it = std::prev(goal_pose_it); + auto point = linearInterpolation( + prev_pose_it->pose.position, + goal_pose_it->pose.position, interpolation_dist); + + // Calculate orientation towards interpolated position + // Convert yaw to quaternion + double yaw = atan2( + point.y - prev_pose_it->pose.position.y, + point.x - prev_pose_it->pose.position.x); + + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = prev_pose_it->header.frame_id; + pose.header.stamp = goal_pose_it->header.stamp; + pose.pose.position = point; + pose.pose.orientation = geometry_utils::orientationAroundZAxis(yaw); + return pose; + } + + return *goal_pose_it; +} +} // namespace nav2_util diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 912d73d0859..d333df9cf14 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -50,6 +50,11 @@ bool transformPoseInTargetFrame( { static rclcpp::Logger logger = rclcpp::get_logger("transformPoseInTargetFrame"); + if(input_pose.header.frame_id == target_frame) { + transformed_pose = input_pose; + return true; + } + try { transformed_pose = tf_buffer.transform( input_pose, target_frame, diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 4527fd8373d..c8447eaab6e 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -1,3 +1,5 @@ +add_subdirectory(regression) + ament_add_gtest(test_execution_timer test_execution_timer.cpp) target_link_libraries(test_execution_timer ${library_name}) @@ -16,6 +18,9 @@ target_link_libraries(test_odometry_utils ${library_name} ${nav_msgs_TARGETS} ${ ament_add_gtest(test_robot_utils test_robot_utils.cpp) target_link_libraries(test_robot_utils ${library_name} ${geometry_msgs_TARGETS}) +ament_add_gtest(test_controller_utils test_controller_utils.cpp) +target_link_libraries(test_controller_utils ${library_name} ${nav_msgs_TARGETS} ${geometry_msgs_TARGETS}) + ament_add_gtest(test_base_footprint_publisher test_base_footprint_publisher.cpp) target_include_directories(test_base_footprint_publisher PRIVATE "$") diff --git a/nav2_util/test/regression/CMakeLists.txt b/nav2_util/test/regression/CMakeLists.txt new file mode 100644 index 00000000000..f216fbe7751 --- /dev/null +++ b/nav2_util/test/regression/CMakeLists.txt @@ -0,0 +1,3 @@ +# Bresenham2D corner cases test +ament_add_gtest(map_bresenham_2d map_bresenham_2d.cpp) +target_link_libraries(map_bresenham_2d ${library_name}) diff --git a/nav2_util/test/regression/map_bresenham_2d.cpp b/nav2_util/test/regression/map_bresenham_2d.cpp new file mode 100644 index 00000000000..220962948e5 --- /dev/null +++ b/nav2_util/test/regression/map_bresenham_2d.cpp @@ -0,0 +1,170 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Alexey Merzlyakov + +#include + +#include + +#include "nav2_util/raytrace_line_2d.hpp" + +// MapAction - is a functor class used to cover raytraceLine algorithm. +// It contains char map inside, which is an abstract one and not related +// to any concrete representation (like Costmap2D or OccupancyGrid). +class MapAction +{ +public: + explicit MapAction( + char * map, unsigned int size, char mark_val = 100) + : map_(map), size_(size), mark_val_(mark_val) + { + } + + inline void operator()(unsigned int off) + { + ASSERT_TRUE(off < size_); + map_[off] = mark_val_; + } + + inline unsigned int get(unsigned int off) + { + return map_[off]; + } + +private: + char * map_; + unsigned int size_; + char mark_val_; +}; + +class MapTest +{ +public: + MapTest( + unsigned int size_x, unsigned int size_y, + char default_val = 0) + : size_x_(size_x), size_y_(size_y) + { + map_ = new char[size_x * size_y]; + memset(map_, default_val, size_x * size_y); + } + + char * getMap() + { + return map_; + } + + unsigned int getSize() + { + return size_x_ * size_y_; + } + + void raytraceLine( + MapAction ma, unsigned int x0, unsigned int y0, unsigned int x1, + unsigned int y1, + unsigned int max_length = UINT_MAX, unsigned int min_length = 0) + { + nav2_util::raytraceLine(ma, x0, y0, x1, y1, size_x_, max_length, min_length); + } + +private: + char * map_; + unsigned int size_x_, size_y_; +}; + +TEST(map_2d, bresenham2DBoundariesCheck) +{ + const unsigned int sz_x = 60; + const unsigned int sz_y = 60; + const unsigned int max_length = 60; + const unsigned int min_length = 6; + MapTest mt(sz_x, sz_y); + MapAction ma(mt.getMap(), mt.getSize()); + + // Initial point - some asymmetrically standing point in order to cover most corner cases + const unsigned int x0 = 2; + const unsigned int y0 = 4; + // (x1, y1) point will move + unsigned int x1, y1; + + // Running on (x, 0) edge + y1 = 0; + for (x1 = 0; x1 < sz_x; x1++) { + mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (x, sz_y) edge + y1 = sz_y - 1; + for (x1 = 0; x1 < sz_x; x1++) { + mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (0, y) edge + x1 = 0; + for (y1 = 0; y1 < sz_y; y1++) { + mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (sz_x, y) edge + x1 = sz_x - 1; + for (y1 = 0; y1 < sz_y; y1++) { + mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length); + } +} + +TEST(map_2d, bresenham2DSamePoint) +{ + const unsigned int sz_x = 60; + const unsigned int sz_y = 60; + const unsigned int max_length = 60; + const unsigned int min_length = 0; + MapTest mt(sz_x, sz_y, 0.1); + MapAction ma(mt.getMap(), mt.getSize()); + + // Initial point + const double x0 = 2; + const double y0 = 4; + + unsigned int offset = y0 * sz_x + x0; + char val_before = ma.get(offset); + // Same point to check + mt.raytraceLine(ma, x0, y0, x0, y0, max_length, min_length); + char val_after = ma.get(offset); + ASSERT_FALSE(val_before == val_after); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/nav2_util/test/test_controller_utils.cpp b/nav2_util/test/test_controller_utils.cpp new file mode 100644 index 00000000000..3c13d8e9bbc --- /dev/null +++ b/nav2_util/test/test_controller_utils.cpp @@ -0,0 +1,265 @@ +// Copyright (c) 2020 Samsung Research +// +// 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 "nav2_util/controller_utils.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "gtest/gtest.h" + + +using CircleSegmentIntersectionParam = std::tuple< + std::pair, + std::pair, + double, + std::pair +>; + +class CircleSegmentIntersectionTest + : public ::testing::TestWithParam +{}; + +TEST_P(CircleSegmentIntersectionTest, circleSegmentIntersection) +{ + auto pair1 = std::get<0>(GetParam()); + auto pair2 = std::get<1>(GetParam()); + auto r = std::get<2>(GetParam()); + auto expected_pair = std::get<3>(GetParam()); + auto pair_to_point = [](std::pair p) -> geometry_msgs::msg::Point { + geometry_msgs::msg::Point point; + point.x = p.first; + point.y = p.second; + point.z = 0.0; + return point; + }; + auto p1 = pair_to_point(pair1); + auto p2 = pair_to_point(pair2); + auto actual = nav2_util::circleSegmentIntersection(p1, p2, r); + auto expected_point = pair_to_point(expected_pair); + EXPECT_DOUBLE_EQ(actual.x, expected_point.x); + EXPECT_DOUBLE_EQ(actual.y, expected_point.y); + // Expect that the intersection point is actually r away from the origin + EXPECT_DOUBLE_EQ(r, std::hypot(actual.x, actual.y)); +} + +INSTANTIATE_TEST_SUITE_P( + InterpolationTest, + CircleSegmentIntersectionTest, + testing::Values( + // Origin to the positive X axis + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {2.0, 0.0}, + 1.0, + {1.0, 0.0} +}, + // Origin to the negative X axis + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {-2.0, 0.0}, + 1.0, + {-1.0, 0.0} +}, + // Origin to the positive Y axis + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {0.0, 2.0}, + 1.0, + {0.0, 1.0} +}, + // Origin to the negative Y axis + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {0.0, -2.0}, + 1.0, + {0.0, -1.0} +}, + // non-origin to the X axis with non-unit circle, with the second point inside + CircleSegmentIntersectionParam{ + {4.0, 0.0}, + {-1.0, 0.0}, + 2.0, + {2.0, 0.0} +}, + // non-origin to the Y axis with non-unit circle, with the second point inside + CircleSegmentIntersectionParam{ + {0.0, 4.0}, + {0.0, -0.5}, + 2.0, + {0.0, 2.0} +}, + // origin to the positive X axis, on the circle + CircleSegmentIntersectionParam{ + {2.0, 0.0}, + {0.0, 0.0}, + 2.0, + {2.0, 0.0} +}, + // origin to the positive Y axis, on the circle + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {0.0, 2.0}, + 2.0, + {0.0, 2.0} +}, + // origin to the upper-right quadrant (3-4-5 triangle) + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {6.0, 8.0}, + 5.0, + {3.0, 4.0} +}, + // origin to the lower-left quadrant (3-4-5 triangle) + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {-6.0, -8.0}, + 5.0, + {-3.0, -4.0} +}, + // origin to the upper-left quadrant (3-4-5 triangle) + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {-6.0, 8.0}, + 5.0, + {-3.0, 4.0} +}, + // origin to the lower-right quadrant (3-4-5 triangle) + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {6.0, -8.0}, + 5.0, + {3.0, -4.0} +} +)); + +TEST(InterpolationUtils, lookaheadInterpolation) +{ + // test Lookahead Point Interpolation + double dist = 1.0; + nav_msgs::msg::Path path; + path.poses.resize(10); + for (uint i = 0; i != path.poses.size(); i++) { + path.poses[i].pose.position.x = static_cast(i); + } + + // test exact hits + auto pt = nav2_util::getLookAheadPoint(dist, path); + EXPECT_EQ(pt.pose.position.x, 1.0); + + // test interpolation + dist = 3.8; + pt = nav2_util::getLookAheadPoint(dist, path); + EXPECT_EQ(pt.pose.position.x, 3.8); +} + +TEST(InterpolationUtils, lookaheadExtrapolation) +{ + // Test extrapolation beyond goal + double EPSILON = std::numeric_limits::epsilon(); + + nav_msgs::msg::Path path; + double lookahead_dist = 10.0; + // More than 2 poses + path.poses.resize(4); + path.poses[0].pose.position.x = 0.0; + path.poses[1].pose.position.x = 1.0; + path.poses[2].pose.position.x = 2.0; + path.poses[3].pose.position.x = 3.0; + auto pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true); + EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON); + + // 2 poses fwd + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = 2.0; + path.poses[1].pose.position.x = 3.0; + pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true); + EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON); + + // 2 poses at 45° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = 2.0; + path.poses[0].pose.position.y = 2.0; + path.poses[1].pose.position.x = 3.0; + path.poses[1].pose.position.y = 3.0; + pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true); + EXPECT_NEAR(pt.pose.position.x, cos(45.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(45.0 * M_PI / 180) * 10.0, EPSILON); + + // 2 poses at 90° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = 0.0; + path.poses[0].pose.position.y = 2.0; + path.poses[1].pose.position.x = 0.0; + path.poses[1].pose.position.y = 3.0; + pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true); + EXPECT_NEAR(pt.pose.position.x, cos(90.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(90.0 * M_PI / 180) * 10.0, EPSILON); + + // 2 poses at 135° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = -2.0; + path.poses[0].pose.position.y = 2.0; + path.poses[1].pose.position.x = -3.0; + path.poses[1].pose.position.y = 3.0; + pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true); + EXPECT_NEAR(pt.pose.position.x, cos(135.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(135.0 * M_PI / 180) * 10.0, EPSILON); + + // 2 poses back + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = -2.0; + path.poses[1].pose.position.x = -3.0; + pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true); + EXPECT_NEAR(pt.pose.position.x, -10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON); + + // 2 poses at -135° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = -2.0; + path.poses[0].pose.position.y = -2.0; + path.poses[1].pose.position.x = -3.0; + path.poses[1].pose.position.y = -3.0; + pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true); + EXPECT_NEAR(pt.pose.position.x, cos(-135.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(-135.0 * M_PI / 180) * 10.0, EPSILON); + + // 2 poses at -90° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = 0.0; + path.poses[0].pose.position.y = -2.0; + path.poses[1].pose.position.x = 0.0; + path.poses[1].pose.position.y = -3.0; + pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true); + EXPECT_NEAR(pt.pose.position.x, cos(-90.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(-90.0 * M_PI / 180) * 10.0, EPSILON); + + // 2 poses at -45° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = 2.0; + path.poses[0].pose.position.y = -2.0; + path.poses[1].pose.position.x = 3.0; + path.poses[1].pose.position.y = -3.0; + pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true); + EXPECT_NEAR(pt.pose.position.x, cos(-45.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(-45.0 * M_PI / 180) * 10.0, EPSILON); +} diff --git a/nav2_util/test/test_robot_utils.cpp b/nav2_util/test/test_robot_utils.cpp index 119addc9355..33c55aa2a3b 100644 --- a/nav2_util/test/test_robot_utils.cpp +++ b/nav2_util/test/test_robot_utils.cpp @@ -16,11 +16,11 @@ #include #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_util/robot_utils.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "gtest/gtest.h" -#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/create_timer_ros.hpp" TEST(RobotUtils, LookupExceptionError) { diff --git a/nav2_velocity_smoother/CMakeLists.txt b/nav2_velocity_smoother/CMakeLists.txt index 75c59b8a89c..f7437297d42 100644 --- a/nav2_velocity_smoother/CMakeLists.txt +++ b/nav2_velocity_smoother/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_velocity_smoother) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_util REQUIRED) diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index b0ed66dd3e6..52cfb61d9d7 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -10,6 +10,7 @@ ament_cmake nav2_common + backward_ros geometry_msgs nav2_util rclcpp diff --git a/nav2_voxel_grid/CMakeLists.txt b/nav2_voxel_grid/CMakeLists.txt index e204367ada8..e4d6d1866a8 100644 --- a/nav2_voxel_grid/CMakeLists.txt +++ b/nav2_voxel_grid/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_voxel_grid) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index ef9eedd9b42..36d5b14ba6b 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -12,6 +12,7 @@ ament_cmake nav2_common + backward_ros rclcpp ament_lint_common diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index f41c6e5f730..aafd4548a5e 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -1,7 +1,8 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(nav2_waypoint_follower) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(cv_bridge REQUIRED) find_package(geographic_msgs REQUIRED) find_package(geometry_msgs REQUIRED) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 83edf012880..816feb9eca3 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -37,9 +37,9 @@ #include "nav2_core/waypoint_task_executor.hpp" #include "robot_localization/srv/from_ll.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_listener.hpp" namespace nav2_waypoint_follower { diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 4aa90590587..980881aa7a9 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -10,6 +10,7 @@ ament_cmake nav2_common + backward_ros cv_bridge geographic_msgs geometry_msgs diff --git a/navigation2/CMakeLists.txt b/navigation2/CMakeLists.txt index 3b82e6c0027..ac8cac17ffa 100644 --- a/navigation2/CMakeLists.txt +++ b/navigation2/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(navigation2) find_package(ament_cmake REQUIRED) diff --git a/tools/smoother_benchmarking/smoother_benchmark_bringup.py b/tools/smoother_benchmarking/smoother_benchmark_bringup.py index 86baea19ce5..5506b8a1f74 100644 --- a/tools/smoother_benchmarking/smoother_benchmark_bringup.py +++ b/tools/smoother_benchmarking/smoother_benchmark_bringup.py @@ -19,6 +19,7 @@ from launch.actions import ExecuteProcess, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node +from nav2_common.launch import RewrittenYaml def generate_launch_description() -> LaunchDescription: @@ -30,6 +31,14 @@ def generate_launch_description() -> LaunchDescription: ) map_file = os.path.join(benchmark_dir, 'maps', 'smoothers_world.yaml') lifecycle_nodes = ['map_server', 'planner_server', 'smoother_server'] + config = RewrittenYaml( + source_file=config, root_key='', param_rewrites={}, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, + convert_types=True + ) static_transform_one = Node( package='tf2_ros',