From 154feba58250840e75000616efc113686effffc5 Mon Sep 17 00:00:00 2001 From: turtlewizard73 Date: Mon, 27 May 2024 12:25:52 +0200 Subject: [PATCH 1/7] 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 2/7] 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 3/7] 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 4/7] 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 5/7] 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 d3947385c390d0d1d09299227e6f9b3481aa059f Mon Sep 17 00:00:00 2001 From: turtlewizard73 Date: Mon, 16 Dec 2024 12:41:20 +0100 Subject: [PATCH 6/7] Vince fix --- nav2_mppi_controller/src/controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index a1a998ba099..4128d9839a9 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -75,7 +75,7 @@ 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 1dbf4631bb7480ac874ea3980f5c8964be82a4c7 Mon Sep 17 00:00:00 2001 From: turtlewizard73 Date: Mon, 16 Dec 2024 12:41:53 +0100 Subject: [PATCH 7/7] remove duplicate function --- .../nav2_mppi_controller/critic_manager.hpp | 2 -- nav2_mppi_controller/src/controller.cpp | 2 +- nav2_mppi_controller/src/critic_manager.cpp | 21 ------------------- nav2_mppi_controller/src/optimizer.cpp | 14 ++++++++++++- 4 files changed, 14 insertions(+), 25 deletions(-) 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 c6903846e80..be294b11701 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp @@ -69,8 +69,6 @@ class CriticManager */ void evalTrajectoriesScores(CriticData & data) const; - xt::xtensor evalTrajectory(CriticData & data) const; - std::vector getCriticNames() const; protected: diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 4128d9839a9..dd005a29f42 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -67,7 +67,7 @@ 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()); diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp index a11bd902034..0e21a7d8775 100644 --- a/nav2_mppi_controller/src/critic_manager.cpp +++ b/nav2_mppi_controller/src/critic_manager.cpp @@ -81,25 +81,4 @@ void CriticManager::evalTrajectoriesScores( critics_[q]->score(data); } } - -xt::xtensor CriticManager::evalTrajectory( - CriticData & data) const -{ - 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_DEBUG(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 c2e66c2fa06..c0a1f3d41dc 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -182,8 +182,20 @@ xt::xtensor Optimizer::getOptimizationResults() dummy_data.furthest_reached_path_point.reset(); dummy_data.path_pts_valid.reset(); + // use evalTrajectoriesScores + critic_manager_.evalTrajectoriesScores(dummy_data); + + size_t num_critics = critic_manager_.getCriticNames().size(); + + xt::xtensor critic_scores = xt::zeros(std::vector{num_critics}); + for (size_t i = 0; i < num_critics; i++) { + critic_scores(i) = dummy_data.costs(0); // Assuming costs are updated for each critic + } + + return critic_scores; + // evaluate the optimized trajectory - return critic_manager_.evalTrajectory(dummy_data); + // return critic_manager_.evalTrajectory(dummy_data); } std::vector Optimizer::getCriticNames() const