16 #include "nav2_mppi_controller/optimizer.hpp"
26 #include "nav2_core/controller_exceptions.hpp"
27 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
28 #include "nav2_ros_common/node_utils.hpp"
29 #include "nav2_ros_common/tf2_factories.hpp"
35 nav2::LifecycleNode::WeakPtr parent,
const std::string & name,
36 std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
37 nav2::TransformBuffer::SharedPtr tf_buffer,
42 costmap_ros_ = costmap_ros;
43 costmap_ = costmap_ros_->getCostmap();
44 parameters_handler_ = param_handler;
45 tf_buffer_ = tf_buffer;
47 auto node = parent_.lock();
52 critic_manager_.
on_configure(parent_, name_, costmap_ros_, parameters_handler_);
56 nav2::declare_parameter_if_not_declared(
57 node, name_ +
".TrajectoryValidator.plugin",
58 rclcpp::ParameterValue(
"mppi::DefaultOptimalTrajectoryValidator"));
59 std::string validator_plugin_type = nav2::get_plugin_type_param(
60 node, name_ +
".TrajectoryValidator");
61 validator_loader_ = std::make_unique<pluginlib::ClassLoader<OptimalTrajectoryValidator>>(
62 "nav2_mppi_controller",
"mppi::OptimalTrajectoryValidator");
63 trajectory_validator_ = validator_loader_->createUniqueInstance(validator_plugin_type);
64 trajectory_validator_->initialize(
65 parent_, name_ +
".TrajectoryValidator",
66 costmap_ros_, parameters_handler_, tf_buffer, settings_);
67 RCLCPP_INFO(
logger_,
"Loaded trajectory validator plugin: %s", validator_plugin_type.c_str());
79 std::string motion_model_name;
86 auto kinematic_guard = [
this](
87 const rclcpp::Parameter & param,
88 rcl_interfaces::msg::SetParametersResult & result) {
90 result.successful =
false;
91 if (!result.reason.empty()) {
92 result.reason +=
"\n";
94 result.reason +=
"Rejected dynamic update to '" + param.get_name() +
95 "': speed limit is active. Clear the speed limit first.";
99 const std::vector<std::string> kinematic_params = {
100 "vx_max",
"vx_min",
"vy_max",
"wz_max"};
101 for (
const auto & p : kinematic_params) {
102 parameters_handler_->
addPreCallback(name_ +
"." + p, kinematic_guard);
105 auto batch_size_guard = [](
106 const rclcpp::Parameter & param,
107 rcl_interfaces::msg::SetParametersResult & result)
109 const auto value = param.as_int();
110 if (value <= 0 || value > 100000) {
111 result.successful =
false;
112 if (!result.reason.empty()) {
113 result.reason +=
"\n";
115 result.reason +=
"MPPI batch_size must be in [1, 100000]";
116 if (value > 100000) {
117 result.reason +=
". If you need a larger value, contact the maintainers to discuss "
118 "extending this limit";
122 parameters_handler_->
addPreCallback(name_ +
".batch_size", batch_size_guard);
124 auto time_steps_guard = [](
125 const rclcpp::Parameter & param,
126 rcl_interfaces::msg::SetParametersResult & result)
128 const auto value = param.as_int();
129 if (value <= 0 || value > 1000) {
130 result.successful =
false;
131 if (!result.reason.empty()) {
132 result.reason +=
"\n";
134 result.reason +=
"MPPI time_steps must be in [1, 1000]";
136 result.reason +=
". If you need a larger value, contact the maintainers to discuss "
137 "extending this limit";
141 parameters_handler_->
addPreCallback(name_ +
".time_steps", time_steps_guard);
143 getParam(s.model_dt,
"model_dt", 0.05f);
144 getParam(s.model_delay_vx,
"model_delay_vx", 0.0f);
145 getParam(s.model_delay_vy,
"model_delay_vy", 0.0f);
146 getParam(s.model_delay_wz,
"model_delay_wz", 0.0f);
147 getParam(s.clamp_raw_controls,
"clamp_raw_controls",
false);
148 getParam(s.time_steps,
"time_steps", 56);
149 getParam(s.batch_size,
"batch_size", 1000);
150 getParam(s.iteration_count,
"iteration_count", 1);
151 getParam(s.temperature,
"temperature", 0.3f);
152 getParam(s.gamma,
"gamma", 0.015f);
153 getParam(s.base_constraints.vx_max,
"vx_max", 0.5f);
154 getParam(s.base_constraints.vx_min,
"vx_min", -0.35f);
155 getParam(s.base_constraints.vy,
"vy_max", 0.5f);
156 getParam(s.base_constraints.wz,
"wz_max", 1.9f);
157 getParam(s.base_constraints.ax_max,
"ax_max", 3.0f);
158 getParam(s.base_constraints.ax_min,
"ax_min", -3.0f);
159 getParam(s.base_constraints.ay_max,
"ay_max", 3.0f);
160 getParam(s.base_constraints.ay_min,
"ay_min", -3.0f);
161 getParam(s.base_constraints.az_max,
"az_max", 3.5f);
162 getParam(s.sampling_std.vx,
"vx_std", 0.2f);
163 getParam(s.sampling_std.vy,
"vy_std", 0.2f);
164 getParam(s.sampling_std.wz,
"wz_std", 0.4f);
165 getParam(s.retry_attempt_limit,
"retry_attempt_limit", 1);
166 getParam(s.open_loop,
"open_loop",
false);
167 getParam(s.sgf_order,
"sgf_order", 2);
168 if (s.sgf_order < 1 || s.sgf_order > 2) {
169 RCLCPP_WARN(
logger_,
"sgf_order must be 1 or 2, defaulting to 2");
173 s.base_constraints.ax_max = fabs(s.base_constraints.ax_max);
174 if (s.base_constraints.ax_min > 0.0) {
175 s.base_constraints.ax_min = -1.0 * s.base_constraints.ax_min;
178 "Sign of the parameter ax_min is incorrect, consider setting it negative.");
181 if (s.base_constraints.ay_min > 0.0) {
182 s.base_constraints.ay_min = -1.0 * s.base_constraints.ay_min;
185 "Sign of the parameter ay_min is incorrect, consider setting it negative.");
188 getParam(motion_model_name,
"motion_model", std::string(
"diff_drive"));
190 if (s.batch_size == 0u || s.batch_size > 100000u) {
192 logger_,
"batch_size must be in [1, 100000], defaulting to 1000. "
193 "If you need a larger value, contact the maintainers to discuss extending this limit");
194 s.batch_size = 1000u;
196 if (s.time_steps == 0u || s.time_steps > 1000u) {
198 logger_,
"time_steps must be in [1, 1000], defaulting to 56. "
199 "If you need a larger value, contact the maintainers to discuss extending this limit");
203 s.constraints = s.base_constraints;
208 double controller_frequency;
209 getParentParam(controller_frequency,
"controller_frequency", 0.0, ParameterType::Static);
210 s.controller_period =
static_cast<float>(1.0 / controller_frequency);
216 constexpr
double eps = 1e-6;
218 if ((controller_period + eps) < settings_.model_dt) {
221 "Controller period is less then model dt, consider setting it equal");
222 }
else if (abs(controller_period - settings_.model_dt) < eps) {
225 "Controller period is equal to model dt. Control sequence "
227 settings_.shift_control_sequence =
true;
230 "Controller period more then model dt, set it equal to model dt");
236 state_.
reset(settings_.batch_size, settings_.time_steps);
237 control_sequence_.reset(settings_.time_steps);
238 control_history_[0] = {0.0f, 0.0f, 0.0f};
239 control_history_[1] = {0.0f, 0.0f, 0.0f};
240 control_history_[2] = {0.0f, 0.0f, 0.0f};
241 control_history_[3] = {0.0f, 0.0f, 0.0f};
243 last_command_vel_ = geometry_msgs::msg::Twist();
245 if (reset_dynamic_speed_limits) {
246 settings_.constraints = settings_.base_constraints;
249 costs_.setZero(settings_.batch_size);
250 generated_trajectories_.
reset(settings_.batch_size, settings_.time_steps);
253 motion_model_->setConstraints(settings_.constraints, settings_.model_dt,
254 settings_.model_delay_vx, settings_.model_delay_vy, settings_.model_delay_wz,
255 settings_.clamp_raw_controls);
256 motion_model_->clearCommandHistory();
257 trajectory_validator_->initialize(
258 parent_, name_ +
".TrajectoryValidator",
259 costmap_ros_, parameters_handler_, tf_buffer_, settings_);
261 RCLCPP_INFO(
logger_,
"Optimizer reset");
266 return motion_model_->isHolonomic();
273 const auto & base = settings_.base_constraints;
274 const auto & curr = settings_.constraints;
275 return base.vx_max != curr.vx_max ||
276 base.vx_min != curr.vx_min ||
277 base.vy != curr.vy ||
282 const geometry_msgs::msg::PoseStamped & robot_pose,
283 const geometry_msgs::msg::Twist & robot_speed,
284 const nav_msgs::msg::Path & plan,
285 const geometry_msgs::msg::Pose & goal,
288 prepare(robot_pose, robot_speed, plan, goal, goal_checker);
289 Eigen::ArrayXXf optimal_trajectory;
290 bool trajectory_valid =
true;
295 switch (trajectory_validator_->validateTrajectory(
296 optimal_trajectory, control_sequence_, robot_pose, robot_speed, plan, goal))
298 case mppi::ValidationResult::SOFT_RESET:
299 trajectory_valid =
false;
300 RCLCPP_WARN(
logger_,
"Soft reset triggered by trajectory validator");
302 case mppi::ValidationResult::FAILURE:
304 "Trajectory validator failed to validate trajectory, hard reset triggered.");
305 case mppi::ValidationResult::SUCCESS:
307 trajectory_valid =
true;
310 }
while (
fallback(critics_data_.fail_flag || !trajectory_valid));
314 last_command_vel_ = control.twist;
316 if (settings_.shift_control_sequence) {
320 return std::make_tuple(control, optimal_trajectory);
325 for (
size_t i = 0; i < settings_.iteration_count; ++i) {
343 if (++fallback_count_ > settings_.retry_attempt_limit) {
352 const geometry_msgs::msg::PoseStamped & robot_pose,
353 const geometry_msgs::msg::Twist & robot_speed,
354 const nav_msgs::msg::Path & plan,
355 const geometry_msgs::msg::Pose & goal,
358 state_.pose = robot_pose;
359 if (settings_.open_loop) {
360 state_.speed = last_command_vel_;
365 const auto & c = settings_.constraints;
366 const double dt = settings_.controller_period;
367 float max_delta_vx = dt * c.ax_max;
368 float min_delta_vx = dt * c.ax_min;
369 float max_delta_wz = dt * c.az_max;
370 state_.speed = robot_speed;
371 state_.speed.linear.x = utils::clampVelocityByAccel(
372 robot_speed.linear.x, last_command_vel_.linear.x, min_delta_vx, max_delta_vx);
373 state_.speed.angular.z = utils::clampVelocityByAccel(
374 robot_speed.angular.z, last_command_vel_.angular.z, -max_delta_wz, max_delta_wz);
376 float max_delta_vy = dt * c.ay_max;
377 float min_delta_vy = dt * c.ay_min;
378 state_.speed.linear.y = utils::clampVelocityByAccel(
379 robot_speed.linear.y, last_command_vel_.linear.y, min_delta_vy, max_delta_vy);
382 motion_model_->predictPose(state_.pose.pose, robot_speed, dt);
384 state_.local_path_length = nav2_util::geometry_utils::calculate_path_length(plan);
385 path_ = utils::toTensor(plan);
386 costs_.setZero(settings_.batch_size);
388 critics_data_.fail_flag =
false;
389 critics_data_.goal_checker = goal_checker;
390 critics_data_.motion_model = motion_model_;
391 critics_data_.furthest_reached_path_point.reset();
392 critics_data_.path_pts_valid.reset();
397 auto size = control_sequence_.vx.size();
398 utils::shiftColumnsByOnePlace(control_sequence_.vx, -1);
399 utils::shiftColumnsByOnePlace(control_sequence_.wz, -1);
400 control_sequence_.vx(size - 1) = control_sequence_.vx(size - 2);
401 control_sequence_.wz(size - 1) = control_sequence_.wz(size - 2);
404 utils::shiftColumnsByOnePlace(control_sequence_.vy, -1);
405 control_sequence_.vy(size - 1) = control_sequence_.vy(size - 2);
422 auto & s = settings_;
423 float first_dt = s.controller_period;
424 float max_delta_vx = first_dt * s.constraints.ax_max;
425 float min_delta_vx = first_dt * s.constraints.ax_min;
426 float max_delta_vy = first_dt * s.constraints.ay_max;
427 float min_delta_vy = first_dt * s.constraints.ay_min;
428 float max_delta_wz = first_dt * s.constraints.az_max;
430 float speed_vx =
static_cast<float>(state_.speed.linear.x);
431 float speed_wz =
static_cast<float>(state_.speed.angular.z);
432 if (s.shift_control_sequence) {
435 control_sequence_.vx(0) = speed_vx;
436 control_sequence_.wz(0) = speed_wz;
438 control_sequence_.vy(0) =
static_cast<float>(state_.speed.linear.y);
442 control_sequence_.vx(0) = utils::clampVelocityByAccel(
443 speed_vx, control_sequence_.vx(0), min_delta_vx, max_delta_vx);
444 control_sequence_.wz(0) = utils::clampVelocityByAccel(
445 speed_wz, control_sequence_.wz(0), -max_delta_wz, max_delta_wz);
447 float speed_vy =
static_cast<float>(state_.speed.linear.y);
448 control_sequence_.vy(0) = utils::clampVelocityByAccel(
449 speed_vy, control_sequence_.vy(0), min_delta_vy, max_delta_vy);
456 auto & s = settings_;
459 motion_model_->applyConstraints(control_sequence_);
462 float first_dt = s.controller_period;
463 float max_delta_vx = first_dt * s.constraints.ax_max;
464 float min_delta_vx = first_dt * s.constraints.ax_min;
465 float max_delta_vy = first_dt * s.constraints.ay_max;
466 float min_delta_vy = first_dt * s.constraints.ay_min;
467 float max_delta_wz = first_dt * s.constraints.az_max;
470 float vx_last =
static_cast<float>(state_.speed.linear.x);
471 float wz_last =
static_cast<float>(state_.speed.angular.z);
472 float vy_last =
isHolonomic() ?
static_cast<float>(state_.speed.linear.y) : 0.0f;
476 if (s.shift_control_sequence) {
477 control_sequence_.vx(0) = vx_last;
478 control_sequence_.wz(0) = wz_last;
480 control_sequence_.vy(0) = vy_last;
484 for (
unsigned int i = 0; i != control_sequence_.vx.size(); i++) {
487 max_delta_vx = s.model_dt * s.constraints.ax_max;
488 min_delta_vx = s.model_dt * s.constraints.ax_min;
489 max_delta_vy = s.model_dt * s.constraints.ay_max;
490 min_delta_vy = s.model_dt * s.constraints.ay_min;
491 max_delta_wz = s.model_dt * s.constraints.az_max;
494 float & vx_curr = control_sequence_.vx(i);
495 vx_curr = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, vx_curr);
496 vx_curr = utils::clampVelocityByAccel(vx_last, vx_curr, min_delta_vx, max_delta_vx);
499 float & wz_curr = control_sequence_.wz(i);
500 wz_curr = utils::clamp(-s.constraints.wz, s.constraints.wz, wz_curr);
501 wz_curr = utils::clampVelocityByAccel(wz_last, wz_curr, -max_delta_wz, max_delta_wz);
505 float & vy_curr = control_sequence_.vy(i);
506 vy_curr = utils::clamp(-s.constraints.vy, s.constraints.vy, vy_curr);
507 vy_curr = utils::clampVelocityByAccel(vy_last, vy_curr, min_delta_vy, max_delta_vy);
513 motion_model_->applyConstraints(control_sequence_);
525 state.vx.col(0) =
static_cast<float>(state.speed.linear.x);
526 state.wz.col(0) =
static_cast<float>(state.speed.angular.z);
529 state.vy.col(0) =
static_cast<float>(state.speed.linear.y);
536 motion_model_->predict(state);
540 Eigen::Array<float, Eigen::Dynamic, 3> & trajectory,
541 const Eigen::ArrayXXf & sequence)
const
543 float initial_yaw =
static_cast<float>(tf2::getYaw(state_.pose.pose.orientation));
545 const auto vx = sequence.col(0);
546 const auto wz = sequence.col(1);
548 auto traj_x = trajectory.col(0);
549 auto traj_y = trajectory.col(1);
550 auto traj_yaws = trajectory.col(2);
552 const size_t n_size = traj_yaws.size();
557 float last_yaw = initial_yaw;
558 for (
size_t i = 0; i != n_size; i++) {
559 last_yaw += wz(i) * settings_.model_dt;
560 traj_yaws(i) = last_yaw;
563 Eigen::ArrayXf yaw_cos = traj_yaws.cos();
564 Eigen::ArrayXf yaw_sin = traj_yaws.sin();
565 utils::shiftColumnsByOnePlace(yaw_cos, 1);
566 utils::shiftColumnsByOnePlace(yaw_sin, 1);
567 yaw_cos(0) = cosf(initial_yaw);
568 yaw_sin(0) = sinf(initial_yaw);
570 auto dx = (vx * yaw_cos).eval();
571 auto dy = (vx * yaw_sin).eval();
574 auto vy = sequence.col(2);
575 dx = (dx - vy * yaw_sin).eval();
576 dy = (dy + vy * yaw_cos).eval();
579 float last_x = state_.pose.pose.position.x;
580 float last_y = state_.pose.pose.position.y;
581 for (
size_t i = 0; i != n_size; i++) {
582 last_x += dx(i) * settings_.model_dt;
583 last_y += dy(i) * settings_.model_dt;
593 auto initial_yaw =
static_cast<float>(tf2::getYaw(state.pose.pose.orientation));
594 const size_t n_cols = trajectories.yaws.cols();
596 Eigen::ArrayXf last_yaws = Eigen::ArrayXf::Constant(trajectories.yaws.rows(), initial_yaw);
597 for (
size_t i = 0; i != n_cols; i++) {
598 last_yaws += state.wz.col(i) * settings_.model_dt;
599 trajectories.yaws.col(i) = last_yaws;
602 Eigen::ArrayXXf yaw_cos = trajectories.yaws.cos();
603 Eigen::ArrayXXf yaw_sin = trajectories.yaws.sin();
604 utils::shiftColumnsByOnePlace(yaw_cos, 1);
605 utils::shiftColumnsByOnePlace(yaw_sin, 1);
606 yaw_cos.col(0) = cosf(initial_yaw);
607 yaw_sin.col(0) = sinf(initial_yaw);
609 auto dx = (state.vx * yaw_cos).eval();
610 auto dy = (state.vx * yaw_sin).eval();
613 dx -= state.vy * yaw_sin;
614 dy += state.vy * yaw_cos;
617 Eigen::ArrayXf last_x = Eigen::ArrayXf::Constant(
618 trajectories.x.rows(),
619 state.pose.pose.position.x);
620 Eigen::ArrayXf last_y = Eigen::ArrayXf::Constant(
621 trajectories.y.rows(),
622 state.pose.pose.position.y);
624 for (
size_t i = 0; i != n_cols; i++) {
625 last_x += dx.col(i) * settings_.model_dt;
626 last_y += dy.col(i) * settings_.model_dt;
627 trajectories.x.col(i) = last_x;
628 trajectories.y.col(i) = last_y;
635 Eigen::ArrayXXf sequence = Eigen::ArrayXXf(settings_.time_steps, is_holo ? 3 : 2);
636 Eigen::Array<float, Eigen::Dynamic, 3> trajectories =
637 Eigen::Array<float, Eigen::Dynamic, 3>(settings_.time_steps, 3);
639 sequence.col(0) = control_sequence_.vx;
640 sequence.col(1) = control_sequence_.wz;
643 sequence.col(2) = control_sequence_.vy;
652 return control_sequence_;
658 auto & s = settings_;
660 auto vx_T = control_sequence_.vx.transpose();
661 auto bounded_noises_vx = state_.cvx.rowwise() - vx_T;
662 const float gamma_vx = s.gamma / (s.sampling_std.vx * s.sampling_std.vx);
663 costs_ += (gamma_vx * (bounded_noises_vx.rowwise() * vx_T).rowwise().sum()).eval();
665 if (s.sampling_std.wz > 0.0f) {
666 auto wz_T = control_sequence_.wz.transpose();
667 auto bounded_noises_wz = state_.cwz.rowwise() - wz_T;
668 const float gamma_wz = s.gamma / (s.sampling_std.wz * s.sampling_std.wz);
669 costs_ += (gamma_wz * (bounded_noises_wz.rowwise() * wz_T).rowwise().sum()).eval();
673 auto vy_T = control_sequence_.vy.transpose();
674 auto bounded_noises_vy = state_.cvy.rowwise() - vy_T;
675 const float gamma_vy = s.gamma / (s.sampling_std.vy * s.sampling_std.vy);
676 costs_ += (gamma_vy * (bounded_noises_vy.rowwise() * vy_T).rowwise().sum()).eval();
679 auto costs_normalized = costs_ - costs_.minCoeff();
680 const float inv_temp = 1.0f / s.temperature;
681 auto softmaxes = (-inv_temp * costs_normalized).exp().eval();
682 softmaxes /= softmaxes.sum();
684 auto softmax_mat = softmaxes.matrix();
685 control_sequence_.vx = state_.cvx.transpose().matrix() * softmax_mat;
686 control_sequence_.wz = state_.cwz.transpose().matrix() * softmax_mat;
689 control_sequence_.vy = state_.cvy.transpose().matrix() * softmax_mat;
692 utils::savitskyGolayFilter(control_sequence_, control_history_, settings_);
698 const builtin_interfaces::msg::Time & stamp)
700 unsigned int offset = settings_.shift_control_sequence ? 1 : 0;
702 auto vx = control_sequence_.vx(offset);
703 auto wz = control_sequence_.wz(offset);
704 auto vy =
isHolonomic() ? control_sequence_.vy(offset) : 0.0f;
707 motion_model_->pushCommandHistory(vx, vy, wz);
710 return utils::toTwistStamped(vx, vy, wz, stamp, costmap_ros_->getBaseFrameID());
713 return utils::toTwistStamped(vx, wz, stamp, costmap_ros_->getBaseFrameID());
718 auto node = parent_.lock();
719 const std::string plugin_ns = name_ +
"." + motion_model_name;
720 std::string plugin_type;
721 motion_model_loader_ =
722 std::make_unique<pluginlib::ClassLoader<MotionModel>>(
723 "nav2_mppi_controller",
"mppi::MotionModel");
726 plugin_type = nav2::get_plugin_type_param(node, plugin_ns);
727 motion_model_ = motion_model_loader_->createSharedInstance(plugin_type);
728 motion_model_->initialize(parameters_handler_, plugin_ns);
729 motion_model_->setConstraints(settings_.constraints, settings_.model_dt,
730 settings_.model_delay_vx, settings_.model_delay_vy, settings_.model_delay_wz,
731 settings_.clamp_raw_controls);
732 }
catch (
const pluginlib::PluginlibException & ex) {
734 std::string(
"Failed to load motion model plugin '") + motion_model_name +
738 RCLCPP_INFO(
logger_,
"Loaded motion model plugin: %s", plugin_type.c_str());
743 auto & s = settings_;
744 if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) {
745 s.constraints.vx_max = s.base_constraints.vx_max;
746 s.constraints.vx_min = s.base_constraints.vx_min;
747 s.constraints.vy = s.base_constraints.vy;
748 s.constraints.wz = s.base_constraints.wz;
752 double ratio = speed_limit / 100.0;
753 s.constraints.vx_max = s.base_constraints.vx_max * ratio;
754 s.constraints.vx_min = s.base_constraints.vx_min * ratio;
755 s.constraints.vy = s.base_constraints.vy * ratio;
756 s.constraints.wz = s.base_constraints.wz * ratio;
759 double ratio = speed_limit / s.base_constraints.vx_max;
760 s.constraints.vx_max = s.base_constraints.vx_max * ratio;
761 s.constraints.vx_min = s.base_constraints.vx_min * ratio;
762 s.constraints.vy = s.base_constraints.vy * ratio;
763 s.constraints.wz = s.base_constraints.wz * ratio;
766 motion_model_->setConstraints(settings_.constraints, settings_.model_dt,
767 settings_.model_delay_vx, settings_.model_delay_vy, settings_.model_delay_wz,
768 settings_.clamp_raw_controls);
773 return generated_trajectories_;
void on_configure(nav2::LifecycleNode::WeakPtr parent, const std::string &name, std::shared_ptr< nav2_costmap_2d::Costmap2DROS >, ParametersHandler *)
Configure critic manager on bringup and load plugins.
void evalTrajectoriesScores(CriticData &data)
Score trajectories by the set of loaded critic functions.
void reset(mppi::models::OptimizerSettings &settings, bool is_holonomic)
Reset noise generator with settings and model types.
void initialize(mppi::models::OptimizerSettings &settings, bool is_holonomic, const std::string &name, ParametersHandler *param_handler)
Initialize noise generator with settings and model types.
void setNoisedControls(models::State &state, const models::ControlSequence &control_sequence)
set noised control_sequence to state controls
void shutdown()
Shutdown noise generator thread.
void generateNextNoises()
Signal to the noise thread the controller is ready to generate a new noised control for the next iter...
void updateStateVelocities(models::State &state) const
Update velocities in state.
const models::ControlSequence & getOptimalControlSequence()
Get the optimal control sequence for a cycle for visualization.
void setMotionModel(const std::string &model)
Set the motion model of the vehicle platform.
Eigen::ArrayXXf getOptimizedTrajectory()
Get the optimal trajectory for a cycle for visualization.
void reset(bool reset_dynamic_speed_limits=true)
Reset the optimization problem to initial conditions.
rclcpp::Logger logger_
Caution, keep references.
void prepare(const geometry_msgs::msg::PoseStamped &robot_pose, const geometry_msgs::msg::Twist &robot_speed, const nav_msgs::msg::Path &plan, const geometry_msgs::msg::Pose &goal, nav2_core::GoalChecker *goal_checker)
Prepare state information on new request for trajectory rollouts.
std::tuple< geometry_msgs::msg::TwistStamped, Eigen::ArrayXXf > evalControl(const geometry_msgs::msg::PoseStamped &robot_pose, const geometry_msgs::msg::Twist &robot_speed, const nav_msgs::msg::Path &plan, const geometry_msgs::msg::Pose &goal, nav2_core::GoalChecker *goal_checker)
Compute control using MPPI algorithm.
void integrateStateVelocities(models::Trajectories &trajectories, const models::State &state) const
Rollout velocities in state to poses.
void applyControlSequenceInterIterationConstraints()
Apply inter-iteration dynamic feasibility constraints on the first control sequence element before no...
void updateControlSequence()
Update control sequence with state controls weighted by costs using softmax function.
bool isSpeedLimitActive() const
Check if a dynamic speed limit is currently active.
void generateNoisedTrajectories()
updates generated trajectories with noised trajectories from the last cycle's optimal control
bool fallback(bool fail)
Perform fallback behavior to try to recover from a set of trajectories in collision.
bool isHolonomic() const
Whether the motion model is holonomic.
models::Trajectories & getGeneratedTrajectories()
Get the trajectories generated in a cycle for visualization.
void updateInitialStateVelocities(models::State &state) const
Update initial velocity in state.
void shutdown()
Shutdown for optimizer at process end.
void optimize()
Main function to generate, score, and return trajectories.
void setOffset(double controller_period)
Using control period and time step size, determine if trajectory offset should be used to populate in...
void setSpeedLimit(double speed_limit, bool percentage)
Set the maximum speed based on the speed limits callback.
void shiftControlSequence()
Shift the optimal control sequence after processing for next iterations initial conditions after exec...
void applyControlSequenceConstraints()
Apply hard vehicle constraints on control sequence.
void getParams()
Obtain the main controller's parameters.
void propagateStateVelocitiesFromInitials(models::State &state) const
predict velocities in state using model for time horizon equal to timesteps
geometry_msgs::msg::TwistStamped getControlFromSequenceAsTwist(const builtin_interfaces::msg::Time &stamp)
Convert control sequence to a twist command.
void initialize(nav2::LifecycleNode::WeakPtr parent, const std::string &name, std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmap_ros, nav2::TransformBuffer::SharedPtr tf_buffer, ParametersHandler *dynamic_parameters_handler)
Initializes optimizer on startup.
Handles getting parameters and dynamic parameter changes.
void addPostCallback(T &&callback)
Set a callback to process after parameter changes.
void addPreCallback(const std::string &name, T &&callback)
Set a callback to process before parameter changes.
auto getParamGetter(const std::string &ns)
Get an object to retrieve parameters.
Function-object for checking whether a goal has been reached.
A control sequence over time (e.g. trajectory)
State information: velocities, controls, poses, speed.
void reset(unsigned int batch_size, unsigned int time_steps)
Reset state data.
void reset(unsigned int batch_size, unsigned int time_steps)
Reset state data.