diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp index 75a29bb91b..40ea9c042b 100644 --- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -116,7 +116,8 @@ static void BM_Omni(benchmark::State & state) { bool consider_footprint = true; std::string motion_model = "Omni"; - std::vector critics = {}; + std::vector critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); } diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index 893342e733..82b850bc46 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -20,7 +20,7 @@ #include "nav2_mppi_controller/tools/path_handler.hpp" #include "nav2_mppi_controller/optimizer.hpp" -//#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp" +#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp" #include "nav2_mppi_controller/models/constraints.hpp" #include "nav2_mppi_controller/tools/utils.hpp" @@ -117,7 +117,7 @@ class MPPIController : public nav2_core::Controller std::unique_ptr parameters_handler_; Optimizer optimizer_; PathHandler path_handler_; - //TrajectoryVisualizer trajectory_visualizer_; + TrajectoryVisualizer trajectory_visualizer_; bool visualize_; }; diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index dacdca432b..6c83131263 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -41,9 +41,9 @@ void MPPIController::configure( // Configure composed objects optimizer_.initialize(parent_, name_, costmap_ros_, parameters_handler_.get()); path_handler_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get()); - //trajectory_visualizer_.on_configure( - //parent_, name_, - //costmap_ros_->getGlobalFrameID(), parameters_handler_.get()); + trajectory_visualizer_.on_configure( + parent_, name_, + costmap_ros_->getGlobalFrameID(), parameters_handler_.get()); RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str()); } @@ -51,21 +51,21 @@ void MPPIController::configure( void MPPIController::cleanup() { optimizer_.shutdown(); - //trajectory_visualizer_.on_cleanup(); + trajectory_visualizer_.on_cleanup(); parameters_handler_.reset(); RCLCPP_INFO(logger_, "Cleaned up MPPI Controller: %s", name_.c_str()); } void MPPIController::activate() { - //trajectory_visualizer_.on_activate(); + trajectory_visualizer_.on_activate(); parameters_handler_->start(); RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str()); } void MPPIController::deactivate() { - //trajectory_visualizer_.on_deactivate(); + trajectory_visualizer_.on_deactivate(); RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str()); } @@ -105,11 +105,11 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( return cmd; } -void MPPIController::visualize(nav_msgs::msg::Path /*transformed_plan*/) +void MPPIController::visualize(nav_msgs::msg::Path transformed_plan) { - //trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories"); - //trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory"); - //trajectory_visualizer_.visualize(std::move(transformed_plan)); + trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories"); + trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory"); + trajectory_visualizer_.visualize(std::move(transformed_plan)); } void MPPIController::setPlan(const nav_msgs::msg::Path & path) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index b0fd04e9ae..0fd0a5dff9 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -169,7 +169,6 @@ geometry_msgs::msg::TwistStamped Optimizer::evalControl( void Optimizer::optimize() { for (size_t i = 0; i < settings_.iteration_count; ++i) { - generateNoisedTrajectories(); critic_manager_.evalTrajectoriesScores(critics_data_); updateControlSequence(); @@ -233,32 +232,28 @@ void Optimizer::generateNoisedTrajectories() void Optimizer::applyControlSequenceConstraints() { auto & s = settings_; - float & vx_min = s.constraints.vx_min; - float & vx_max = s.constraints.vx_max; - float & vy = s.constraints.vy; - float & wz = s.constraints.wz; - - float && max_delta_vx = s.model_dt * s.constraints.ax_max; - float && min_delta_vx = s.model_dt * s.constraints.ax_min; - float && max_delta_vy = s.model_dt * s.constraints.ay_max; - float && max_delta_wz = s.model_dt * s.constraints.az_max; - float vx_last = std::min(vx_max, std::max(control_sequence_.vx(0), vx_min)); - float vy_last = std::min(vy, std::max(control_sequence_.vy(0), -vy)); - float wz_last = std::min(wz, std::max(control_sequence_.wz(0), -wz)); + + float max_delta_vx = s.model_dt * s.constraints.ax_max; + float min_delta_vx = s.model_dt * s.constraints.ax_min; + float max_delta_vy = s.model_dt * s.constraints.ay_max; + float max_delta_wz = s.model_dt * s.constraints.az_max; + float vx_last = std::min(s.constraints.vx_max, std::max(control_sequence_.vx(0), s.constraints.vx_min)); + float vy_last = std::min(s.constraints.vy, std::max(control_sequence_.vy(0), -s.constraints.vy)); + float wz_last = std::min(s.constraints.wz, std::max(control_sequence_.wz(0), -s.constraints.wz)); for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) { float & vx_curr = control_sequence_.vx(i); - vx_curr = std::min(vx_max, std::max(vx_curr, vx_min)); + vx_curr = std::min(s.constraints.vx_max, std::max(vx_curr, s.constraints.vx_min)); vx_curr = std::min(vx_last + max_delta_vx, std::max(vx_curr, vx_last + min_delta_vx)); vx_last = vx_curr; float & wz_curr = control_sequence_.wz(i); - wz_curr = std::min(wz, std::max(wz_curr, -wz)); + wz_curr = std::min(s.constraints.wz, std::max(wz_curr, -s.constraints.wz)); wz_curr = std::min(wz_last + max_delta_wz, std::max(wz_curr, wz_last - max_delta_wz)); wz_last = wz_curr; if (isHolonomic()) { float & vy_curr = control_sequence_.vy(i); - vy_curr = std::min(vy, std::max(vy_curr, -vy)); + vy_curr = std::min(s.constraints.vy, std::max(vy_curr, -s.constraints.vy)); vy_curr = std::min(vy_last + max_delta_vy, std::max(vy_curr, vy_last - max_delta_vy)); vy_last = vy_curr; } @@ -292,11 +287,10 @@ void Optimizer::propagateStateVelocitiesFromInitials( } void Optimizer::integrateStateVelocities( - Eigen::ArrayXXf & trajectory, + Eigen::Array & trajectory, const Eigen::ArrayXXf & sequence) const { float initial_yaw = static_cast(tf2::getYaw(state_.pose.pose.orientation)); - const float & dt_time = settings_.model_dt; const auto vx = sequence.col(0); const auto wz = sequence.col(1); @@ -305,19 +299,19 @@ void Optimizer::integrateStateVelocities( auto traj_y = trajectory.col(1); auto traj_yaws = trajectory.col(2); - unsigned int n_size = traj_yaws.size(); + size_t n_size = traj_yaws.size(); - traj_yaws(0) = wz(0) * dt_time + initial_yaw; + traj_yaws(0) = wz(0) * settings_.model_dt + initial_yaw; float last_yaw = traj_yaws(0); - for(unsigned int i = 1; i != n_size; i++) + for(size_t i = 1; i != n_size; i++) { float & curr_yaw = traj_yaws(i); - curr_yaw = last_yaw + wz(i) * dt_time; + curr_yaw = last_yaw + wz(i) * settings_.model_dt; last_yaw = curr_yaw; } - auto yaw_cos = (utils::rollColumns(traj_yaws, -1).cos()).eval(); - auto yaw_sin = (utils::rollColumns(traj_yaws, -1).sin()).eval(); + Eigen::ArrayXf yaw_cos = utils::rollColumns(traj_yaws, -1).cos(); + Eigen::ArrayXf yaw_sin = utils::rollColumns(traj_yaws, -1).sin(); yaw_cos(0) = cosf(initial_yaw); yaw_sin(0) = sinf(initial_yaw); @@ -330,16 +324,16 @@ void Optimizer::integrateStateVelocities( dy = (dy + vy * yaw_cos).eval(); } - traj_x(0) = state_.pose.pose.position.x + dx(0) * dt_time; - traj_y(0) = state_.pose.pose.position.y + dy(0) * dt_time; + traj_x(0) = state_.pose.pose.position.x + dx(0) * settings_.model_dt; + traj_y(0) = state_.pose.pose.position.y + dy(0) * settings_.model_dt; float last_x = traj_x(0); float last_y = traj_y(0); for(unsigned int i = 1; i != n_size; i++) { float & curr_x = traj_x(i); float & curr_y = traj_y(i); - curr_x = last_x + dx(i) * dt_time; - curr_y = last_y + dy(i) * dt_time; + curr_x = last_x + dx(i) * settings_.model_dt; + curr_y = last_y + dy(i) * settings_.model_dt; last_x = curr_x; last_y = curr_y; } @@ -351,12 +345,11 @@ void Optimizer::integrateStateVelocities( { const float initial_yaw = static_cast(tf2::getYaw(state.pose.pose.orientation)); const unsigned int n_cols = trajectories.yaws.cols(); - const float & dt_time = settings_.model_dt; - trajectories.yaws.col(0) = state.wz.col(0) * dt_time + initial_yaw; + trajectories.yaws.col(0) = state.wz.col(0) * settings_.model_dt + initial_yaw; for(unsigned int i = 1; i != n_cols; i++) { - trajectories.yaws.col(i) = trajectories.yaws.col(i - 1) + state.wz.col(i) * dt_time; + trajectories.yaws.col(i) = trajectories.yaws.col(i - 1) + state.wz.col(i) * settings_.model_dt; } auto yaw_cos = (utils::rollColumns(trajectories.yaws, -1).cos()).eval(); @@ -373,20 +366,20 @@ void Optimizer::integrateStateVelocities( dy = dy + state.vy * yaw_cos; } - trajectories.x.col(0) = dx.col(0) * dt_time + state.pose.pose.position.x; - trajectories.y.col(0) = dy.col(0) * dt_time + state.pose.pose.position.y; + trajectories.x.col(0) = dx.col(0) * settings_.model_dt + state.pose.pose.position.x; + trajectories.y.col(0) = dy.col(0) * settings_.model_dt + state.pose.pose.position.y; for(unsigned int i = 1; i != n_cols; i++) { - trajectories.x.col(i) = trajectories.x.col(i - 1) + dx.col(i) * dt_time; - trajectories.y.col(i) = trajectories.y.col(i - 1) + dy.col(i) * dt_time; + trajectories.x.col(i) = trajectories.x.col(i - 1) + dx.col(i) * settings_.model_dt; + trajectories.y.col(i) = trajectories.y.col(i - 1) + dy.col(i) * settings_.model_dt; } } Eigen::ArrayXXf Optimizer::getOptimizedTrajectory() { const bool is_holo = isHolonomic(); - auto && sequence = Eigen::ArrayXXf(settings_.time_steps, is_holo ? 3 : 2); - auto && trajectories = Eigen::ArrayXXf(settings_.time_steps, 3); + Eigen::ArrayXXf sequence = Eigen::ArrayXXf(settings_.time_steps, is_holo ? 3 : 2); + Eigen::Array trajectories = Eigen::Array(settings_.time_steps, 3); sequence.col(0) = control_sequence_.vx; sequence.col(1) = control_sequence_.wz; @@ -396,7 +389,7 @@ Eigen::ArrayXXf Optimizer::getOptimizedTrajectory() } integrateStateVelocities(trajectories, sequence); - return std::move(trajectories); + return trajectories; } void Optimizer::updateControlSequence()