From a97c9efa8db50f7f9ad16d1611a271227c38f196 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?L=C3=A1szl=C3=B3=20M=C3=A1t=C3=A9?= <56221639+turtlewizard73@users.noreply.github.com> Date: Mon, 16 Dec 2024 13:16:03 +0100 Subject: [PATCH] new scorecritic single msg (#5) * new scorecritic single msg * Vince fix * remove duplicate function * linting --- nav2_mppi_controller/CMakeLists.txt | 1 + .../nav2_mppi_controller/controller.hpp | 1 + .../nav2_mppi_controller/critic_manager.hpp | 2 - nav2_mppi_controller/msg/CriticScore.msg | 2 + nav2_mppi_controller/msg/CriticScores.msg | 5 +- nav2_mppi_controller/src/controller.cpp | 26 +++++---- nav2_mppi_controller/src/critic_manager.cpp | 24 -------- nav2_mppi_controller/src/optimizer.cpp | 55 ++++++------------- 8 files changed, 41 insertions(+), 75 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 692560018b5..264914a73d8 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -45,6 +45,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/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/msg/CriticScore.msg b/nav2_mppi_controller/msg/CriticScore.msg new file mode 100644 index 00000000000..fdc2901a901 --- /dev/null +++ b/nav2_mppi_controller/msg/CriticScore.msg @@ -0,0 +1,2 @@ +std_msgs/String name +std_msgs/Float32 score diff --git a/nav2_mppi_controller/msg/CriticScores.msg b/nav2_mppi_controller/msg/CriticScores.msg index 4797457b2f6..b6fae696d61 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 diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 104800c0fd8..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()); @@ -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()); } @@ -124,20 +124,26 @@ 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 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 3952a97b080..0e21a7d8775 100644 --- a/nav2_mppi_controller/src/critic_manager.cpp +++ b/nav2_mppi_controller/src/critic_manager.cpp @@ -81,28 +81,4 @@ void CriticManager::evalTrajectoriesScores( critics_[q]->score(data); } } - -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 12eba65d10d..63503e9a4af 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -163,58 +163,39 @@ 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());*/ + // 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; - return critic_manager_.evalTrajectory(dummy_data); + // evaluate the optimized trajectory + // return critic_manager_.evalTrajectory(dummy_data); } std::vector Optimizer::getCriticNames() const @@ -496,7 +477,7 @@ void Optimizer::setSpeedLimit(double speed_limit, bool percentage) s.constraints.vx_min = s.base_constraints.vx_min * ratio; s.constraints.vy = s.base_constraints.vy * ratio; s.constraints.wz = s.base_constraints.wz * ratio; - } else { + } else if (speed_limit < s.base_constraints.vx_max) { // Speed limit is expressed in absolute value double ratio = speed_limit / s.base_constraints.vx_max; s.constraints.vx_max = s.base_constraints.vx_max * ratio;