From 154feba58250840e75000616efc113686effffc5 Mon Sep 17 00:00:00 2001 From: turtlewizard73 Date: Mon, 27 May 2024 12:25:52 +0200 Subject: [PATCH 01/13] critic publisher --- nav2_mppi_controller/CMakeLists.txt | 14 +++++ .../nav2_mppi_controller/controller.hpp | 5 ++ .../nav2_mppi_controller/critic_manager.hpp | 8 ++- .../nav2_mppi_controller/optimizer.hpp | 9 +++ nav2_mppi_controller/msg/CriticScores.msg | 2 + nav2_mppi_controller/package.xml | 5 ++ nav2_mppi_controller/src/controller.cpp | 31 ++++++++++ nav2_mppi_controller/src/critic_manager.cpp | 30 +++++++++ nav2_mppi_controller/src/optimizer.cpp | 61 +++++++++++++++++++ 9 files changed, 163 insertions(+), 2 deletions(-) create mode 100644 nav2_mppi_controller/msg/CriticScores.msg diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 3ca6735e40b..1ce40c2cfa1 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -14,6 +14,7 @@ set(XTENSOR_USE_XSIMD 1) find_package(ament_cmake REQUIRED) find_package(xtensor REQUIRED) find_package(xsimd REQUIRED) +find_package(rosidl_default_generators REQUIRED) include_directories( include @@ -27,12 +28,14 @@ set(dependencies_pkgs geometry_msgs visualization_msgs nav_msgs + nav2_msgs nav2_core nav2_costmap_2d nav2_util tf2_geometry_msgs tf2_eigen tf2_ros + std_msgs ) foreach(pkg IN LISTS dependencies_pkgs) @@ -41,6 +44,11 @@ endforeach() nav2_package() +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/CriticScores.msg" + DEPENDENCIES std_msgs +) + include(CheckCXXCompilerFlag) check_cxx_compiler_flag("-mno-avx512f" COMPILER_SUPPORTS_AVX512) @@ -120,8 +128,14 @@ if(BUILD_TESTING) # add_subdirectory(benchmark) endif() +rosidl_get_typesupport_target(cpp_typesupport_target + ${PROJECT_NAME} rosidl_typesupport_cpp) + +target_link_libraries(mppi_controller "${cpp_typesupport_target}") + ament_export_libraries(${libraries}) ament_export_dependencies(${dependencies_pkgs}) +ament_export_dependencies(rosidl_default_runtime) ament_export_include_directories(include) pluginlib_export_plugin_description_file(nav2_core mppic.xml) pluginlib_export_plugin_description_file(nav2_mppi_controller critics.xml) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index 2d7f2aa8ca4..ea6daa02f30 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -23,6 +23,7 @@ #include "nav2_mppi_controller/tools/trajectory_visualizer.hpp" #include "nav2_mppi_controller/models/constraints.hpp" #include "nav2_mppi_controller/tools/utils.hpp" +#include "nav2_mppi_controller/msg/critic_scores.hpp" #include "nav2_core/controller.hpp" #include "nav2_core/goal_checker.hpp" @@ -121,10 +122,14 @@ class MPPIController : public nav2_core::Controller TrajectoryVisualizer trajectory_visualizer_; bool visualize_; + bool publish_critics_; double reset_period_; // Last time computeVelocityCommands was called rclcpp::Time last_time_called_; + + std::shared_ptr> + critics_publisher_; }; } // namespace nav2_mppi_controller 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 b2e2c178d96..c6903846e80 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp @@ -46,12 +46,12 @@ class CriticManager * @brief Constructor for mppi::CriticManager */ CriticManager() = default; - + /** * @brief Virtual Destructor for mppi::CriticManager */ virtual ~CriticManager() = default; - + /** * @brief Configure critic manager on bringup and load plugins * @param parent WeakPtr to node @@ -69,6 +69,10 @@ class CriticManager */ void evalTrajectoriesScores(CriticData & data) const; + xt::xtensor evalTrajectory(CriticData & data) const; + + std::vector getCriticNames() const; + protected: /** * @brief Get parameters (critics to load) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index b7200575f70..d2cb56903ef 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -104,6 +104,15 @@ class Optimizer */ xt::xtensor getOptimizedTrajectory(); + + /** + * @brief Get the critic costs for given trajectory + * @return Names and costs of the critics + */ + xt::xtensor getOptimizationResults(); + + std::vector getCriticNames() const; + /** * @brief Set the maximum speed based on the speed limits callback * @param speed_limit Limit of the speed for use diff --git a/nav2_mppi_controller/msg/CriticScores.msg b/nav2_mppi_controller/msg/CriticScores.msg new file mode 100644 index 00000000000..5a908a39132 --- /dev/null +++ b/nav2_mppi_controller/msg/CriticScores.msg @@ -0,0 +1,2 @@ +std_msgs/String[] critic_names +std_msgs/Float32[] critic_scores \ No newline at end of file diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index da7fbf07cb3..f56e1f95fb2 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -10,6 +10,9 @@ ament_cmake ament_cmake_ros + rosidl_default_generators + + rosidl_default_runtime rclcpp nav2_common @@ -33,6 +36,8 @@ ament_lint_auto ament_lint_common ament_cmake_gtest + + rosidl_interface_packages ament_cmake diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 8a631f97526..71d9be21108 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -39,6 +39,7 @@ void MPPIController::configure( // Get high-level controller parameters auto getParam = parameters_handler_->getParamGetter(name_); getParam(visualize_, "visualize", false); + getParam(publish_critics_, "publish_critics", false); getParam(reset_period_, "reset_period", 1.0); // Configure composed objects @@ -48,6 +49,11 @@ void MPPIController::configure( parent_, name_, costmap_ros_->getGlobalFrameID(), parameters_handler_.get()); + if (publish_critics_) { + critics_publisher_ = node->create_publisher( + "/mppi_critic_scores", 1); + } + RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str()); } @@ -61,6 +67,7 @@ void MPPIController::cleanup() void MPPIController::activate() { + critics_publisher_->on_activate(); trajectory_visualizer_.on_activate(); parameters_handler_->start(); RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str()); @@ -68,6 +75,7 @@ void MPPIController::activate() void MPPIController::deactivate() { + critics_publisher_->on_deactivate(); trajectory_visualizer_.on_deactivate(); RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str()); } @@ -110,6 +118,29 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( visualize(std::move(transformed_plan)); } + if (publish_critics_) { + std::vector critic_names = optimizer_.getCriticNames(); + xt::xtensor critic_costs = optimizer_.getOptimizationResults(); + + // log critic names and costs + for (size_t i = 0; i < critic_names.size(); i++) { + RCLCPP_INFO(logger_, "Critic: %s, Cost: %f", critic_names[i].c_str(), critic_costs[i]); + } + + // make msg + auto critic_scores_ = std::make_unique(); + for (size_t i = 0; i < critic_names.size(); i++) { + std_msgs::msg::String name_msg; + name_msg.data = critic_names[i]; + critic_scores_->critic_names.push_back(std::move(name_msg)); + + std_msgs::msg::Float32 cost_msg; + cost_msg.data = critic_costs[i]; + critic_scores_->critic_scores.push_back(std::move(cost_msg)); + } + critics_publisher_->publish(std::move(critic_scores_)); + } + return cmd; } diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp index 2a7a77a2346..3952a97b080 100644 --- a/nav2_mppi_controller/src/critic_manager.cpp +++ b/nav2_mppi_controller/src/critic_manager.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "nav2_mppi_controller/critic_manager.hpp" namespace mppi @@ -64,6 +66,11 @@ std::string CriticManager::getFullName(const std::string & name) return "mppi::critics::" + name; } +std::vector CriticManager::getCriticNames() const +{ + return critic_names_; +} + void CriticManager::evalTrajectoriesScores( CriticData & data) const { @@ -75,4 +82,27 @@ void CriticManager::evalTrajectoriesScores( } } +xt::xtensor CriticManager::evalTrajectory( + CriticData & data) const +{ + // log the critic_scores shape + RCLCPP_INFO(logger_, "(BEFORE FOR)"); + + xt::xtensor critic_scores = xt::zeros({critics_.size()}); + + for (size_t q = 0; q < critics_.size(); q++) { + if (data.fail_flag) { + break; + } + data.costs = xt::zeros({1}); + // log costs values + critics_[q]->score(data); + critic_scores(q) = data.costs[0]; + } + // log the for cycle finished in criticmanager + RCLCPP_INFO(logger_, "CriticManager: Critic evaluation (FOR) finished"); + + return critic_scores; +} + } // namespace mppi diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 3de703bcc44..51c1ebe9fcc 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -161,6 +161,67 @@ void Optimizer::optimize() } } +xt::xtensor Optimizer::getOptimizationResults() +{ + const xt::xtensor optimized_trajectory = getOptimizedTrajectory(); + xt::xtensor costs = xt::zeros({1}); + + /*auto size = optimized_trajectory.size(); // size = 6 + auto dim = optimized_trajectory.dimension(); // dim = 2 + auto shape = optimized_trajectory.shape(); // shape = {2, 3} + + //log size, dim, and shape + RCLCPP_INFO( + logger_, "getOptimizedTrajectory() size: %ld, dim: %ld, shape: {%ld, %ld}", + size, dim, shape[0], shape[1]);*/ + + models::Trajectories dummy_trajectories; + /*size = dummy_trajectories.x.size(); + dim = dummy_trajectories.x.dimension(); + shape = dummy_trajectories.x.shape(); + RCLCPP_INFO( + logger_, "[after creation] dummy_trajectories size: %ld, dim: %ld, shape: {%ld, %ld}", + size, dim, shape[0], shape[1]);*/ + + dummy_trajectories.reset(1, settings_.time_steps); + /*size = dummy_trajectories.x.size(); + dim = dummy_trajectories.x.dimension(); + shape = dummy_trajectories.x.shape(); + RCLCPP_INFO( + logger_, "[after reset] dummy_trajectories size: %ld, dim: %ld, shape: {%ld, %ld}", + size, dim, shape[0], shape[1]);*/ + + dummy_trajectories.x += xt::view(optimized_trajectory, xt::all(), 0); + dummy_trajectories.y += xt::view(optimized_trajectory, xt::all(), 1); + dummy_trajectories.yaws += xt::view(optimized_trajectory, xt::all(), 2); + + /*size = dummy_trajectories.x.size(); + dim = dummy_trajectories.x.dimension(); + shape = dummy_trajectories.x.shape(); + RCLCPP_INFO( + logger_, "[after valuedump] dummy_trajectories size: %ld, dim: %ld, shape: {%ld, %ld}", + size, dim, shape[0], shape[1]);*/ + + CriticData dummy_data = { + state_, dummy_trajectories, path_, costs, settings_.model_dt, + false, critics_data_.goal_checker, critics_data_.motion_model, std::nullopt, std::nullopt}; + // dummy_data.goal_checker = critics_data_.goal_checker; + // dummy_data.motion_model = critics_data_.motion_model; + dummy_data.furthest_reached_path_point.reset(); + dummy_data.path_pts_valid.reset(); + + /*RCLCPP_INFO( + logger_, "dummy_data type: %s", + typeid(dummy_data).name());*/ + + return critic_manager_.evalTrajectory(dummy_data); +} + +std::vector Optimizer::getCriticNames() const +{ + return critic_manager_.getCriticNames(); +} + bool Optimizer::fallback(bool fail) { static size_t counter = 0; From 0a81bebba071cf25fb1b9bf59156df243365c15a Mon Sep 17 00:00:00 2001 From: turtlewizard73 Date: Mon, 27 May 2024 18:20:22 +0200 Subject: [PATCH 02/13] add header to criticscores msg --- nav2_mppi_controller/msg/CriticScores.msg | 1 + nav2_mppi_controller/src/controller.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/nav2_mppi_controller/msg/CriticScores.msg b/nav2_mppi_controller/msg/CriticScores.msg index 5a908a39132..4797457b2f6 100644 --- a/nav2_mppi_controller/msg/CriticScores.msg +++ b/nav2_mppi_controller/msg/CriticScores.msg @@ -1,2 +1,3 @@ +std_msgs/Header header # ROS time that this log message was sent. std_msgs/String[] critic_names std_msgs/Float32[] critic_scores \ No newline at end of file diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 71d9be21108..104800c0fd8 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -138,6 +138,7 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( cost_msg.data = critic_costs[i]; critic_scores_->critic_scores.push_back(std::move(cost_msg)); } + critic_scores_->header.stamp = clock_->now(); critics_publisher_->publish(std::move(critic_scores_)); } From 1f8c9b0eaef5210bd7cf61e0ea81a5bcc9d8467e Mon Sep 17 00:00:00 2001 From: turtlewizard73 Date: Wed, 29 May 2024 12:04:58 +0200 Subject: [PATCH 03/13] remove unnecessary logging --- nav2_mppi_controller/CMakeLists.txt | 1 - nav2_mppi_controller/src/controller.cpp | 2 +- nav2_mppi_controller/src/critic_manager.cpp | 5 ----- 3 files changed, 1 insertion(+), 7 deletions(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 1ce40c2cfa1..a315850fdb1 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -28,7 +28,6 @@ set(dependencies_pkgs geometry_msgs visualization_msgs nav_msgs - nav2_msgs nav2_core nav2_costmap_2d nav2_util diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 104800c0fd8..859301792c8 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -124,7 +124,7 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( // log critic names and costs for (size_t i = 0; i < critic_names.size(); i++) { - RCLCPP_INFO(logger_, "Critic: %s, Cost: %f", critic_names[i].c_str(), critic_costs[i]); + RCLCPP_DEBUG(logger_, "Critic: %s, Cost: %f", critic_names[i].c_str(), critic_costs[i]); } // make msg diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp index 3952a97b080..85f521f38a4 100644 --- a/nav2_mppi_controller/src/critic_manager.cpp +++ b/nav2_mppi_controller/src/critic_manager.cpp @@ -85,9 +85,6 @@ void CriticManager::evalTrajectoriesScores( xt::xtensor CriticManager::evalTrajectory( CriticData & data) const { - // log the critic_scores shape - RCLCPP_INFO(logger_, "(BEFORE FOR)"); - xt::xtensor critic_scores = xt::zeros({critics_.size()}); for (size_t q = 0; q < critics_.size(); q++) { @@ -99,8 +96,6 @@ xt::xtensor CriticManager::evalTrajectory( critics_[q]->score(data); critic_scores(q) = data.costs[0]; } - // log the for cycle finished in criticmanager - RCLCPP_INFO(logger_, "CriticManager: Critic evaluation (FOR) finished"); return critic_scores; } From c11a79d3957aabef1c7f7ece3bb92793f293fd7f Mon Sep 17 00:00:00 2001 From: turtlewizard Date: Thu, 24 Oct 2024 13:06:46 +0200 Subject: [PATCH 04/13] new scorecritic single msg --- nav2_mppi_controller/CMakeLists.txt | 1 + .../nav2_mppi_controller/controller.hpp | 1 + nav2_mppi_controller/msg/CriticScore.msg | 2 ++ nav2_mppi_controller/msg/CriticScores.msg | 5 +++-- nav2_mppi_controller/src/controller.cpp | 20 ++++++++++++------- nav2_mppi_controller/src/critic_manager.cpp | 2 ++ 6 files changed, 22 insertions(+), 9 deletions(-) create mode 100644 nav2_mppi_controller/msg/CriticScore.msg diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index a315850fdb1..dac13f6edef 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -44,6 +44,7 @@ endforeach() nav2_package() rosidl_generate_interfaces(${PROJECT_NAME} + "msg/CriticScore.msg" "msg/CriticScores.msg" DEPENDENCIES std_msgs ) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index ea6daa02f30..11b5a3875da 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -23,6 +23,7 @@ #include "nav2_mppi_controller/tools/trajectory_visualizer.hpp" #include "nav2_mppi_controller/models/constraints.hpp" #include "nav2_mppi_controller/tools/utils.hpp" +#include "nav2_mppi_controller/msg/critic_score.hpp" #include "nav2_mppi_controller/msg/critic_scores.hpp" #include "nav2_core/controller.hpp" diff --git a/nav2_mppi_controller/msg/CriticScore.msg b/nav2_mppi_controller/msg/CriticScore.msg new file mode 100644 index 00000000000..9a8719acaed --- /dev/null +++ b/nav2_mppi_controller/msg/CriticScore.msg @@ -0,0 +1,2 @@ +std_msgs/String name +std_msgs/Float32 score \ No newline at end of file diff --git a/nav2_mppi_controller/msg/CriticScores.msg b/nav2_mppi_controller/msg/CriticScores.msg index 4797457b2f6..39741448a6f 100644 --- a/nav2_mppi_controller/msg/CriticScores.msg +++ b/nav2_mppi_controller/msg/CriticScores.msg @@ -1,3 +1,4 @@ std_msgs/Header header # ROS time that this log message was sent. -std_msgs/String[] critic_names -std_msgs/Float32[] critic_scores \ No newline at end of file +CriticScore[] critic_scores +# std_msgs/String[] critic_names +# std_msgs/Float32[] critic_scores \ No newline at end of file diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 859301792c8..a1a998ba099 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -129,15 +129,21 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( // make msg auto critic_scores_ = std::make_unique(); - for (size_t i = 0; i < critic_names.size(); i++) { - std_msgs::msg::String name_msg; - name_msg.data = critic_names[i]; - critic_scores_->critic_names.push_back(std::move(name_msg)); + if (critic_names.size() != critic_costs.size()) { + RCLCPP_ERROR( + logger_, + "Critic names %ld and costs %ld size mismatch!", + critic_names.size(), critic_costs.size()); + return cmd; + } - std_msgs::msg::Float32 cost_msg; - cost_msg.data = critic_costs[i]; - critic_scores_->critic_scores.push_back(std::move(cost_msg)); + for (size_t i = 0; i < critic_names.size(); i++) { + nav2_mppi_controller::msg::CriticScore critic_score; + critic_score.name.data = critic_names[i]; + critic_score.score.data = critic_costs[i]; + critic_scores_->critic_scores.push_back(critic_score); } + critic_scores_->header.stamp = clock_->now(); critics_publisher_->publish(std::move(critic_scores_)); } diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp index 85f521f38a4..a11bd902034 100644 --- a/nav2_mppi_controller/src/critic_manager.cpp +++ b/nav2_mppi_controller/src/critic_manager.cpp @@ -96,6 +96,8 @@ xt::xtensor CriticManager::evalTrajectory( critics_[q]->score(data); critic_scores(q) = data.costs[0]; } + // log the for cycle finished in criticmanager + RCLCPP_DEBUG(logger_, "CriticManager: Critic evaluation (FOR) finished"); return critic_scores; } From 95cd8e3e8c604bcbd4e2f059cd1039d6ed9b7820 Mon Sep 17 00:00:00 2001 From: turtlewizard73 Date: Sat, 14 Dec 2024 14:16:26 +0100 Subject: [PATCH 05/13] clean up comments and linting --- nav2_mppi_controller/msg/CriticScore.msg | 2 +- nav2_mppi_controller/msg/CriticScores.msg | 4 +-- nav2_mppi_controller/src/optimizer.cpp | 41 +++-------------------- 3 files changed, 7 insertions(+), 40 deletions(-) diff --git a/nav2_mppi_controller/msg/CriticScore.msg b/nav2_mppi_controller/msg/CriticScore.msg index 9a8719acaed..fdc2901a901 100644 --- a/nav2_mppi_controller/msg/CriticScore.msg +++ b/nav2_mppi_controller/msg/CriticScore.msg @@ -1,2 +1,2 @@ std_msgs/String name -std_msgs/Float32 score \ No newline at end of file +std_msgs/Float32 score diff --git a/nav2_mppi_controller/msg/CriticScores.msg b/nav2_mppi_controller/msg/CriticScores.msg index 39741448a6f..4da10efe3db 100644 --- a/nav2_mppi_controller/msg/CriticScores.msg +++ b/nav2_mppi_controller/msg/CriticScores.msg @@ -1,4 +1,2 @@ -std_msgs/Header header # ROS time that this log message was sent. +std_msgs/Header header # when msg was sent CriticScore[] critic_scores -# std_msgs/String[] critic_names -# std_msgs/Float32[] critic_scores \ No newline at end of file diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 51c1ebe9fcc..c2e66c2fa06 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -163,57 +163,26 @@ void Optimizer::optimize() xt::xtensor Optimizer::getOptimizationResults() { + // get the final optimized trajectory const xt::xtensor optimized_trajectory = getOptimizedTrajectory(); xt::xtensor costs = xt::zeros({1}); - /*auto size = optimized_trajectory.size(); // size = 6 - auto dim = optimized_trajectory.dimension(); // dim = 2 - auto shape = optimized_trajectory.shape(); // shape = {2, 3} - - //log size, dim, and shape - RCLCPP_INFO( - logger_, "getOptimizedTrajectory() size: %ld, dim: %ld, shape: {%ld, %ld}", - size, dim, shape[0], shape[1]);*/ - + // evalTrajectory evals multiple trajectories, but we only have one + // create a dummy_trajectories object and put the optimized in it models::Trajectories dummy_trajectories; - /*size = dummy_trajectories.x.size(); - dim = dummy_trajectories.x.dimension(); - shape = dummy_trajectories.x.shape(); - RCLCPP_INFO( - logger_, "[after creation] dummy_trajectories size: %ld, dim: %ld, shape: {%ld, %ld}", - size, dim, shape[0], shape[1]);*/ - dummy_trajectories.reset(1, settings_.time_steps); - /*size = dummy_trajectories.x.size(); - dim = dummy_trajectories.x.dimension(); - shape = dummy_trajectories.x.shape(); - RCLCPP_INFO( - logger_, "[after reset] dummy_trajectories size: %ld, dim: %ld, shape: {%ld, %ld}", - size, dim, shape[0], shape[1]);*/ - dummy_trajectories.x += xt::view(optimized_trajectory, xt::all(), 0); dummy_trajectories.y += xt::view(optimized_trajectory, xt::all(), 1); dummy_trajectories.yaws += xt::view(optimized_trajectory, xt::all(), 2); - /*size = dummy_trajectories.x.size(); - dim = dummy_trajectories.x.dimension(); - shape = dummy_trajectories.x.shape(); - RCLCPP_INFO( - logger_, "[after valuedump] dummy_trajectories size: %ld, dim: %ld, shape: {%ld, %ld}", - size, dim, shape[0], shape[1]);*/ - + // create a dummy_data object to pass to evalTrajectory CriticData dummy_data = { state_, dummy_trajectories, path_, costs, settings_.model_dt, false, critics_data_.goal_checker, critics_data_.motion_model, std::nullopt, std::nullopt}; - // dummy_data.goal_checker = critics_data_.goal_checker; - // dummy_data.motion_model = critics_data_.motion_model; dummy_data.furthest_reached_path_point.reset(); dummy_data.path_pts_valid.reset(); - /*RCLCPP_INFO( - logger_, "dummy_data type: %s", - typeid(dummy_data).name());*/ - + // evaluate the optimized trajectory return critic_manager_.evalTrajectory(dummy_data); } From 768a7a6d9893c7024afb05a856d03cb0db3fabfb Mon Sep 17 00:00:00 2001 From: redvinaa Date: Mon, 16 Dec 2024 11:34:55 +0000 Subject: [PATCH 06/13] Fix crash --- nav2_mppi_controller/src/controller.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index a1a998ba099..f526909431d 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -67,7 +67,9 @@ void MPPIController::cleanup() void MPPIController::activate() { - critics_publisher_->on_activate(); + if (publish_critics_) { + critics_publisher_->on_activate(); + } trajectory_visualizer_.on_activate(); parameters_handler_->start(); RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str()); @@ -75,7 +77,9 @@ void MPPIController::activate() void MPPIController::deactivate() { - critics_publisher_->on_deactivate(); + if (publish_critics_) { + critics_publisher_->on_deactivate(); + } trajectory_visualizer_.on_deactivate(); RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str()); } From 551d122efa9fc47c819163feb330abf94f97d3ad Mon Sep 17 00:00:00 2001 From: redvinaa Date: Tue, 17 Dec 2024 15:20:25 +0100 Subject: [PATCH 07/13] Fix another crash --- nav2_mppi_controller/src/critic_manager.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp index a11bd902034..6950c97734b 100644 --- a/nav2_mppi_controller/src/critic_manager.cpp +++ b/nav2_mppi_controller/src/critic_manager.cpp @@ -78,7 +78,13 @@ void CriticManager::evalTrajectoriesScores( if (data.fail_flag) { break; } - critics_[q]->score(data); + + const std::unique_ptr & critic = critics_[q]; + if (!critic) { + continue; + } + + critic->score(data); } } From 860f2ccbd47adbc24699622b67743818afc3a566 Mon Sep 17 00:00:00 2001 From: redvinaa Date: Fri, 20 Dec 2024 15:30:24 +0100 Subject: [PATCH 08/13] Add goal pose to CriticData --- .../nav2_mppi_controller/critic_data.hpp | 5 +- .../nav2_mppi_controller/optimizer.hpp | 11 ++-- .../tools/path_handler.hpp | 6 +++ .../nav2_mppi_controller/tools/utils.hpp | 28 +++++++++++ nav2_mppi_controller/src/controller.cpp | 4 +- .../src/critics/cost_critic.cpp | 2 +- .../src/critics/goal_angle_critic.cpp | 3 +- .../src/critics/goal_critic.cpp | 9 ++-- .../src/critics/obstacles_critic.cpp | 2 +- .../src/critics/path_align_critic.cpp | 3 +- .../src/critics/path_align_legacy_critic.cpp | 3 +- .../src/critics/path_angle_critic.cpp | 2 +- .../src/critics/path_follow_critic.cpp | 2 +- .../src/critics/prefer_forward_critic.cpp | 3 +- nav2_mppi_controller/src/optimizer.cpp | 14 ++++-- nav2_mppi_controller/src/path_handler.cpp | 14 ++++++ .../test/critic_manager_test.cpp | 3 +- nav2_mppi_controller/test/critics_tests.cpp | 50 +++++++++++-------- .../test/optimizer_smoke_test.cpp | 3 +- .../test/optimizer_unit_tests.cpp | 9 ++-- nav2_mppi_controller/test/utils_test.cpp | 30 ++++++++--- 21 files changed, 146 insertions(+), 60 deletions(-) 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 31965eb1cfc..2811cfea5e3 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp @@ -32,14 +32,15 @@ namespace mppi /** * @struct mppi::CriticData - * @brief Data to pass to critics for scoring, including state, trajectories, path, costs, and - * important parameters to share + * @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; xt::xtensor & costs; float & model_dt; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index d2cb56903ef..0c333d76b24 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -90,7 +90,7 @@ class Optimizer geometry_msgs::msg::TwistStamped evalControl( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan, - nav2_core::GoalChecker * goal_checker); + const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker); /** * @brief Get the trajectories generated in a cycle for visualization @@ -141,7 +141,8 @@ class Optimizer void prepare( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, - const nav_msgs::msg::Path & plan, nav2_core::GoalChecker * goal_checker); + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker); /** * @brief Obtain the main controller's parameters @@ -259,10 +260,12 @@ class Optimizer std::array control_history_; models::Trajectories generated_trajectories_; models::Path path_; + geometry_msgs::msg::Pose goal_; xt::xtensor costs_; - CriticData critics_data_ = - {state_, generated_trajectories_, path_, costs_, settings_.model_dt, false, nullptr, nullptr, + 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")}; 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 2c9f3aef8bb..10435e2b3c7 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 @@ -89,6 +89,12 @@ class PathHandler */ nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose); + /** + * @brief Get the global goal pose transformed to the local frame + * @return Transformed goal pose + */ + geometry_msgs::msg::PoseStamped getTransformedGoal(); + protected: /** * @brief Transform a pose to another frame 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 9d8c49a9cfd..cedb0d64c30 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -229,6 +229,11 @@ inline bool withinPositionGoalTolerance( /** * @brief Check if the robot pose is within tolerance to the goal + * + * Deprecated! + * This uses the end of pruned path as the goal, if you want to use + * the global goal, use the other overload + * * @param pose_tolerance Pose tolerance to use * @param robot Pose of robot * @param path Path to retreive goal pose from @@ -257,6 +262,29 @@ inline bool withinPositionGoalTolerance( 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 path Path to retreive goal pose from + * @return bool If robot is within tolerance to the goal + */ +inline bool withinPositionGoalTolerance( + float pose_tolerance, const CriticData & data) +{ + const double & dist_sq = + std::pow(data.goal.position.x - data.state.pose.pose.position.x, 2) + + std::pow(data.goal.position.y - data.state.pose.pose.position.y, 2); + + const auto 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_PI circle to +M_PI circle diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index f526909431d..ee0508422cb 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -104,13 +104,15 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( last_time_called_ = clock_->now(); std::lock_guard param_lock(*parameters_handler_->getLock()); + geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal().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())); geometry_msgs::msg::TwistStamped cmd = - optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal_checker); + optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker); #ifdef BENCHMARK_TESTING auto end = std::chrono::system_clock::now(); diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index a9dd9e9e814..d270e8901be 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -119,7 +119,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, data.path)) { + if (utils::withinPositionGoalTolerance(near_goal_distance_, data)) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 604ee24eb1d..99c2f9c83e1 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -35,8 +35,7 @@ void GoalAngleCritic::initialize() void GoalAngleCritic::score(CriticData & data) { - if (!enabled_ || !utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, data.path)) + if (!enabled_ || !utils::withinPositionGoalTolerance(threshold_to_consider_, data)) { return; } diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index f61d6fd2b21..d530e012d43 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -35,16 +35,13 @@ void GoalCritic::initialize() void GoalCritic::score(CriticData & data) { - if (!enabled_ || !utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, data.path)) + if (!enabled_ || !utils::withinPositionGoalTolerance(threshold_to_consider_, data)) { return; } - const auto goal_idx = data.path.x.shape(0) - 1; - - const auto goal_x = data.path.x(goal_idx); - const auto goal_y = data.path.y(goal_idx); + const auto & goal_x = data.goal.position.x; + const auto & goal_y = data.goal.position.y; const auto traj_x = xt::view(data.trajectories.x, xt::all(), xt::all()); const auto traj_y = xt::view(data.trajectories.y, xt::all(), xt::all()); diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index af1b6636d18..13f977159ba 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -125,7 +125,7 @@ void ObstaclesCritic::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, data.path)) { + if (utils::withinPositionGoalTolerance(near_goal_distance_, data)) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index b103f5b5f64..172590ba321 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -46,8 +46,7 @@ void PathAlignCritic::initialize() void PathAlignCritic::score(CriticData & data) { // Don't apply close to goal, let the goal critics take over - if (!enabled_ || - utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + if (!enabled_ || utils::withinPositionGoalTolerance(threshold_to_consider_, data)) { return; } diff --git a/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp b/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp index a04f4a9fa95..4acd9784594 100644 --- a/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp @@ -46,8 +46,7 @@ void PathAlignLegacyCritic::initialize() void PathAlignLegacyCritic::score(CriticData & data) { // Don't apply close to goal, let the goal critics take over - if (!enabled_ || - utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + if (!enabled_ || utils::withinPositionGoalTolerance(threshold_to_consider_, data)) { return; } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index f09554bf189..55ac01e9ec1 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -62,7 +62,7 @@ void PathAngleCritic::score(CriticData & data) return; } - if (utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) { + if (utils::withinPositionGoalTolerance(threshold_to_consider_, data)) { return; } diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index 01ecb91728e..5131e00d82c 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -35,7 +35,7 @@ void PathFollowCritic::initialize() void PathFollowCritic::score(CriticData & data) { if (!enabled_ || data.path.x.shape(0) < 2 || - utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + utils::withinPositionGoalTolerance(threshold_to_consider_, data)) { return; } diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 08e755dd6af..998bfc7903a 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -33,8 +33,7 @@ void PreferForwardCritic::initialize() void PreferForwardCritic::score(CriticData & data) { using xt::evaluation_strategy::immediate; - if (!enabled_ || - utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + if (!enabled_ || utils::withinPositionGoalTolerance(threshold_to_consider_, data)) { return; } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index c2e66c2fa06..baad2abcce6 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -134,9 +134,11 @@ void Optimizer::reset() geometry_msgs::msg::TwistStamped Optimizer::evalControl( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, - const nav_msgs::msg::Path & plan, nav2_core::GoalChecker * goal_checker) + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal, + nav2_core::GoalChecker * goal_checker) { - prepare(robot_pose, robot_speed, plan, goal_checker); + prepare(robot_pose, robot_speed, plan, goal, goal_checker); do { optimize(); @@ -177,7 +179,7 @@ xt::xtensor Optimizer::getOptimizationResults() // create a dummy_data object to pass to evalTrajectory CriticData dummy_data = { - state_, dummy_trajectories, path_, costs, settings_.model_dt, + state_, dummy_trajectories, path_, goal_, costs, settings_.model_dt, false, critics_data_.goal_checker, critics_data_.motion_model, std::nullopt, std::nullopt}; dummy_data.furthest_reached_path_point.reset(); dummy_data.path_pts_valid.reset(); @@ -213,11 +215,15 @@ bool Optimizer::fallback(bool fail) void Optimizer::prepare( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, - const nav_msgs::msg::Path & plan, nav2_core::GoalChecker * goal_checker) + 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; path_ = utils::toTensor(plan); + goal_ = goal; + costs_.fill(0); critics_data_.fail_flag = false; diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 893f9286c8d..5c5718e8e24 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -186,6 +186,20 @@ void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) plan.poses.erase(plan.poses.begin(), end); } +geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal() +{ + auto goal = global_plan_.poses.back(); + goal.header.stamp = rclcpp::Time(0); + if (goal.header.frame_id.empty()) { + throw std::runtime_error("Goal pose has an empty frame_id"); + } + geometry_msgs::msg::PoseStamped transformed_goal; + if (!transformPose(costmap_->getGlobalFrameID(), goal, transformed_goal)) { + throw std::runtime_error("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 diff --git a/nav2_mppi_controller/test/critic_manager_test.cpp b/nav2_mppi_controller/test/critic_manager_test.cpp index af4e1860499..c4fc86eeba9 100644 --- a/nav2_mppi_controller/test/critic_manager_test.cpp +++ b/nav2_mppi_controller/test/critic_manager_test.cpp @@ -103,10 +103,11 @@ TEST(CriticManagerTests, BasicCriticOperations) models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; + geometry_msgs::msg::Pose goal; xt::xtensor costs; float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.fail_flag = true; diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index ad227f63982..dbdfe1a6783 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -56,11 +56,12 @@ TEST(CriticTests, ConstraintsCritic) models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; + geometry_msgs::msg::Pose goal; xt::xtensor costs = xt::zeros({1000}); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, - std::nullopt}; + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); // Initialization testing @@ -129,11 +130,12 @@ TEST(CriticTests, GoalAngleCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + geometry_msgs::msg::Pose goal; xt::xtensor costs = xt::zeros({1000}); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, - std::nullopt}; + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); // Initialization testing @@ -181,11 +183,12 @@ TEST(CriticTests, GoalCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + geometry_msgs::msg::Pose goal; xt::xtensor costs = xt::zeros({1000}); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, - std::nullopt}; + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); // Initialization testing @@ -231,11 +234,12 @@ TEST(CriticTests, PathAngleCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + geometry_msgs::msg::Pose goal; xt::xtensor costs = xt::zeros({1000}); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, - std::nullopt}; + {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 @@ -288,11 +292,12 @@ TEST(CriticTests, PreferForwardCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + geometry_msgs::msg::Pose goal; xt::xtensor costs = xt::zeros({1000}); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, - std::nullopt}; + {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 @@ -341,11 +346,12 @@ TEST(CriticTests, TwirlingCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + geometry_msgs::msg::Pose goal; xt::xtensor costs = xt::zeros({1000}); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, - std::nullopt}; + {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; @@ -401,11 +407,12 @@ TEST(CriticTests, PathFollowCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + geometry_msgs::msg::Pose goal; xt::xtensor costs = xt::zeros({1000}); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, - std::nullopt}; + {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; @@ -449,11 +456,12 @@ TEST(CriticTests, PathAlignCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + geometry_msgs::msg::Pose goal; xt::xtensor costs = xt::zeros({1000}); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, - std::nullopt}; + {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; @@ -553,11 +561,12 @@ TEST(CriticTests, PathAlignLegacyCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + geometry_msgs::msg::Pose goal; xt::xtensor costs = xt::zeros({1000}); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, - std::nullopt}; + {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; @@ -658,11 +667,12 @@ TEST(CriticTests, VelocityDeadbandCritic) models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; + geometry_msgs::msg::Pose goal; xt::xtensor costs = xt::zeros({1000}); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, - std::nullopt}; + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); // Initialization testing diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index 9a77f6d8b06..b2210ee2aaf 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -85,9 +85,10 @@ TEST_P(OptimizerSuite, OptimizerTest) { auto pose = getDummyPointStamped(node, start_pose); auto velocity = getDummyTwist(); auto path = getIncrementalDummyPath(node, path_settings); + auto goal = path.poses.back().pose; nav2_core::GoalChecker * dummy_goal_checker{nullptr}; - EXPECT_NO_THROW(optimizer->evalControl(pose, velocity, path, dummy_goal_checker)); + EXPECT_NO_THROW(optimizer->evalControl(pose, velocity, path, goal, dummy_goal_checker)); } INSTANTIATE_TEST_SUITE_P( diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index e16cdafb6aa..2b90dcd6d54 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -123,9 +123,11 @@ class OptimizerTester : public Optimizer void testPrepare( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, - const nav_msgs::msg::Path & plan, nav2_core::GoalChecker * goal_checker) + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal, + nav2_core::GoalChecker * goal_checker) { - prepare(robot_pose, robot_speed, plan, goal_checker); + prepare(robot_pose, robot_speed, plan, goal, goal_checker); EXPECT_EQ(critics_data_.goal_checker, nullptr); EXPECT_NEAR(xt::sum(costs_, immediate)(), 0, 1e-6); // should be reset @@ -367,9 +369,10 @@ TEST(OptimizerTests, PrepareTests) geometry_msgs::msg::Twist speed; speed.linear.y = 4.0; nav_msgs::msg::Path path; + geometry_msgs::msg::Pose goal; path.poses.resize(17); - optimizer_tester.testPrepare(pose, speed, path, nullptr); + optimizer_tester.testPrepare(pose, speed, path, goal, nullptr); } TEST(OptimizerTests, shiftControlSequenceTests) diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index f964181dd93..c14c2fdff56 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -132,14 +132,27 @@ TEST(UtilsTests, WithTolTests) nav2_core::GoalChecker * goal_checker = new TestGoalChecker; - // Test not in tolerance nav_msgs::msg::Path path; path.poses.resize(2); + + // Create CriticData with state and goal initialized + models::State state; + state.pose.pose = pose; + models::Trajectories generated_trajectories; + models::Path path_critic; + xt::xtensor costs; + float model_dt; + CriticData data = { + state, generated_trajectories, path_critic, path.poses.back().pose, + costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; + + // Test not in tolerance path.poses[1].pose.position.x = 0.0; path.poses[1].pose.position.y = 0.0; models::Path path_t = toTensor(path); EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, path_t)); EXPECT_FALSE(withinPositionGoalTolerance(0.25, pose, path_t)); + EXPECT_FALSE(withinPositionGoalTolerance(0.25, data)); // Test in tolerance path.poses[1].pose.position.x = 9.8; @@ -147,18 +160,21 @@ TEST(UtilsTests, WithTolTests) path_t = toTensor(path); EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, path_t)); EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, path_t)); + EXPECT_TRUE(withinPositionGoalTolerance(0.25, data)); path.poses[1].pose.position.x = 10.0; path.poses[1].pose.position.y = 0.76; path_t = toTensor(path); EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, path_t)); EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, path_t)); + EXPECT_TRUE(withinPositionGoalTolerance(0.25, data)); path.poses[1].pose.position.x = 9.76; path.poses[1].pose.position.y = 1.0; path_t = toTensor(path); EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, path_t)); EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, path_t)); + EXPECT_TRUE(withinPositionGoalTolerance(0.25, data)); delete goal_checker; goal_checker = nullptr; @@ -210,11 +226,12 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) models::State state; models::Trajectories generated_trajectories; models::Path path; + geometry_msgs::msg::Pose goal; xt::xtensor costs; float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + {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 @@ -224,7 +241,7 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) // Attempt to set if not set already with no other information, should fail CriticData data2 = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + {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); @@ -243,7 +260,7 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) path = toTensor(plan); CriticData data3 = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references EXPECT_EQ(findPathFurthestReachedPoint(data3), 5u); EXPECT_EQ(findPathTrajectoryInitialPoint(data3), 5u); @@ -254,11 +271,12 @@ TEST(UtilsTests, findPathCosts) models::State state; models::Trajectories generated_trajectories; models::Path path; + geometry_msgs::msg::Pose goal; xt::xtensor costs; float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + {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 @@ -271,7 +289,7 @@ TEST(UtilsTests, findPathCosts) EXPECT_EQ(data.path_pts_valid->size(), 10u); CriticData data3 = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references auto costmap_ros = std::make_shared( From 029d426821c16d9378aed78a548ae4d0a3643b7b Mon Sep 17 00:00:00 2001 From: redvinaa Date: Mon, 30 Dec 2024 13:19:31 +0000 Subject: [PATCH 09/13] Pass goal pose directly to withinPositionGoalTolerance Signed-off-by: redvinaa --- .../nav2_mppi_controller/tools/utils.hpp | 53 +++---------------- .../src/critics/cost_critic.cpp | 2 +- .../src/critics/goal_angle_critic.cpp | 3 +- .../src/critics/goal_critic.cpp | 3 +- .../src/critics/obstacles_critic.cpp | 2 +- .../src/critics/path_align_critic.cpp | 3 +- .../src/critics/path_align_legacy_critic.cpp | 3 +- .../src/critics/path_angle_critic.cpp | 4 +- .../src/critics/path_follow_critic.cpp | 2 +- .../src/critics/prefer_forward_critic.cpp | 3 +- .../src/critics/twirling_critic.cpp | 2 +- 11 files changed, 25 insertions(+), 55 deletions(-) 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 cedb0d64c30..17c418b49b0 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -195,18 +195,14 @@ inline models::Path toTensor(const nav_msgs::msg::Path & path) * @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 path Path to retreive goal pose from + * @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 models::Path & path) + const geometry_msgs::msg::Pose & goal) { - const auto goal_idx = path.x.shape(0) - 1; - const auto goal_x = path.x(goal_idx); - const auto goal_y = path.y(goal_idx); - if (goal_checker) { geometry_msgs::msg::Pose pose_tolerance; geometry_msgs::msg::Twist velocity_tolerance; @@ -214,8 +210,8 @@ inline bool withinPositionGoalTolerance( const auto pose_tolerance_sq = pose_tolerance.position.x * pose_tolerance.position.x; - auto dx = robot.position.x - goal_x; - auto dy = robot.position.y - goal_y; + auto dx = robot.position.x - goal.position.x; + auto dy = robot.position.y - goal.position.y; auto dist_sq = dx * dx + dy * dy; @@ -229,52 +225,19 @@ inline bool withinPositionGoalTolerance( /** * @brief Check if the robot pose is within tolerance to the goal - * - * Deprecated! - * This uses the end of pruned path as the goal, if you want to use - * the global goal, use the other overload - * * @param pose_tolerance Pose tolerance to use * @param robot Pose of robot - * @param path Path to retreive goal pose from + * @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 models::Path & path) -{ - const auto goal_idx = path.x.shape(0) - 1; - const auto goal_x = path.x(goal_idx); - const auto goal_y = path.y(goal_idx); - - const auto pose_tolerance_sq = pose_tolerance * pose_tolerance; - - auto dx = robot.position.x - goal_x; - auto dy = robot.position.y - goal_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 path Path to retreive goal pose from - * @return bool If robot is within tolerance to the goal - */ -inline bool withinPositionGoalTolerance( - float pose_tolerance, const CriticData & data) + const geometry_msgs::msg::Pose & goal) { const double & dist_sq = - std::pow(data.goal.position.x - data.state.pose.pose.position.x, 2) + - std::pow(data.goal.position.y - data.state.pose.pose.position.y, 2); + std::pow(goal.position.x - robot.position.x, 2) + + std::pow(goal.position.y - robot.position.y, 2); const auto pose_tolerance_sq = pose_tolerance * pose_tolerance; diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index d270e8901be..f9d4fec67b6 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -119,7 +119,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)) { + if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.goal)) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 99c2f9c83e1..5af36b8bf44 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -35,7 +35,8 @@ void GoalAngleCritic::initialize() void GoalAngleCritic::score(CriticData & data) { - if (!enabled_ || !utils::withinPositionGoalTolerance(threshold_to_consider_, data)) + if (!enabled_ || !utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, data.goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index d530e012d43..50966079b7a 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -35,7 +35,8 @@ void GoalCritic::initialize() void GoalCritic::score(CriticData & data) { - if (!enabled_ || !utils::withinPositionGoalTolerance(threshold_to_consider_, data)) + if (!enabled_ || utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, data.goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 13f977159ba..12410d68280 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -125,7 +125,7 @@ void ObstaclesCritic::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)) { + if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.goal)) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 172590ba321..7ecca24d702 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -46,7 +46,8 @@ void PathAlignCritic::initialize() void PathAlignCritic::score(CriticData & data) { // Don't apply close to goal, let the goal critics take over - if (!enabled_ || utils::withinPositionGoalTolerance(threshold_to_consider_, data)) + if (!enabled_ || utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, data.goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp b/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp index 4acd9784594..f9ef0db22f8 100644 --- a/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp @@ -46,7 +46,8 @@ void PathAlignLegacyCritic::initialize() void PathAlignLegacyCritic::score(CriticData & data) { // Don't apply close to goal, let the goal critics take over - if (!enabled_ || utils::withinPositionGoalTolerance(threshold_to_consider_, data)) + if (!enabled_ || utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, data.goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index 55ac01e9ec1..61bfb3aacc6 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -62,7 +62,9 @@ void PathAngleCritic::score(CriticData & data) return; } - if (utils::withinPositionGoalTolerance(threshold_to_consider_, data)) { + if (utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, data.goal)) + { return; } diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index 5131e00d82c..b6cb435bee2 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -35,7 +35,7 @@ void PathFollowCritic::initialize() void PathFollowCritic::score(CriticData & data) { if (!enabled_ || data.path.x.shape(0) < 2 || - utils::withinPositionGoalTolerance(threshold_to_consider_, data)) + utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 998bfc7903a..347be9082a5 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -33,7 +33,8 @@ void PreferForwardCritic::initialize() void PreferForwardCritic::score(CriticData & data) { using xt::evaluation_strategy::immediate; - if (!enabled_ || utils::withinPositionGoalTolerance(threshold_to_consider_, data)) + if (!enabled_ || utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, data.goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index 6b492d13ba0..e2a12234f97 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -32,7 +32,7 @@ void TwirlingCritic::score(CriticData & data) { using xt::evaluation_strategy::immediate; if (!enabled_ || - utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.path)) + utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.goal)) { return; } From 0d346274d1783a80533f7b55a56d4e6d7bcb347c Mon Sep 17 00:00:00 2001 From: redvinaa Date: Thu, 2 Jan 2025 12:28:34 +0100 Subject: [PATCH 10/13] Fix condition not Signed-off-by: redvinaa --- nav2_mppi_controller/src/critics/goal_critic.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 50966079b7a..45eb52fa589 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -35,7 +35,7 @@ void GoalCritic::initialize() void GoalCritic::score(CriticData & data) { - if (!enabled_ || utils::withinPositionGoalTolerance( + if (!enabled_ || !utils::withinPositionGoalTolerance( threshold_to_consider_, data.state.pose.pose, data.goal)) { return; From 6c9d8f97e76f4af21dd0ec8a8a024dd4440b9646 Mon Sep 17 00:00:00 2001 From: redvinaa Date: Thu, 2 Jan 2025 12:31:15 +0100 Subject: [PATCH 11/13] Add goal positions to tests Signed-off-by: redvinaa --- nav2_mppi_controller/test/critics_tests.cpp | 25 ++++++++++- nav2_mppi_controller/test/utils_test.cpp | 49 +++++++++------------ 2 files changed, 44 insertions(+), 30 deletions(-) diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index dbdfe1a6783..09b5809f3f2 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -153,6 +153,7 @@ TEST(CriticTests, GoalAngleCritic) path.x(9) = 10.0; path.y(9) = 0.0; path.yaws(9) = 3.14; + goal.position.x = 10.0; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); @@ -205,6 +206,7 @@ TEST(CriticTests, GoalCritic) path.reset(10); path.x(9) = 10.0; path.y(9) = 0.0; + goal.position.x = 10.0; critic.score(data); EXPECT_NEAR(costs(2), 0.0, 1e-6); // (0 * 5.0 weight EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); // Should all be 0 * 1000 @@ -213,6 +215,7 @@ TEST(CriticTests, GoalCritic) // provide state pose and path close path.x(9) = 0.5; path.y(9) = 0.0; + goal.position.x = 0.5; critic.score(data); EXPECT_NEAR(costs(2), 2.5, 1e-6); // (sqrt(10.0 * 10.0) * 5.0 weight EXPECT_NEAR(xt::sum(costs, immediate)(), 2500.0, 1e-6); // should be 2.5 * 1000 @@ -257,11 +260,13 @@ TEST(CriticTests, PathAngleCritic) state.pose.pose.position.y = 0.0; path.reset(10); path.x(9) = 0.15; + goal.position.x = 0.15; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 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; 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; @@ -314,11 +319,13 @@ TEST(CriticTests, PreferForwardCritic) state.pose.pose.position.x = 1.0; path.reset(10); path.x(9) = 10.0; + goal.position.x = 10.0; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 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.vx = xt::ones({1000, 30}); critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0f, 1e-6f); @@ -369,11 +376,13 @@ TEST(CriticTests, TwirlingCritic) state.pose.pose.position.x = 1.0; path.reset(10); path.x(9) = 10.0; + goal.position.x = 10.0; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 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.wz = xt::zeros({1000, 30}); critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); @@ -426,16 +435,18 @@ TEST(CriticTests, PathFollowCritic) // Scoring testing - // provide state poses and path close within positional tolerances + // provide state poses and goal close within positional tolerances state.pose.pose.position.x = 2.0; path.reset(6); - path.x(5) = 1.7; + path.x(5) = 1.8; + goal.position.x = 1.8; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 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; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 750.0, 1e-2); // 0.15 * 5 weight * 1000 } @@ -479,12 +490,14 @@ TEST(CriticTests, PathAlignCritic) state.pose.pose.position.x = 1.0; path.reset(10); path.x(9) = 0.85; + goal.position.x = 0.85; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 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; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); @@ -492,6 +505,7 @@ TEST(CriticTests, PathAlignCritic) // 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; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); @@ -522,6 +536,7 @@ TEST(CriticTests, PathAlignCritic) path.x(19) = 0.9; path.x(20) = 0.9; path.x(21) = 0.9; + goal.position.x = 0.9; generated_trajectories.x = 0.66 * xt::ones({1000, 30}); critic.score(data); // 0.66 * 1000 * 10 weight * 6 num pts eval / 6 normalization term @@ -541,6 +556,7 @@ TEST(CriticTests, PathAlignCritic) costs = xt::zeros({1000}); path.x = 1.5 * xt::ones({22}); path.y = 1.5 * xt::ones({22}); + goal.position.x = 1.5; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); } @@ -584,12 +600,14 @@ TEST(CriticTests, PathAlignLegacyCritic) state.pose.pose.position.x = 1.0; path.reset(10); path.x(9) = 0.85; + goal.position.x = 0.85; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 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; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); @@ -597,6 +615,7 @@ TEST(CriticTests, PathAlignLegacyCritic) // 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; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); @@ -627,6 +646,7 @@ TEST(CriticTests, PathAlignLegacyCritic) path.x(19) = 0.9; path.x(20) = 0.9; path.x(21) = 0.9; + goal.position.x = 0.9; generated_trajectories.x = 0.66 * xt::ones({1000, 30}); critic.score(data); // 0.04 * 1000 * 10 weight * 6 num pts eval / 6 normalization term @@ -646,6 +666,7 @@ TEST(CriticTests, PathAlignLegacyCritic) costs = xt::zeros({1000}); path.x = 1.5 * xt::ones({22}); path.y = 1.5 * xt::ones({22}); + goal.position.x = 1.5; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); } diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index c14c2fdff56..9e13ba2f0bf 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -134,6 +134,7 @@ TEST(UtilsTests, WithTolTests) 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; @@ -143,42 +144,34 @@ TEST(UtilsTests, WithTolTests) xt::xtensor costs; float model_dt; CriticData data = { - state, generated_trajectories, path_critic, path.poses.back().pose, + state, generated_trajectories, path_critic, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; // Test not in tolerance - path.poses[1].pose.position.x = 0.0; - path.poses[1].pose.position.y = 0.0; - models::Path path_t = toTensor(path); - EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, path_t)); - EXPECT_FALSE(withinPositionGoalTolerance(0.25, pose, path_t)); - EXPECT_FALSE(withinPositionGoalTolerance(0.25, data)); + 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 - path.poses[1].pose.position.x = 9.8; - path.poses[1].pose.position.y = 0.95; - path_t = toTensor(path); - EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, path_t)); - EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, path_t)); - EXPECT_TRUE(withinPositionGoalTolerance(0.25, data)); - - path.poses[1].pose.position.x = 10.0; - path.poses[1].pose.position.y = 0.76; - path_t = toTensor(path); - EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, path_t)); - EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, path_t)); - EXPECT_TRUE(withinPositionGoalTolerance(0.25, data)); - - path.poses[1].pose.position.x = 9.76; - path.poses[1].pose.position.y = 1.0; - path_t = toTensor(path); - EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, path_t)); - EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, path_t)); - EXPECT_TRUE(withinPositionGoalTolerance(0.25, data)); + 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, path_t)); + EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, goal)); } TEST(UtilsTests, AnglesTests) From 467fd247345811d2dde2c4318f627e4a6cc7c69e Mon Sep 17 00:00:00 2001 From: redvinaa Date: Fri, 3 Jan 2025 11:35:57 +0100 Subject: [PATCH 12/13] Use plan stamp Signed-off-by: redvinaa --- nav2_mppi_controller/src/path_handler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 5c5718e8e24..8c90f453891 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -189,7 +189,7 @@ void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal() { auto goal = global_plan_.poses.back(); - goal.header.stamp = rclcpp::Time(0); + goal.header.stamp = global_plan_.header.stamp; if (goal.header.frame_id.empty()) { throw std::runtime_error("Goal pose has an empty frame_id"); } From d86c975215a8226361e924f6ee88b191163eae3a Mon Sep 17 00:00:00 2001 From: redvinaa Date: Fri, 3 Jan 2025 11:56:53 +0100 Subject: [PATCH 13/13] Use float instead of auto Signed-off-by: redvinaa --- .../include/nav2_mppi_controller/tools/utils.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 17c418b49b0..60c85c23805 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -239,7 +239,7 @@ inline bool withinPositionGoalTolerance( std::pow(goal.position.x - robot.position.x, 2) + std::pow(goal.position.y - robot.position.y, 2); - const auto pose_tolerance_sq = pose_tolerance * pose_tolerance; + const float pose_tolerance_sq = pose_tolerance * pose_tolerance; if (dist_sq < pose_tolerance_sq) { return true;