From c757f43624c2d948c3d258fae3513eb9bc232206 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 6 Oct 2022 14:30:25 -0700 Subject: [PATCH 01/53] adding gamma term on noise smoothing --- README.md | 1 + include/mppic/models/optimizer_settings.hpp | 1 + src/optimizer.cpp | 19 +++++++++++++++++++ 3 files changed, 21 insertions(+) diff --git a/README.md b/README.md index 1dd80a39..90bbdf9e 100644 --- a/README.md +++ b/README.md @@ -70,6 +70,7 @@ sudo make install | vx_min | double | Min VX | | wz_max | double | Max WZ | | temperature | double | Selectiveness of trajectories by their costs (The closer this value to 0, the "more" we take in considiration controls with less cost), 0 mean use control with best cost, huge value will lead to just taking mean of all trajectories without cost consideration | + | gamma | double | A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default of `0.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information. | | visualize | bool | Use visualization | | retry_attempt_limit | int | Number of attempts to find feasible trajectory before failure | diff --git a/include/mppic/models/optimizer_settings.hpp b/include/mppic/models/optimizer_settings.hpp index 3359901f..bbd4636b 100644 --- a/include/mppic/models/optimizer_settings.hpp +++ b/include/mppic/models/optimizer_settings.hpp @@ -14,6 +14,7 @@ struct OptimizerSettings models::SamplingStd sampling_std{0, 0, 0}; float model_dt{0}; float temperature{0}; + float gamma{0}; unsigned int batch_size{0}; unsigned int time_steps{0}; unsigned int iteration_count{0}; diff --git a/src/optimizer.cpp b/src/optimizer.cpp index 214c67b0..144b45da 100644 --- a/src/optimizer.cpp +++ b/src/optimizer.cpp @@ -58,6 +58,7 @@ void Optimizer::getParams() getParam(s.batch_size, "batch_size", 400); getParam(s.iteration_count, "iteration_count", 1); getParam(s.temperature, "temperature", 0.25f); + getParam(s.gamma, "gamma", 0.10f); getParam(s.base_constraints.vx_max, "vx_max", 0.5); getParam(s.base_constraints.vx_min, "vx_min", -0.35); getParam(s.base_constraints.vy, "vy_max", 0.5); @@ -333,6 +334,24 @@ xt::xtensor Optimizer::getOptimizedTrajectory() void Optimizer::updateControlSequence() { + auto & s = settings_; + auto bounded_noises_vx = state_.cvx - control_sequence_.vx; + auto bounded_noises_wz = state_.cwz - control_sequence_.wz; + xt::noalias(costs_) += + s.gamma / s.sampling_std.vx * xt::sum( + xt::view(control_sequence_.vx, xt::newaxis(), xt::all()) * bounded_noises_vx, 1, immediate); + xt::noalias(costs_) += + s.gamma / s.sampling_std.wz * xt::sum( + xt::view(control_sequence_.wz, xt::newaxis(), xt::all()) * bounded_noises_wz, 1, immediate); + + if (isHolonomic()) { + auto bounded_noises_vy = state_.cvy - control_sequence_.vy; + xt::noalias(costs_) += + s.gamma / s.sampling_std.vy * xt::sum( + xt::view(control_sequence_.vy, xt::newaxis(), xt::all()) * bounded_noises_vy, + 1, immediate); + } + auto && costs_normalized = costs_ - xt::amin(costs_, immediate); auto && exponents = xt::eval(xt::exp(-1 / settings_.temperature * costs_normalized)); auto && softmaxes = xt::eval(exponents / xt::sum(exponents, immediate)); From 5f27dc7d2ee7a29df00445cbbef04e41b84ea291 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 6 Oct 2022 14:42:51 -0700 Subject: [PATCH 02/53] updating default for temp --- src/optimizer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/optimizer.cpp b/src/optimizer.cpp index 144b45da..5f7f33dc 100644 --- a/src/optimizer.cpp +++ b/src/optimizer.cpp @@ -57,7 +57,7 @@ void Optimizer::getParams() getParam(s.time_steps, "time_steps", 15); getParam(s.batch_size, "batch_size", 400); getParam(s.iteration_count, "iteration_count", 1); - getParam(s.temperature, "temperature", 0.25f); + getParam(s.temperature, "temperature", 0.15f); getParam(s.gamma, "gamma", 0.10f); getParam(s.base_constraints.vx_max, "vx_max", 0.5); getParam(s.base_constraints.vx_min, "vx_min", -0.35); From bcfa06627a52f3c24bc63a4fc5254044c7f950c7 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Mon, 10 Oct 2022 17:17:25 -0700 Subject: [PATCH 03/53] add squared terms to control cost --- src/optimizer.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/optimizer.cpp b/src/optimizer.cpp index 5f7f33dc..9994874b 100644 --- a/src/optimizer.cpp +++ b/src/optimizer.cpp @@ -338,16 +338,16 @@ void Optimizer::updateControlSequence() auto bounded_noises_vx = state_.cvx - control_sequence_.vx; auto bounded_noises_wz = state_.cwz - control_sequence_.wz; xt::noalias(costs_) += - s.gamma / s.sampling_std.vx * xt::sum( + s.gamma / std::pow(s.sampling_std.vx, 2) * xt::sum( xt::view(control_sequence_.vx, xt::newaxis(), xt::all()) * bounded_noises_vx, 1, immediate); xt::noalias(costs_) += - s.gamma / s.sampling_std.wz * xt::sum( + s.gamma / std::pow(s.sampling_std.wz, 2) * xt::sum( xt::view(control_sequence_.wz, xt::newaxis(), xt::all()) * bounded_noises_wz, 1, immediate); if (isHolonomic()) { auto bounded_noises_vy = state_.cvy - control_sequence_.vy; xt::noalias(costs_) += - s.gamma / s.sampling_std.vy * xt::sum( + s.gamma / std::pow(s.sampling_std.vy, 2) * xt::sum( xt::view(control_sequence_.vy, xt::newaxis(), xt::all()) * bounded_noises_vy, 1, immediate); } From 2a2051db6a43bcd62cc191d7eb80ef75c21059a6 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Tue, 11 Oct 2022 15:37:11 -0700 Subject: [PATCH 04/53] adding updated smoother obstacles critic --- src/critics/goal_critic.cpp | 1 - src/critics/obstacles_critic.cpp | 9 +++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/critics/goal_critic.cpp b/src/critics/goal_critic.cpp index e8184ab5..b3e92edd 100644 --- a/src/critics/goal_critic.cpp +++ b/src/critics/goal_critic.cpp @@ -17,7 +17,6 @@ void GoalCritic::initialize() void GoalCritic::score(CriticData & data) { - if (!enabled_) { return; } diff --git a/src/critics/obstacles_critic.cpp b/src/critics/obstacles_critic.cpp index eb47a39f..225bef31 100644 --- a/src/critics/obstacles_critic.cpp +++ b/src/critics/obstacles_critic.cpp @@ -9,7 +9,7 @@ void ObstaclesCritic::initialize() auto getParam = parameters_handler_->getParamGetter(name_); getParam(consider_footprint_, "consider_footprint", false); getParam(power_, "cost_power", 2); - getParam(weight_, "cost_weight", 1.25); + getParam(weight_, "cost_weight", 2.0); getParam(collision_cost_, "collision_cost", 2000.0); collision_checker_.setCostmap(costmap_); @@ -61,17 +61,18 @@ void ObstaclesCritic::score(CriticData & data) return; } + const size_t traj_len = data.trajectories.x.shape(1); bool all_trajectories_collide = true; for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) { bool trajectory_collide = false; unsigned char trajectory_cost = nav2_costmap_2d::FREE_SPACE; - for (size_t j = 0; j < data.trajectories.x.shape(1); j++) { + for (size_t j = 0; j < traj_len; j++) { unsigned char pose_cost = costAtPose( data.trajectories.x(i, j), data.trajectories.y(i, j), data.trajectories.yaws(i, j)); - trajectory_cost = std::max(trajectory_cost, pose_cost); + trajectory_cost += pose_cost; - if (inCollision(trajectory_cost)) { + if (inCollision(pose_cost)) { trajectory_collide = true; break; } From b3b03a4e30234cefa2580966d768ac29d27c66ab Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 12 Oct 2022 12:13:57 -0700 Subject: [PATCH 05/53] fix normalization term --- src/critics/path_align_critic.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/critics/path_align_critic.cpp b/src/critics/path_align_critic.cpp index 09a966e3..6b5f9c31 100644 --- a/src/critics/path_align_critic.cpp +++ b/src/critics/path_align_critic.cpp @@ -69,6 +69,7 @@ void PathAlignCritic::score(CriticData & data) return dx * dx + dy * dy; }; + size_t traj_pts_eval = floor(time_steps / trajectory_point_step_); size_t max_s = 0; for (size_t t = 0; t < batch_size; ++t) { float mean_dist = 0; @@ -101,7 +102,7 @@ void PathAlignCritic::score(CriticData & data) mean_dist += std::sqrt(min_dist_sq); } - cost(t) = mean_dist / time_steps; + cost(t) = mean_dist / traj_pts_eval; } data.furthest_reached_path_point = max_s; From dc64589e20c4faa4397db5cd0226a7f8a43adadb Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 12 Oct 2022 14:42:36 -0700 Subject: [PATCH 06/53] minor optimizations in visualizer code --- include/mppic/tools/trajectory_visualizer.hpp | 2 +- src/trajectory_visualizer.cpp | 35 +++++++++++-------- 2 files changed, 22 insertions(+), 15 deletions(-) diff --git a/include/mppic/tools/trajectory_visualizer.hpp b/include/mppic/tools/trajectory_visualizer.hpp index 03c382b5..d6263fab 100644 --- a/include/mppic/tools/trajectory_visualizer.hpp +++ b/include/mppic/tools/trajectory_visualizer.hpp @@ -32,7 +32,7 @@ class TrajectoryVisualizer void add(const xt::xtensor & trajectory); void add(const models::Trajectories & trajectories); - void visualize(nav_msgs::msg::Path plan); + void visualize(const nav_msgs::msg::Path & plan); void reset(); protected: diff --git a/src/trajectory_visualizer.cpp b/src/trajectory_visualizer.cpp index e3e5fc8c..80a13f37 100644 --- a/src/trajectory_visualizer.cpp +++ b/src/trajectory_visualizer.cpp @@ -8,7 +8,8 @@ namespace mppi namespace { -geometry_msgs::msg::Pose createPose(double x, double y, double z) + +inline geometry_msgs::msg::Pose createPose(double x, double y, double z) { geometry_msgs::msg::Pose pose; pose.position.x = x; @@ -21,7 +22,7 @@ geometry_msgs::msg::Pose createPose(double x, double y, double z) return pose; } -geometry_msgs::msg::Vector3 createScale(double x, double y, double z) +inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z) { geometry_msgs::msg::Vector3 scale; scale.x = x; @@ -30,7 +31,7 @@ geometry_msgs::msg::Vector3 createScale(double x, double y, double z) return scale; } -std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a) +inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a) { std_msgs::msg::ColorRGBA color; color.r = r; @@ -40,7 +41,7 @@ std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a) return color; } -visualization_msgs::msg::Marker createMarker( +inline visualization_msgs::msg::Marker createMarker( int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale, const std_msgs::msg::ColorRGBA & color, const std::string & frame_id) { @@ -116,22 +117,23 @@ void TrajectoryVisualizer::add(const xt::xtensor & trajectory) points_->markers.push_back(marker); }; - for (size_t i = 0; i < size - 1; i += time_step_) { + for (size_t i = 0; i < size; i++) { add_marker(i); } - - add_marker(size - 1); } void TrajectoryVisualizer::add( const models::Trajectories & trajectories) { - auto & shape = trajectories.x.shape(); + const float shape_1 = static_cast(shape[1]); + points_->markers.reserve(floor(shape[0] / trajectory_step_) * floor(shape[1] * time_step_)); + for (size_t i = 0; i < shape[0]; i += trajectory_step_) { for (size_t j = 0; j < shape[1]; j += time_step_) { - float blue_component = 1.0f - static_cast(j) / static_cast(shape[1]); - float green_component = static_cast(j) / static_cast(shape[1]); + const float j_flt = static_cast(j); + float blue_component = 1.0f - j_flt / shape_1; + float green_component = j_flt / shape_1; auto pose = createPose(trajectories.x(i, j), trajectories.y(i, j), 0.03); auto scale = createScale(0.03, 0.03, 0.03); @@ -149,13 +151,18 @@ void TrajectoryVisualizer::reset() points_ = std::make_unique(); } -void TrajectoryVisualizer::visualize(nav_msgs::msg::Path plan) +void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan) { - trajectories_publisher_->publish(std::move(points_)); + if (trajectories_publisher_->get_subscription_count() > 0) { + trajectories_publisher_->publish(std::move(points_)); + } + reset(); - auto plan_ptr = std::make_unique(std::move(plan)); - transformed_path_pub_->publish(std::move(plan_ptr)); + if (transformed_path_pub_->get_subscription_count() > 0) { + auto plan_ptr = std::make_unique(plan); + transformed_path_pub_->publish(std::move(plan_ptr)); + } } } // namespace mppi From 7b5fcc1e9798725e0b4dc61eb33cf15663883a1e Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 12 Oct 2022 16:38:26 -0700 Subject: [PATCH 07/53] fixing a math typo --- src/critics/path_align_critic.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/critics/path_align_critic.cpp b/src/critics/path_align_critic.cpp index 6b5f9c31..86f7a86e 100644 --- a/src/critics/path_align_critic.cpp +++ b/src/critics/path_align_critic.cpp @@ -57,7 +57,7 @@ void PathAlignCritic::score(CriticData & data) const auto && P2_P1_norm_sq = xt::eval(P2_P1_dx * P2_P1_dx + P2_P1_dy * P2_P1_dy); auto evaluate_u = [&](size_t t, size_t p, size_t s) { - return ((P3_x(t, p) - P1_x(s)) * P2_P1_dx(s)) + ((P3_y(t, p) - P1_y(s)) * P2_P1_dy(s)) / + return (((P3_x(t, p) - P1_x(s)) * P2_P1_dx(s)) + ((P3_y(t, p) - P1_y(s)) * P2_P1_dy(s))) / P2_P1_norm_sq(s); }; From b92c8375fde06fbc70caba35f55185789b705a77 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 12 Oct 2022 17:09:48 -0700 Subject: [PATCH 08/53] integrated cost for obstacle critic + normalizing by trajectory length --- include/mppic/critics/obstacles_critic.hpp | 5 +++-- src/critics/obstacles_critic.cpp | 7 +++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/mppic/critics/obstacles_critic.hpp b/include/mppic/critics/obstacles_critic.hpp index f4604c02..0ac5028c 100644 --- a/include/mppic/critics/obstacles_critic.hpp +++ b/include/mppic/critics/obstacles_critic.hpp @@ -25,7 +25,7 @@ class ObstaclesCritic : public CriticFunction protected: bool inCollision(unsigned char cost) const; - double scoreCost(unsigned char cost); + double scoreCost(unsigned char cost, const size_t traj_len); unsigned char maxCost(); unsigned char costAtPose(double x, double y, double theta); @@ -43,7 +43,8 @@ class ObstaclesCritic : public CriticFunction collision_checker_{nullptr}; bool consider_footprint_{true}; - double collision_cost_{0}; + float collision_cost_{0}; + unsigned char possibly_inscribed_cost_; unsigned int power_{0}; diff --git a/src/critics/obstacles_critic.cpp b/src/critics/obstacles_critic.cpp index 225bef31..7e259bf5 100644 --- a/src/critics/obstacles_critic.cpp +++ b/src/critics/obstacles_critic.cpp @@ -80,8 +80,7 @@ void ObstaclesCritic::score(CriticData & data) if (!trajectory_collide) {all_trajectories_collide = false;} data.costs[i] += - trajectory_collide ? collision_cost_ : scoreCost(trajectory_cost); - + trajectory_collide ? collision_cost_ : scoreCost(trajectory_cost, traj_len); } data.fail_flag = all_trajectories_collide; @@ -126,11 +125,11 @@ unsigned char ObstaclesCritic::maxCost() nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1; } -double ObstaclesCritic::scoreCost(unsigned char cost_arg) +double ObstaclesCritic::scoreCost(unsigned char cost_arg, const size_t traj_len) { const double max_cost = static_cast(maxCost()); - double cost = static_cast(cost_arg) / max_cost; + double cost = static_cast(cost_arg) / (max_cost * traj_len); return pow(cost * weight_, power_); } From 10b88661150eb533585024a5a61e6b8d735a4772 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 13 Oct 2022 11:59:01 -0700 Subject: [PATCH 09/53] adding moved control clamping + constraint critic --- CMakeLists.txt | 1 + critics.xml | 3 ++ include/mppic/critic_function.hpp | 4 +- include/mppic/critics/constraint_critic.hpp | 30 +++++++++++++ include/mppic/optimizer.hpp | 2 +- src/critic_manager.cpp | 2 +- src/critics/constraint_critic.cpp | 49 +++++++++++++++++++++ src/optimizer.cpp | 13 +++--- 8 files changed, 94 insertions(+), 10 deletions(-) create mode 100644 include/mppic/critics/constraint_critic.hpp create mode 100644 src/critics/constraint_critic.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a306dc99..a426031a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,6 +53,7 @@ add_library(critics SHARED src/critics/path_angle_critic.cpp src/critics/prefer_forward_critic.cpp src/critics/twirling_critic.cpp + src/critics/constraint_critic.cpp ) set(libraries mppic critics) diff --git a/critics.xml b/critics.xml index 9119892b..e580496d 100644 --- a/critics.xml +++ b/critics.xml @@ -33,6 +33,9 @@ mppi critic for preventing twirling behavior when using omnidirectional models + + mppi critic for incentivizing moving within dynamic bounds + diff --git a/include/mppic/critic_function.hpp b/include/mppic/critic_function.hpp index 84048cbf..14b45833 100644 --- a/include/mppic/critic_function.hpp +++ b/include/mppic/critic_function.hpp @@ -21,6 +21,7 @@ class CriticFunction void on_configure( rclcpp_lifecycle::LifecycleNode::WeakPtr parent, + const std::string & parent_name, const std::string & name, std::shared_ptr costmap_ros, ParametersHandler * param_handler) @@ -28,6 +29,7 @@ class CriticFunction parent_ = parent; logger_ = parent_.lock()->get_logger(); name_ = name; + parent_name_ = parent_name; costmap_ros_ = costmap_ros; costmap_ = costmap_ros_->getCostmap(); parameters_handler_ = param_handler; @@ -49,7 +51,7 @@ class CriticFunction protected: bool enabled_; - std::string name_; + std::string name_, parent_name_; rclcpp_lifecycle::LifecycleNode::WeakPtr parent_; std::shared_ptr costmap_ros_; nav2_costmap_2d::Costmap2D * costmap_{nullptr}; diff --git a/include/mppic/critics/constraint_critic.hpp b/include/mppic/critics/constraint_critic.hpp new file mode 100644 index 00000000..ba97de48 --- /dev/null +++ b/include/mppic/critics/constraint_critic.hpp @@ -0,0 +1,30 @@ +// Copyright 2022 FastSense, Samsung Research +#pragma once + +#include "mppic/critic_function.hpp" +#include "mppic/models/state.hpp" +#include "mppic/tools/utils.hpp" + +namespace mppi::critics +{ + +class ConstraintCritic : public CriticFunction +{ +public: + void initialize() override; + + /** + * @brief Evaluate cost related to goal following + * + * @param costs [out] add reference cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + unsigned int power_{0}; + float weight_{0}; + float min_vel_; + float max_vel_; +}; + +} // namespace mppi::critics diff --git a/include/mppic/optimizer.hpp b/include/mppic/optimizer.hpp index 82be45d8..f303bd58 100644 --- a/include/mppic/optimizer.hpp +++ b/include/mppic/optimizer.hpp @@ -77,7 +77,7 @@ class Optimizer */ void generateNoisedTrajectories(); - void applyControlConstraints(); + void applyControlSequenceConstraints(); /** * @brief Update velocities in state_ diff --git a/src/critic_manager.cpp b/src/critic_manager.cpp index 95904d61..617c5697 100644 --- a/src/critic_manager.cpp +++ b/src/critic_manager.cpp @@ -39,7 +39,7 @@ void CriticManager::loadCritics() auto instance = std::unique_ptr( loader_->createUnmanagedInstance(fullname)); critics_.push_back(std::move(instance)); - critics_.back()->on_configure(parent_, name_ + "." + name, costmap_ros_, parameters_handler_); + critics_.back()->on_configure(parent_, name_, name_ + "." + name, costmap_ros_, parameters_handler_); RCLCPP_INFO(logger_, "Critic loaded : %s", fullname.c_str()); } } diff --git a/src/critics/constraint_critic.cpp b/src/critics/constraint_critic.cpp new file mode 100644 index 00000000..3b1d5e51 --- /dev/null +++ b/src/critics/constraint_critic.cpp @@ -0,0 +1,49 @@ +// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +#include "mppic/critics/constraint_critic.hpp" + +namespace mppi::critics +{ + +void ConstraintCritic::initialize() +{ + auto getParam = parameters_handler_->getParamGetter(name_); + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 1.0); + RCLCPP_INFO( + logger_, "ConstraintCritic instantiated with %d power and %f weight.", + power_, weight_); + + float vx_max, vy_max, vx_min, vy_min; + getParentParam(vx_max, "vx_max", 0.5); + getParentParam(vy_max, "vy_max", 0.0); + getParentParam(vx_min, "vx_min", -0.35); + getParentParam(vy_min, "vy_min", 0.0); + + const float min_sgn = vx_min > 0.0 ? 1.0 : -1.0; + max_vel_ = std::sqrt(vx_max * vx_max + vy_max * vy_max); + min_vel_ = min_sgn * std::sqrt(vx_min * vx_min + vy_min * vy_min); +} + +void ConstraintCritic::score(CriticData & data) +{ + using xt::evaluation_strategy::immediate; + + if (!enabled_) { + return; + } + + auto out_of_max_bounds_motion = xt::maximum(data.state.vx - max_vel_, 0); + auto out_of_min_bounds_motion = xt::maximum(min_vel_ - data.state.vx, 0); + data.costs += xt::pow( + xt::sum( + std::move(out_of_max_bounds_motion) + std::move(out_of_min_bounds_motion) * data.model_dt, + {1}, immediate) * weight_, power_); +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS(mppi::critics::ConstraintCritic, mppi::critics::CriticFunction) diff --git a/src/optimizer.cpp b/src/optimizer.cpp index 9994874b..3b36e147 100644 --- a/src/optimizer.cpp +++ b/src/optimizer.cpp @@ -199,25 +199,22 @@ void Optimizer::generateNoisedTrajectories() { noise_generator_.setNoisedControls(state_, control_sequence_); noise_generator_.generateNextNoises(); - applyControlConstraints(); updateStateVelocities(state_); integrateStateVelocities(generated_trajectories_, state_); } bool Optimizer::isHolonomic() const {return motion_model_->isHolonomic();} -void Optimizer::applyControlConstraints() +void Optimizer::applyControlSequenceConstraints() { auto & s = settings_; if (isHolonomic()) { - state_.cvy = xt::clip(state_.cvy, -s.constraints.vy, s.constraints.vy); + control_sequence_.vy = xt::clip(control_sequence_.vy, -s.constraints.vy, s.constraints.vy); } - state_.cvx = xt::clip(state_.cvx, s.constraints.vx_min, s.constraints.vx_max); - state_.cwz = xt::clip(state_.cwz, -s.constraints.wz, s.constraints.wz); - - motion_model_->applyConstraints(state_); + control_sequence_.vx = xt::clip(control_sequence_.vx, s.constraints.vx_min, s.constraints.vx_max); + control_sequence_.wz = xt::clip(control_sequence_.wz, -s.constraints.wz, s.constraints.wz); } void Optimizer::updateStateVelocities( @@ -360,6 +357,8 @@ void Optimizer::updateControlSequence() xt::noalias(control_sequence_.vx) = xt::sum(state_.cvx * softmaxes_extened, 0, immediate); xt::noalias(control_sequence_.vy) = xt::sum(state_.cvy * softmaxes_extened, 0, immediate); xt::noalias(control_sequence_.wz) = xt::sum(state_.cwz * softmaxes_extened, 0, immediate); + + applyControlSequenceConstraints(); } geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist( From 743dc48dab4e9d83c7e7b4aa67e25c7d8eb682f1 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 14 Oct 2022 15:54:00 -0700 Subject: [PATCH 10/53] adding last bits --- include/mppic/critic_data.hpp | 2 ++ include/mppic/motion_models.hpp | 13 ++++++++----- include/mppic/optimizer.hpp | 4 ++-- src/critics/constraint_critic.cpp | 21 +++++++++++++++++++-- src/optimizer.cpp | 9 ++++++--- 5 files changed, 37 insertions(+), 12 deletions(-) diff --git a/include/mppic/critic_data.hpp b/include/mppic/critic_data.hpp index 4d134b7e..06341e30 100644 --- a/include/mppic/critic_data.hpp +++ b/include/mppic/critic_data.hpp @@ -9,6 +9,7 @@ #include "mppic/models/state.hpp" #include "mppic/models/trajectories.hpp" #include "mppic/models/path.hpp" +#include "mppic/motion_models.hpp" namespace mppi @@ -25,6 +26,7 @@ struct CriticData bool fail_flag; nav2_core::GoalChecker * goal_checker; + std::shared_ptr motion_model; std::optional furthest_reached_path_point; }; diff --git a/include/mppic/motion_models.hpp b/include/mppic/motion_models.hpp index 3780f1f7..37f3b571 100644 --- a/include/mppic/motion_models.hpp +++ b/include/mppic/motion_models.hpp @@ -5,6 +5,7 @@ #include +#include "mppic/models/control_sequence.hpp" #include "mppic/models/state.hpp" #include #include @@ -38,7 +39,7 @@ class MotionModel } virtual bool isHolonomic() = 0; - virtual void applyConstraints(models::State & /*state*/) {} + virtual void applyConstraints(models::ControlSequence & /*state*/) {} }; class AckermannMotionModel : public MotionModel @@ -55,17 +56,19 @@ class AckermannMotionModel : public MotionModel return false; } - void applyConstraints(models::State & state) override + void applyConstraints(models::ControlSequence & control_sequence) override { - auto & vx = state.vx; - auto & wz = state.wz; + auto & vx = control_sequence.vx; + auto & wz = control_sequence.wz; auto view = xt::masked_view(wz, vx / wz > min_turning_r_); view = xt::sign(vx) / min_turning_r_; } + float getMinTurningRadius() {return min_turning_r_;} + private: - double min_turning_r_{0}; + float min_turning_r_{0}; }; class DiffDriveMotionModel : public MotionModel diff --git a/include/mppic/optimizer.hpp b/include/mppic/optimizer.hpp index f303bd58..4d5a0d64 100644 --- a/include/mppic/optimizer.hpp +++ b/include/mppic/optimizer.hpp @@ -124,7 +124,7 @@ class Optimizer nav2_costmap_2d::Costmap2D * costmap_; std::string name_; - std::unique_ptr motion_model_; + std::shared_ptr motion_model_; ParametersHandler * parameters_handler_; CriticManager critic_manager_; @@ -139,7 +139,7 @@ class Optimizer xt::xtensor costs_; CriticData critics_data_ = - {state_, generated_trajectories_, path_, costs_, settings_.model_dt, false, nullptr, + {state_, generated_trajectories_, path_, costs_, settings_.model_dt, false, nullptr, nullptr, std::nullopt}; /// Caution, keep references rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; diff --git a/src/critics/constraint_critic.cpp b/src/critics/constraint_critic.cpp index 3b1d5e51..cb435118 100644 --- a/src/critics/constraint_critic.cpp +++ b/src/critics/constraint_critic.cpp @@ -36,10 +36,27 @@ void ConstraintCritic::score(CriticData & data) auto out_of_max_bounds_motion = xt::maximum(data.state.vx - max_vel_, 0); auto out_of_min_bounds_motion = xt::maximum(min_vel_ - data.state.vx, 0); + + auto acker = dynamic_cast(data.motion_model.get()); + if (acker != nullptr) { + auto & vx = data.state.vx; + auto & wz = data.state.wz; + auto out_of_turning_rad_motion = xt::maximum( + acker->getMinTurningRadius() - (xt::fabs(vx) / xt::fabs(wz)), 0.0); + + data.costs += xt::pow( + xt::sum( + (std::move(out_of_max_bounds_motion) + + std::move(out_of_min_bounds_motion) + + std::move(out_of_turning_rad_motion)) + * data.model_dt, {1}, immediate) * weight_, power_); + } + data.costs += xt::pow( xt::sum( - std::move(out_of_max_bounds_motion) + std::move(out_of_min_bounds_motion) * data.model_dt, - {1}, immediate) * weight_, power_); + (std::move(out_of_max_bounds_motion) + + std::move(out_of_min_bounds_motion)) + * data.model_dt, {1}, immediate) * weight_, power_); } } // namespace mppi::critics diff --git a/src/optimizer.cpp b/src/optimizer.cpp index 3b36e147..14ed7680 100644 --- a/src/optimizer.cpp +++ b/src/optimizer.cpp @@ -172,6 +172,7 @@ void Optimizer::prepare( critics_data_.fail_flag = false; critics_data_.goal_checker = goal_checker; + critics_data_.motion_model = motion_model_; } void Optimizer::shiftControlSequence() @@ -215,6 +216,8 @@ void Optimizer::applyControlSequenceConstraints() control_sequence_.vx = xt::clip(control_sequence_.vx, s.constraints.vx_min, s.constraints.vx_max); control_sequence_.wz = xt::clip(control_sequence_.wz, -s.constraints.wz, s.constraints.wz); + + motion_model_->applyConstraints(control_sequence_); } void Optimizer::updateStateVelocities( @@ -380,11 +383,11 @@ geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist( void Optimizer::setMotionModel(const std::string & model) { if (model == "DiffDrive") { - motion_model_ = std::make_unique(); + motion_model_ = std::make_shared(); } else if (model == "Omni") { - motion_model_ = std::make_unique(); + motion_model_ = std::make_shared(); } else if (model == "Ackermann") { - motion_model_ = std::make_unique(parameters_handler_); + motion_model_ = std::make_shared(parameters_handler_); } else { throw std::runtime_error( std::string( From 2c6e77560e2d6d449db32ccb354cf4a9ff865a88 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 14 Oct 2022 16:16:52 -0700 Subject: [PATCH 11/53] minor updates to constraint critic --- critics.xml | 2 +- src/critics/constraint_critic.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/critics.xml b/critics.xml index e580496d..4a05fb87 100644 --- a/critics.xml +++ b/critics.xml @@ -34,7 +34,7 @@ - mppi critic for incentivizing moving within dynamic bounds + mppi critic for incentivizing moving within kinematic and dynamic bounds diff --git a/src/critics/constraint_critic.cpp b/src/critics/constraint_critic.cpp index cb435118..29b1524f 100644 --- a/src/critics/constraint_critic.cpp +++ b/src/critics/constraint_critic.cpp @@ -10,7 +10,7 @@ void ConstraintCritic::initialize() auto getParentParam = parameters_handler_->getParamGetter(parent_name_); getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 1.0); + getParam(weight_, "cost_weight", 4.0); RCLCPP_INFO( logger_, "ConstraintCritic instantiated with %d power and %f weight.", power_, weight_); From a516c3e7a3f0bd8f26ea058a6e7e85d599548891 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 14 Oct 2022 17:05:43 -0700 Subject: [PATCH 12/53] update readme with new critic --- README.md | 44 +++++++++++++++++++++++++++++--------------- 1 file changed, 29 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index 90bbdf9e..2cc7a268 100644 --- a/README.md +++ b/README.md @@ -75,7 +75,7 @@ sudo make install | retry_attempt_limit | int | Number of attempts to find feasible trajectory before failure | -#### AckermannConstrains params +#### Ackermann Motion Model params | Parameter | Type | Definition | | -------------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | min_turning_r | double | minimum turning radius for ackermann motion model | @@ -84,27 +84,35 @@ sudo make install #### GoalCritic params | Parameter | Type | Definition | | -------------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | goal_cost_weight | double | | - | goal_cost_power | int | | + | cost_weight | double | | + | cost_power | int | | #### GoalAngleCritic params | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | goal_angle_cost_weight | double | | - | goal_angle_cost_power | int | | + | cost_weight | double | | + | cost_power | int | | | threshold_to_consider_goal_angle | double | Minimal distance between robot and goal above which angle goal cost considered | +#### PathFollowCritic params + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | cost_weight | double | | + | cost_power | int | | + | offset_from_furthest | int | | + | max_path_ratio | float | | + #### PathAngleCritic params | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | path_angle_cost_weight | double | | - | path_angle_cost_power | int | | + | cost_weight | double | | + | cost_power | int | | #### PathAlignCritic params | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | path_align_cost_weight | double | | - | path_align_cost_power | int | | + | cost_weight | double | | + | cost_power | int | | | enable_nearest_goal_critic | bool | enable critic that scores by mean distance from generated trajectories to nearest to generated trajectories path points | | path_point_step | int | Consider path points with given step | | trajectory_point_step | int | Consider generated trajectories points with given step | @@ -114,20 +122,26 @@ sudo make install | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | consider_footprint | bool | | - | obstacle_cost_weight | double | | - | obstacle_cost_power | int | | + | cost_weight | double | | + | cost_power | int | | #### PreferForwardCritic params | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | prefer_forward_cost_weight | double | | - | prefer_forward_cost_power | int | | + | cost_weight | double | | + | cost_power | int | | #### TwirlingCritic params | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | twirling_cost_weight | double | | - | twirling_cost_power | int | | + | cost_weight | double | | + | cost_power | int | | +#### ConstraintsCritic params + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | cost_weight | double | | + | cost_power | int | + ### XML configuration example ``` From cf163695f4041826178486e735a8f943851d30f0 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Sat, 15 Oct 2022 18:04:36 -0700 Subject: [PATCH 13/53] using floats to manage cost, not unsigned chars, since we're adding them up now --- include/mppic/critics/obstacles_critic.hpp | 2 +- src/critics/obstacles_critic.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/mppic/critics/obstacles_critic.hpp b/include/mppic/critics/obstacles_critic.hpp index 0ac5028c..9a83fdad 100644 --- a/include/mppic/critics/obstacles_critic.hpp +++ b/include/mppic/critics/obstacles_critic.hpp @@ -25,7 +25,7 @@ class ObstaclesCritic : public CriticFunction protected: bool inCollision(unsigned char cost) const; - double scoreCost(unsigned char cost, const size_t traj_len); + double scoreCost(float cost, const size_t traj_len); unsigned char maxCost(); unsigned char costAtPose(double x, double y, double theta); diff --git a/src/critics/obstacles_critic.cpp b/src/critics/obstacles_critic.cpp index 7e259bf5..f256200e 100644 --- a/src/critics/obstacles_critic.cpp +++ b/src/critics/obstacles_critic.cpp @@ -65,12 +65,12 @@ void ObstaclesCritic::score(CriticData & data) bool all_trajectories_collide = true; for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) { bool trajectory_collide = false; - unsigned char trajectory_cost = nav2_costmap_2d::FREE_SPACE; + float trajectory_cost = 0.0; for (size_t j = 0; j < traj_len; j++) { unsigned char pose_cost = costAtPose( data.trajectories.x(i, j), data.trajectories.y(i, j), data.trajectories.yaws(i, j)); - trajectory_cost += pose_cost; + trajectory_cost += static_cast(pose_cost); if (inCollision(pose_cost)) { trajectory_collide = true; @@ -125,11 +125,11 @@ unsigned char ObstaclesCritic::maxCost() nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1; } -double ObstaclesCritic::scoreCost(unsigned char cost_arg, const size_t traj_len) +double ObstaclesCritic::scoreCost(float cost_arg, const size_t traj_len) { - const double max_cost = static_cast(maxCost()); + const float max_cost = static_cast(maxCost()); - double cost = static_cast(cost_arg) / (max_cost * traj_len); + float cost = cost_arg / (max_cost * static_cast(traj_len)); return pow(cost * weight_, power_); } From f8fec2eb990a0beee119bebc5b2f024b42584844 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 16 Nov 2022 16:55:02 -0800 Subject: [PATCH 14/53] distance-based obstacle critic --- README.md | 2 + include/mppic/critics/obstacles_critic.hpp | 21 ++++-- src/critics/obstacles_critic.cpp | 80 +++++++++++++++------- 3 files changed, 73 insertions(+), 30 deletions(-) diff --git a/README.md b/README.md index 79fa3089..709f08cf 100644 --- a/README.md +++ b/README.md @@ -128,6 +128,8 @@ sudo make install | consider_footprint | bool | | | cost_weight | double | | | cost_power | int | | + | collision_margin_distance | double | Margin distance from collision to apply severe penalty. Between 0.05-0.2 is reasonable. | + | trajectory_penalty_distance | double | Minimum trajectory distance from obstacle to apply a penalty to incentivize navigating farther away | #### PreferForwardCritic params | Parameter | Type | Definition | diff --git a/include/mppic/critics/obstacles_critic.hpp b/include/mppic/critics/obstacles_critic.hpp index 9a83fdad..3a9af420 100644 --- a/include/mppic/critics/obstacles_critic.hpp +++ b/include/mppic/critics/obstacles_critic.hpp @@ -11,6 +11,12 @@ namespace mppi::critics { +struct CollisionCost +{ + float cost; + bool using_footprint; +}; + class ObstaclesCritic : public CriticFunction { public: @@ -24,10 +30,10 @@ class ObstaclesCritic : public CriticFunction void score(CriticData & data) override; protected: - bool inCollision(unsigned char cost) const; - double scoreCost(float cost, const size_t traj_len); + bool inCollision(float cost) const; unsigned char maxCost(); - unsigned char costAtPose(double x, double y, double theta); + CollisionCost costAtPose(float x, float y, float theta); + float distanceToObstacle(const CollisionCost & cost); /** * @brief Find the min cost of the inflation decay function for which the robot MAY be @@ -36,16 +42,19 @@ class ObstaclesCritic : public CriticFunction * @return double circumscribed cost, any higher than this and need to do full footprint collision checking * since some element of the robot could be in collision */ - unsigned char findCircumscribedCost(std::shared_ptr costmap); + double findCircumscribedCost(std::shared_ptr costmap); protected: nav2_costmap_2d::FootprintCollisionChecker collision_checker_{nullptr}; bool consider_footprint_{true}; - float collision_cost_{0}; + double collision_cost_{0}; + float inflation_scale_factor_; - unsigned char possibly_inscribed_cost_; + float possibly_inscribed_cost_; + float trajectory_penalty_distance_; + float collision_margin_distance_; unsigned int power_{0}; float weight_{0}; diff --git a/src/critics/obstacles_critic.cpp b/src/critics/obstacles_critic.cpp index f256200e..d82d731b 100644 --- a/src/critics/obstacles_critic.cpp +++ b/src/critics/obstacles_critic.cpp @@ -1,4 +1,5 @@ // Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +#include #include "mppic/critics/obstacles_critic.hpp" namespace mppi::critics @@ -11,6 +12,8 @@ void ObstaclesCritic::initialize() getParam(power_, "cost_power", 2); getParam(weight_, "cost_weight", 2.0); getParam(collision_cost_, "collision_cost", 2000.0); + getParam(trajectory_penalty_distance_, "trajectory_penalty_distance", 1.0); + getParam(collision_margin_distance_, "collision_margin_distance", 0.12); collision_checker_.setCostmap(costmap_); possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); @@ -21,7 +24,7 @@ void ObstaclesCritic::initialize() power_, weight_, consider_footprint_ ? "footprint" : "circular"); } -unsigned char ObstaclesCritic::findCircumscribedCost( +double ObstaclesCritic::findCircumscribedCost( std::shared_ptr costmap) { double result = -1.0; @@ -40,6 +43,7 @@ unsigned char ObstaclesCritic::findCircumscribedCost( const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); const double resolution = costmap->getCostmap()->getResolution(); result = inflation_layer->computeCost(circum_radius / resolution); + inflation_scale_factor_ = static_cast(inflation_layer->getCostScalingFactor()); } if (!inflation_layer_found) { @@ -54,59 +58,96 @@ unsigned char ObstaclesCritic::findCircumscribedCost( return result; } +// improve path alignment / follow critics so doesn't try to shortcut turns +// higher driving force from path track / speed critics? + +// Testing without path align / follow critics. Only follow local path goal -- more flexible +// Testing with exact path following. +// Testing defaults to have generally good path following, but also can go around obstacles on path, and have smooth back out maneuvors + +float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost) +{ + const float scale_factor = inflation_scale_factor_; + const float min_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius(); + float dist_to_obj = (scale_factor * min_radius - log(cost.cost) + log(253.0f)) / scale_factor; + + // If not footprint collision checking, the cost is using the center point cost and + // needs the radius subtracted to obtain the closest distance to the object + if (!cost.using_footprint) { + dist_to_obj -= min_radius; + } + + return dist_to_obj; +} void ObstaclesCritic::score(CriticData & data) { + using xt::evaluation_strategy::immediate; if (!enabled_) { return; } + auto && raw_cost = xt::xtensor::from_shape({data.costs.shape(0)}); const size_t traj_len = data.trajectories.x.shape(1); bool all_trajectories_collide = true; for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) { bool trajectory_collide = false; - float trajectory_cost = 0.0; + float traj_cost = 0.0; + const auto & traj = data.trajectories; + float repulsion_cost = 0.0; for (size_t j = 0; j < traj_len; j++) { - unsigned char pose_cost = costAtPose( - data.trajectories.x(i, j), data.trajectories.y(i, j), data.trajectories.yaws(i, j)); - trajectory_cost += static_cast(pose_cost); + const CollisionCost pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j)); - if (inCollision(pose_cost)) { + if (inCollision(pose_cost.cost)) { trajectory_collide = true; break; } + + // TODO Give each term its own weight? + const float dist_to_obj = distanceToObstacle(pose_cost); + if (dist_to_obj < collision_margin_distance_) { + // Near-collision, all points must be punished + traj_cost += (collision_margin_distance_ - dist_to_obj); + } else if (dist_to_obj < trajectory_penalty_distance_) { + // Prefer general trajectories further from obstacles + repulsion_cost = std::max(repulsion_cost, (trajectory_penalty_distance_ - dist_to_obj)); + } } + traj_cost += repulsion_cost; if (!trajectory_collide) {all_trajectories_collide = false;} - data.costs[i] += - trajectory_collide ? collision_cost_ : scoreCost(trajectory_cost, traj_len); + raw_cost[i] = static_cast(trajectory_collide ? collision_cost_ : traj_cost); } + data.costs = xt::pow(raw_cost * weight_, power_); data.fail_flag = all_trajectories_collide; } -unsigned char ObstaclesCritic::costAtPose(double x, double y, double theta) +CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) { - unsigned char cost; + CollisionCost collision_cost; + float & cost = collision_cost.cost; + collision_cost.using_footprint = false; unsigned int x_i, y_i; collision_checker_.worldToMap(x, y, x_i, y_i); - cost = static_cast(collision_checker_.pointCost(x_i, y_i)); + cost = collision_checker_.pointCost(x_i, y_i); if (consider_footprint_ && cost >= possibly_inscribed_cost_) { - cost = static_cast(collision_checker_.footprintCostAtPose( + cost = static_cast(collision_checker_.footprintCostAtPose( x, y, theta, costmap_ros_->getRobotFootprint())); + collision_cost.using_footprint = true; } - return cost; + return collision_cost; } -bool ObstaclesCritic::inCollision(unsigned char cost) const +bool ObstaclesCritic::inCollision(float cost) const { bool is_tracking_unknown = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); - switch (cost) { + switch (static_cast(cost)) { using namespace nav2_costmap_2d; // NOLINT case (LETHAL_OBSTACLE): return true; @@ -125,15 +166,6 @@ unsigned char ObstaclesCritic::maxCost() nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1; } -double ObstaclesCritic::scoreCost(float cost_arg, const size_t traj_len) -{ - const float max_cost = static_cast(maxCost()); - - float cost = cost_arg / (max_cost * static_cast(traj_len)); - - return pow(cost * weight_, power_); -} - } // namespace mppi::critics #include From 84963f93ca33835c3ba39dc5daa2a1aa84d9af6f Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 17 Nov 2022 14:29:58 -0800 Subject: [PATCH 15/53] completed obstacle critic --- README.md | 3 ++- include/mppic/critics/obstacles_critic.hpp | 1 + src/critics/obstacles_critic.cpp | 16 ++++++++-------- src/critics/path_align_critic.cpp | 1 - 4 files changed, 11 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index 709f08cf..c2ac3772 100644 --- a/README.md +++ b/README.md @@ -129,7 +129,8 @@ sudo make install | cost_weight | double | | | cost_power | int | | | collision_margin_distance | double | Margin distance from collision to apply severe penalty. Between 0.05-0.2 is reasonable. | - | trajectory_penalty_distance | double | Minimum trajectory distance from obstacle to apply a penalty to incentivize navigating farther away | + | trajectory_penalty_distance | double | Minimum trajectory distance from obstacle to apply a preferential penalty to incentivize navigating farther away from obstacles. | + | near_goal_distance | double | Distance near goal to stop applying preferential obstacle term (e.g. `trajectory_penalty_distance` term) to allow robot to smoothly converge to goal pose in close proximity to obstacles. | #### PreferForwardCritic params | Parameter | Type | Definition | diff --git a/include/mppic/critics/obstacles_critic.hpp b/include/mppic/critics/obstacles_critic.hpp index 3a9af420..8431a79b 100644 --- a/include/mppic/critics/obstacles_critic.hpp +++ b/include/mppic/critics/obstacles_critic.hpp @@ -55,6 +55,7 @@ class ObstaclesCritic : public CriticFunction float possibly_inscribed_cost_; float trajectory_penalty_distance_; float collision_margin_distance_; + float near_goal_distance_; unsigned int power_{0}; float weight_{0}; diff --git a/src/critics/obstacles_critic.cpp b/src/critics/obstacles_critic.cpp index d82d731b..7064f189 100644 --- a/src/critics/obstacles_critic.cpp +++ b/src/critics/obstacles_critic.cpp @@ -14,6 +14,7 @@ void ObstaclesCritic::initialize() getParam(collision_cost_, "collision_cost", 2000.0); getParam(trajectory_penalty_distance_, "trajectory_penalty_distance", 1.0); getParam(collision_margin_distance_, "collision_margin_distance", 0.12); + getParam(near_goal_distance_, "near_goal_distance", 0.5); collision_checker_.setCostmap(costmap_); possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); @@ -58,12 +59,6 @@ double ObstaclesCritic::findCircumscribedCost( return result; } -// improve path alignment / follow critics so doesn't try to shortcut turns -// higher driving force from path track / speed critics? - -// Testing without path align / follow critics. Only follow local path goal -- more flexible -// Testing with exact path following. -// Testing defaults to have generally good path following, but also can go around obstacles on path, and have smooth back out maneuvors float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost) { @@ -87,6 +82,12 @@ void ObstaclesCritic::score(CriticData & data) return; } + // 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)) { + near_goal = true; + } + auto && raw_cost = xt::xtensor::from_shape({data.costs.shape(0)}); const size_t traj_len = data.trajectories.x.shape(1); bool all_trajectories_collide = true; @@ -104,12 +105,11 @@ void ObstaclesCritic::score(CriticData & data) break; } - // TODO Give each term its own weight? const float dist_to_obj = distanceToObstacle(pose_cost); if (dist_to_obj < collision_margin_distance_) { // Near-collision, all points must be punished traj_cost += (collision_margin_distance_ - dist_to_obj); - } else if (dist_to_obj < trajectory_penalty_distance_) { + } else if (dist_to_obj < trajectory_penalty_distance_ && !near_goal) { // Prefer general trajectories further from obstacles repulsion_cost = std::max(repulsion_cost, (trajectory_penalty_distance_ - dist_to_obj)); } diff --git a/src/critics/path_align_critic.cpp b/src/critics/path_align_critic.cpp index 9778fd27..01278d19 100644 --- a/src/critics/path_align_critic.cpp +++ b/src/critics/path_align_critic.cpp @@ -27,7 +27,6 @@ void PathAlignCritic::initialize() void PathAlignCritic::score(CriticData & data) { - utils::setPathFurthestPointIfNotSet(data); if (!enabled_ || utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) { return; From 2280b51d4e7b88968de22ff550173bf897f0f4c0 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 17 Nov 2022 14:47:00 -0800 Subject: [PATCH 16/53] update tuned defaults --- README.md | 23 +++++++++++++++-------- src/controller.cpp | 6 ++++++ src/critics/obstacles_critic.cpp | 2 +- src/critics/path_align_critic.cpp | 4 ++-- src/critics/prefer_forward_critic.cpp | 2 +- 5 files changed, 25 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index c2ac3772..6d3a8389 100644 --- a/README.md +++ b/README.md @@ -159,8 +159,8 @@ controller_server: controller_frequency: 30.0 FollowPath: plugin: "mppi::Controller" - time_steps: 30 - model_dt: 0.1 + time_steps: 56 + model_dt: 0.05 batch_size: 2000 vx_std: 0.2 vy_std: 0.2 @@ -172,7 +172,8 @@ controller_server: iteration_count: 1 prune_distance: 1.7 transform_tolerance: 0.1 - temperature: 0.35 + temperature: 0.3 + gamma: 0.015 motion_model: "DiffDrive" visualize: false TrajectoryVisualizer: @@ -180,7 +181,11 @@ controller_server: time_step: 3 AckermannConstrains: min_turning_r: 0.2 - critics: ["ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + critics: ["ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 GoalCritic: enabled: true cost_power: 1 @@ -193,14 +198,16 @@ controller_server: ObstaclesCritic: enabled: true cost_power: 2 - cost_weight: 1.65 + cost_weight: 1.2 consider_footprint: false collision_cost: 2000.0 + trajectory_penalty_distance: 1.0 + collision_margin_distance: 0.12 PathAlignCritic: enabled: true cost_power: 1 - cost_weight: 2.0 - path_point_step: 2 + cost_weight: 1.0 + path_point_step: 1 trajectory_point_step: 3 threshold_to_consider: 0.40 PathFollowCritic: @@ -218,7 +225,7 @@ controller_server: PreferForwardCritic: enabled: true cost_power: 1 - cost_weight: 3.0 + cost_weight: 5.0 threshold_to_consider: 0.4 # TwirlingCritic: # twirling_cost_power: 1 diff --git a/src/controller.cpp b/src/controller.cpp index 347e04f1..dbe70e4a 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -4,6 +4,8 @@ #include "mppic/controller.hpp" #include "mppic/tools/utils.hpp" +// #define BENCHMARK_TESTING + namespace mppi { @@ -59,7 +61,9 @@ geometry_msgs::msg::TwistStamped Controller::computeVelocityCommands( const geometry_msgs::msg::Twist & robot_speed, nav2_core::GoalChecker * goal_checker) { +#ifdef BENCHMARK_TESTING auto start = std::chrono::system_clock::now(); +#endif std::lock_guard lock(*parameters_handler_->getLock()); nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose); @@ -67,9 +71,11 @@ geometry_msgs::msg::TwistStamped Controller::computeVelocityCommands( geometry_msgs::msg::TwistStamped cmd = optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal_checker); +#ifdef BENCHMARK_TESTING auto end = std::chrono::system_clock::now(); auto duration = std::chrono::duration_cast(end - start).count(); RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration); +#endif if (visualize_) { visualize(std::move(transformed_plan)); diff --git a/src/critics/obstacles_critic.cpp b/src/critics/obstacles_critic.cpp index 7064f189..6ef629b2 100644 --- a/src/critics/obstacles_critic.cpp +++ b/src/critics/obstacles_critic.cpp @@ -10,7 +10,7 @@ void ObstaclesCritic::initialize() auto getParam = parameters_handler_->getParamGetter(name_); getParam(consider_footprint_, "consider_footprint", false); getParam(power_, "cost_power", 2); - getParam(weight_, "cost_weight", 2.0); + getParam(weight_, "cost_weight", 1.2); getParam(collision_cost_, "collision_cost", 2000.0); getParam(trajectory_penalty_distance_, "trajectory_penalty_distance", 1.0); getParam(collision_margin_distance_, "collision_margin_distance", 0.12); diff --git a/src/critics/path_align_critic.cpp b/src/critics/path_align_critic.cpp index 01278d19..a0c658ef 100644 --- a/src/critics/path_align_critic.cpp +++ b/src/critics/path_align_critic.cpp @@ -11,9 +11,9 @@ void PathAlignCritic::initialize() { auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 2.0); + getParam(weight_, "cost_weight", 1.0); - getParam(path_point_step_, "path_point_step", 2); + getParam(path_point_step_, "path_point_step", 1); getParam(trajectory_point_step_, "trajectory_point_step", 3); getParam( threshold_to_consider_, diff --git a/src/critics/prefer_forward_critic.cpp b/src/critics/prefer_forward_critic.cpp index 64284dd9..fef7a068 100644 --- a/src/critics/prefer_forward_critic.cpp +++ b/src/critics/prefer_forward_critic.cpp @@ -9,7 +9,7 @@ void PreferForwardCritic::initialize() { auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 3.0); + getParam(weight_, "cost_weight", 5.0); getParam( threshold_to_consider_, "threshold_to_consider", 0.40f); From ae6a9a71d77f852c5da5cbe91cc8b8cc86275663 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 18 Nov 2022 12:23:46 -0800 Subject: [PATCH 17/53] adding Savitzky-Golay filter on control output --- include/mppic/models/control_sequence.hpp | 5 ++ include/mppic/optimizer.hpp | 1 + include/mppic/tools/utils.hpp | 84 ++++++++++++++++++++++- src/optimizer.cpp | 3 + 4 files changed, 92 insertions(+), 1 deletion(-) diff --git a/include/mppic/models/control_sequence.hpp b/include/mppic/models/control_sequence.hpp index 1f47ac50..a198bc27 100644 --- a/include/mppic/models/control_sequence.hpp +++ b/include/mppic/models/control_sequence.hpp @@ -7,6 +7,11 @@ namespace mppi::models { +struct Control +{ + float vx, vy, wz; +}; + struct ControlSequence { xt::xtensor vx; diff --git a/include/mppic/optimizer.hpp b/include/mppic/optimizer.hpp index 4d5a0d64..3ddd75f4 100644 --- a/include/mppic/optimizer.hpp +++ b/include/mppic/optimizer.hpp @@ -134,6 +134,7 @@ class Optimizer models::State state_; models::ControlSequence control_sequence_; + std::array control_history_; models::Trajectories generated_trajectories_; models::Path path_; xt::xtensor costs_; diff --git a/include/mppic/tools/utils.hpp b/include/mppic/tools/utils.hpp index fe84df30..0609d80d 100644 --- a/include/mppic/tools/utils.hpp +++ b/include/mppic/tools/utils.hpp @@ -26,6 +26,7 @@ #include "nav2_util/node_utils.hpp" #include "nav2_core/goal_checker.hpp" +#include "mppic/models/optimizer_settings.hpp" #include "mppic/models/control_sequence.hpp" #include "mppic/models/path.hpp" #include "builtin_interfaces/msg/time.hpp" @@ -33,6 +34,7 @@ namespace mppi::utils { +using xt::evaluation_strategy::immediate; inline geometry_msgs::msg::TwistStamped toTwistStamped( float vx, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame) @@ -223,6 +225,86 @@ inline float getPathRatioReached(const CriticData & data) return furthest_reached_path_point / path_points_count; } + +inline void savitskyGolayFilter( + models::ControlSequence & control_sequence, + std::array & control_history, + const models::OptimizerSettings & settings) +{ + // Savitzky-Golay Quadratic, 5-point Coefficients + xt::xarray filter = {-3.0, 12.0, 17.0, 12.0, -3.0}; + filter /= 35.0; + + const unsigned int num_sequences = control_sequence.vx.shape(0); + + // Too short to smooth meaningfully + if (num_sequences < 10) { + return; + } + + auto applyFilter = [&](const xt::xarray & data) -> float { + return xt::sum(data * filter, {0}, immediate)(); + }; + + auto applyFilterOverAxis = + [&](xt::xtensor & sequence, const float hist_0, const float hist_1) -> void + { + unsigned int idx = 0; + sequence(idx) = applyFilter({ + hist_0, + hist_1, + sequence(idx), + sequence(idx + 1), + sequence(idx + 2)}); + + idx++; + sequence(idx) = applyFilter({ + hist_1, + sequence(idx - 1), + sequence(idx), + sequence(idx + 1), + sequence(idx + 2)}); + + for (idx = 2; idx != num_sequences - 3; idx++) { + sequence(idx) = applyFilter({ + sequence(idx - 2), + sequence(idx - 1), + sequence(idx), + sequence(idx + 1), + sequence(idx + 2)}); + } + + idx++; + sequence(idx) = applyFilter({ + sequence(idx - 2), + sequence(idx - 1), + sequence(idx), + sequence(idx + 1), + sequence(idx + 1)}); + + idx++; + sequence(idx) = applyFilter({ + sequence(idx - 2), + sequence(idx - 1), + sequence(idx), + sequence(idx), + sequence(idx)}); + }; + + // Filter trajectories + applyFilterOverAxis(control_sequence.vx, control_history[0].vx, control_history[1].vx); + applyFilterOverAxis(control_sequence.vy, control_history[0].vy, control_history[1].vy); + applyFilterOverAxis(control_sequence.wz, control_history[0].wz, control_history[1].wz); + + // Update control history + unsigned int offset = settings.shift_control_sequence ? 1 : 0; + control_history[0] = control_history[1]; + control_history[1] = { + control_sequence.vx(offset), + control_sequence.vy(offset), + control_sequence.wz(offset)}; +} + } // namespace mppi::utils -// + #endif // MPPIC__UTILS_HPP_ diff --git a/src/optimizer.cpp b/src/optimizer.cpp index 14ed7680..2201e85c 100644 --- a/src/optimizer.cpp +++ b/src/optimizer.cpp @@ -104,6 +104,8 @@ void Optimizer::reset() { state_.reset(settings_.batch_size, settings_.time_steps); control_sequence_.reset(settings_.time_steps); + control_history_[0] = {0.0, 0.0, 0.0}; + control_history_[1] = {0.0, 0.0, 0.0}; costs_ = xt::zeros({settings_.batch_size}); generated_trajectories_.reset(settings_.batch_size, settings_.time_steps); @@ -123,6 +125,7 @@ geometry_msgs::msg::TwistStamped Optimizer::evalControl( optimize(); } while (fallback(critics_data_.fail_flag)); + utils::savitskyGolayFilter(control_sequence_, control_history_, settings_); auto control = getControlFromSequenceAsTwist(plan.header.stamp); if (settings_.shift_control_sequence) { From f8509d5e2a771bd0b4c331fb7c1b99f632bc5fa8 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Tue, 29 Nov 2022 14:40:43 -0800 Subject: [PATCH 18/53] lint + doxygen --- benchmark/controller_benchmark.cpp | 1 - benchmark/optimizer_benchmark.cpp | 1 - include/mppic/controller.hpp | 44 +++++++ include/mppic/critic_data.hpp | 13 +- include/mppic/critic_function.hpp | 30 +++++ include/mppic/critic_manager.hpp | 29 +++++ include/mppic/critics/constraint_critic.hpp | 7 + include/mppic/critics/goal_angle_critic.hpp | 7 + include/mppic/critics/goal_critic.hpp | 7 + include/mppic/critics/obstacles_critic.hpp | 37 +++++- include/mppic/critics/path_align_critic.hpp | 7 + include/mppic/critics/path_angle_critic.hpp | 9 ++ include/mppic/critics/path_follow_critic.hpp | 14 +- .../mppic/critics/prefer_forward_critic.hpp | 13 ++ include/mppic/critics/twirling_critic.hpp | 13 ++ include/mppic/models/constraints.hpp | 8 ++ include/mppic/models/control_sequence.hpp | 8 ++ include/mppic/models/optimizer_settings.hpp | 11 +- include/mppic/models/path.hpp | 16 ++- include/mppic/models/state.hpp | 10 +- include/mppic/models/trajectories.hpp | 20 ++- include/mppic/motion_models.hpp | 70 +++++++++- include/mppic/optimizer.hpp | 121 ++++++++++++++++-- include/mppic/tools/noise_generator.hpp | 13 +- include/mppic/tools/parameters_handler.hpp | 73 ++++++++++- include/mppic/tools/path_handler.hpp | 69 +++++++++- include/mppic/tools/trajectory_visualizer.hpp | 50 +++++++- include/mppic/tools/utils.hpp | 81 ++++++++++-- src/critic_manager.cpp | 4 +- src/critics/constraint_critic.cpp | 12 +- src/critics/path_align_critic.cpp | 3 +- src/critics/path_angle_critic.cpp | 2 +- src/critics/twirling_critic.cpp | 1 - src/optimizer.cpp | 8 +- test/utils/models.hpp | 1 + test/utils/utils.hpp | 8 +- 36 files changed, 750 insertions(+), 71 deletions(-) diff --git a/benchmark/controller_benchmark.cpp b/benchmark/controller_benchmark.cpp index bdfdcaf6..17d749e9 100644 --- a/benchmark/controller_benchmark.cpp +++ b/benchmark/controller_benchmark.cpp @@ -34,7 +34,6 @@ void prepareAndRunBenchmark( bool consider_footprint, std::string motion_model, std::vector critics, benchmark::State & state) { - bool visualize = false; int batch_size = 300; diff --git a/benchmark/optimizer_benchmark.cpp b/benchmark/optimizer_benchmark.cpp index 98de2cde..9cda8d68 100644 --- a/benchmark/optimizer_benchmark.cpp +++ b/benchmark/optimizer_benchmark.cpp @@ -79,7 +79,6 @@ void prepareAndRunBenchmark( for (auto _ : state) { optimizer->evalControl(pose, velocity, path, dummy_goal_checker); } - } static void BM_DiffDrivePointFootprint(benchmark::State & state) diff --git a/include/mppic/controller.hpp b/include/mppic/controller.hpp index ca42b62d..22f5a5aa 100644 --- a/include/mppic/controller.hpp +++ b/include/mppic/controller.hpp @@ -18,30 +18,74 @@ namespace mppi { +/** + * @class mppi::Controller + * @brief Main plugin controller for MPPI Controller + */ class Controller : public nav2_core::Controller { public: + /** + * @brief Constructor for mppi::Controller + */ Controller() = default; + /** + * @brief Configure controller on bringup + * @param parent WeakPtr to node + * @param name Name of plugin + * @param tf TF buffer to use + * @param costmap_ros Costmap2DROS object of environment + */ void configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, const std::shared_ptr tf, const std::shared_ptr costmap_ros) override; + /** + * @brief Cleanup resources + */ void cleanup() override; + + /** + * @brief Activate controller + */ void activate() override; + + /** + * @brief Deactivate controller + */ void deactivate() override; + /** + * @brief Main method to compute velocities using the optimizer + * @param robot_pose Robot pose + * @param robot_speed Robot speed + * @param goal_checker Pointer to the goal checker for awareness if completed task + */ geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, nav2_core::GoalChecker * goal_checker) override; + /** + * @brief Set new reference path to track + * @param path Path to track + */ void setPlan(const nav_msgs::msg::Path & path) override; + /** + * @brief Set new speed limit from callback + * @param speed_limit Speed limit to use + * @param percentage Bool if the speed limit is absolute or relative + */ void setSpeedLimit(const double & speed_limit, const bool & percentage) override; protected: + /** + * @brief Visualize trajectories + * @param transformed_plan Transformed input plan + */ void visualize(nav_msgs::msg::Path transformed_plan); std::string name_; diff --git a/include/mppic/critic_data.hpp b/include/mppic/critic_data.hpp index 06341e30..2e38e900 100644 --- a/include/mppic/critic_data.hpp +++ b/include/mppic/critic_data.hpp @@ -1,7 +1,9 @@ // Copyright 2022 @artofnothingness Aleksei Budyakov, Samsung Research -#pragma once +#ifndef MPPIC__CRITIC_DATA_HPP_ +#define MPPIC__CRITIC_DATA_HPP_ +#include #include #include "geometry_msgs/msg/pose_stamped.hpp" @@ -15,6 +17,11 @@ namespace mppi { +/** + * @struct mppi::CriticData + * @brief Data to pass to critics for scoring, including state, trajectories, path, costs, and + * important parameters to share + */ struct CriticData { const models::State & state; @@ -30,4 +37,6 @@ struct CriticData std::optional furthest_reached_path_point; }; -} // namespace mppi::models +} // namespace mppi + +#endif // MPPIC__CRITIC_DATA_HPP_ diff --git a/include/mppic/critic_function.hpp b/include/mppic/critic_function.hpp index 14b45833..3cd31079 100644 --- a/include/mppic/critic_function.hpp +++ b/include/mppic/critic_function.hpp @@ -13,12 +13,32 @@ namespace mppi::critics { + +/** + * @class mppi::critics::CriticFunction + * @brief Abstract critic objective function to score trajectories + */ class CriticFunction { public: + /** + * @brief Constructor for mppi::critics::CriticFunction + */ CriticFunction() = default; + + /** + * @brief Destructor for mppi::critics::CriticFunction + */ virtual ~CriticFunction() = default; + /** + * @brief Configure critic on bringup + * @param parent WeakPtr to node + * @param parent_name name of the controller + * @param name Name of plugin + * @param costmap_ros Costmap2DROS object of environment + * @param dynamic_parameter_handler Parameter handler object + */ void on_configure( rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & parent_name, @@ -40,10 +60,20 @@ class CriticFunction initialize(); } + /** + * @brief Main function to score trajectory + * @param data Critic data to use in scoring + */ virtual void score(CriticData & data) = 0; + /** + * @brief Initialize critic + */ virtual void initialize() = 0; + /** + * @brief Get name of critic + */ std::string getName() { return name_; diff --git a/include/mppic/critic_manager.hpp b/include/mppic/critic_manager.hpp index ed8b5d67..7040e4b6 100644 --- a/include/mppic/critic_manager.hpp +++ b/include/mppic/critic_manager.hpp @@ -23,20 +23,49 @@ namespace mppi { +/** + * @class mppi::CriticManager + * @brief Manager of objective function plugins for scoring trajectories + */ class CriticManager { public: + /** + * @brief Constructor for mppi::CriticManager + */ CriticManager() = default; + /** + * @brief Configure critic manager on bringup and load plugins + * @param parent WeakPtr to node + * @param name Name of plugin + * @param costmap_ros Costmap2DROS object of environment + * @param dynamic_parameter_handler Parameter handler object + */ void on_configure( rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name, std::shared_ptr, ParametersHandler *); + /** + * @brief Score trajectories by the set of loaded critic functions + * @param CriticData Struct of necessary information to pass to the critic functions + */ void evalTrajectoriesScores(CriticData & data) const; protected: + /** + * @brief Get parameters (critics to load) + */ void getParams(); + + /** + * @brief Load the critic plugins + */ void loadCritics(); + + /** + * @brief Get full-name namespaced critic IDs + */ std::string getFullName(const std::string & name); protected: diff --git a/include/mppic/critics/constraint_critic.hpp b/include/mppic/critics/constraint_critic.hpp index ba97de48..f3874332 100644 --- a/include/mppic/critics/constraint_critic.hpp +++ b/include/mppic/critics/constraint_critic.hpp @@ -8,9 +8,16 @@ namespace mppi::critics { +/** + * @class mppi::critics::ConstraintCritic + * @brief Critic objective function for enforcing feasible constraints + */ class ConstraintCritic : public CriticFunction { public: + /** + * @brief Initialize critic + */ void initialize() override; /** diff --git a/include/mppic/critics/goal_angle_critic.hpp b/include/mppic/critics/goal_angle_critic.hpp index 34927dd2..da3bf704 100644 --- a/include/mppic/critics/goal_angle_critic.hpp +++ b/include/mppic/critics/goal_angle_critic.hpp @@ -8,9 +8,16 @@ namespace mppi::critics { +/** + * @class mppi::critics::ConstraintCritic + * @brief Critic objective function for driving towards goal orientation + */ class GoalAngleCritic : public CriticFunction { public: + /** + * @brief Initialize critic + */ void initialize() override; /** diff --git a/include/mppic/critics/goal_critic.hpp b/include/mppic/critics/goal_critic.hpp index 4c7c38a3..70cc4037 100644 --- a/include/mppic/critics/goal_critic.hpp +++ b/include/mppic/critics/goal_critic.hpp @@ -8,9 +8,16 @@ namespace mppi::critics { +/** + * @class mppi::critics::ConstraintCritic + * @brief Critic objective function for driving towards goal + */ class GoalCritic : public CriticFunction { public: + /** + * @brief Initialize critic + */ void initialize() override; /** diff --git a/include/mppic/critics/obstacles_critic.hpp b/include/mppic/critics/obstacles_critic.hpp index 8431a79b..c9f5a5fa 100644 --- a/include/mppic/critics/obstacles_critic.hpp +++ b/include/mppic/critics/obstacles_critic.hpp @@ -1,6 +1,6 @@ // Copyright 2022 FastSense, Samsung Research #pragma once - +#include #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" @@ -11,15 +11,26 @@ namespace mppi::critics { +/** + * @class mppi::critics::CollisionCost + * @brief Utility for storing cost information + */ struct CollisionCost { float cost; bool using_footprint; }; +/** + * @class mppi::critics::ConstraintCritic + * @brief Critic objective function for avoiding obstacles + */ class ObstaclesCritic : public CriticFunction { public: + /** + * @brief Initialize critic + */ void initialize() override; /** @@ -30,9 +41,33 @@ class ObstaclesCritic : public CriticFunction void score(CriticData & data) override; protected: + /** + * @brief Checks if cost represents a collision + * @param cost Costmap cost + * @return bool if in collision + */ bool inCollision(float cost) const; + + /** + * @brief Get max useful cost + * @return unsigned char Max cost + */ unsigned char maxCost(); + + /** + * @brief cost at a robot pose + * @param x X of pose + * @param y Y of pose + * @param theta theta of pose + * @return Collision information at pose + */ CollisionCost costAtPose(float x, float y, float theta); + + /** + * @brief Distance to obstacle from cost + * @param cost Costmap cost + * @return float Distance to the obstacle represented by cost + */ float distanceToObstacle(const CollisionCost & cost); /** diff --git a/include/mppic/critics/path_align_critic.hpp b/include/mppic/critics/path_align_critic.hpp index cee000c1..58d76b67 100644 --- a/include/mppic/critics/path_align_critic.hpp +++ b/include/mppic/critics/path_align_critic.hpp @@ -8,9 +8,16 @@ namespace mppi::critics { +/** + * @class mppi::critics::ConstraintCritic + * @brief Critic objective function for aligning to the path + */ class PathAlignCritic : public CriticFunction { public: + /** + * @brief Initialize critic + */ void initialize() override; /** diff --git a/include/mppic/critics/path_angle_critic.hpp b/include/mppic/critics/path_angle_critic.hpp index 794a45cb..a2c2e7b3 100644 --- a/include/mppic/critics/path_angle_critic.hpp +++ b/include/mppic/critics/path_angle_critic.hpp @@ -8,14 +8,23 @@ namespace mppi::critics { +/** + * @class mppi::critics::ConstraintCritic + * @brief Critic objective function for aligning to path in cases of extreme misalignment + */ class PathAngleCritic : public CriticFunction { public: + /** + * @brief Initialize critic + */ void initialize() override; /** * @brief Evaluate cost related to robot orientation at goal pose * (considered only if robot near last goal in current plan) + * + * @param costs [out] add goal angle cost values to this tensor */ void score(CriticData & data) override; diff --git a/include/mppic/critics/path_follow_critic.hpp b/include/mppic/critics/path_follow_critic.hpp index e0cbd851..afac54ae 100644 --- a/include/mppic/critics/path_follow_critic.hpp +++ b/include/mppic/critics/path_follow_critic.hpp @@ -8,11 +8,24 @@ namespace mppi::critics { +/** + * @class mppi::critics::ConstraintCritic + * @brief Critic objective function for following the path + */ class PathFollowCritic : public CriticFunction { public: + /** + * @brief Initialize critic + */ void initialize() override; + /** + * @brief Evaluate cost related to robot orientation at goal pose + * (considered only if robot near last goal in current plan) + * + * @param costs [out] add goal angle cost values to this tensor + */ void score(CriticData & data) override; protected: @@ -21,7 +34,6 @@ class PathFollowCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; - }; } // namespace mppi::critics diff --git a/include/mppic/critics/prefer_forward_critic.hpp b/include/mppic/critics/prefer_forward_critic.hpp index bf7b9f11..8f67d051 100644 --- a/include/mppic/critics/prefer_forward_critic.hpp +++ b/include/mppic/critics/prefer_forward_critic.hpp @@ -8,11 +8,24 @@ namespace mppi::critics { +/** + * @class mppi::critics::ConstraintCritic + * @brief Critic objective function for preferring forward motion + */ class PreferForwardCritic : public CriticFunction { public: + /** + * @brief Initialize critic + */ void initialize() override; + /** + * @brief Evaluate cost related to robot orientation at goal pose + * (considered only if robot near last goal in current plan) + * + * @param costs [out] add goal angle cost values to this tensor + */ void score(CriticData & data) override; protected: diff --git a/include/mppic/critics/twirling_critic.hpp b/include/mppic/critics/twirling_critic.hpp index 17decc74..5768a0ef 100644 --- a/include/mppic/critics/twirling_critic.hpp +++ b/include/mppic/critics/twirling_critic.hpp @@ -7,11 +7,24 @@ namespace mppi::critics { +/** + * @class mppi::critics::ConstraintCritic + * @brief Critic objective function for penalizing wiggling/twirling + */ class TwirlingCritic : public CriticFunction { public: + /** + * @brief Initialize critic + */ void initialize() override; + /** + * @brief Evaluate cost related to robot orientation at goal pose + * (considered only if robot near last goal in current plan) + * + * @param costs [out] add goal angle cost values to this tensor + */ void score(CriticData & data) override; protected: diff --git a/include/mppic/models/constraints.hpp b/include/mppic/models/constraints.hpp index 0d3b6b20..8d9832e1 100644 --- a/include/mppic/models/constraints.hpp +++ b/include/mppic/models/constraints.hpp @@ -5,6 +5,10 @@ namespace mppi::models { +/** + * @struct mppi::models::ControlConstraints + * @brief Constraints on control + */ struct ControlConstraints { double vx_max; @@ -13,6 +17,10 @@ struct ControlConstraints double wz; }; +/** + * @struct mppi::models::SamplingStd + * @brief Noise parameters for sampling trajectories + */ struct SamplingStd { double vx; diff --git a/include/mppic/models/control_sequence.hpp b/include/mppic/models/control_sequence.hpp index a198bc27..17a60e15 100644 --- a/include/mppic/models/control_sequence.hpp +++ b/include/mppic/models/control_sequence.hpp @@ -7,11 +7,19 @@ namespace mppi::models { +/** + * @struct mppi::models::Control + * @brief A set of controls + */ struct Control { float vx, vy, wz; }; +/** + * @struct mppi::models::ControlSequence + * @brief A control sequence over time (e.g. trajectory) + */ struct ControlSequence { xt::xtensor vx; diff --git a/include/mppic/models/optimizer_settings.hpp b/include/mppic/models/optimizer_settings.hpp index bbd4636b..d14711ae 100644 --- a/include/mppic/models/optimizer_settings.hpp +++ b/include/mppic/models/optimizer_settings.hpp @@ -1,12 +1,17 @@ // Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research -#pragma once +#ifndef MPPIC__MODELS__OPTIMIZER_SETTINGS_HPP_ +#define MPPIC__MODELS__OPTIMIZER_SETTINGS_HPP_ -#include "mppic/models/constraints.hpp" #include +#include "mppic/models/constraints.hpp" namespace mppi::models { +/** + * @struct mppi::models::OptimizerSettings + * @brief Settings for the optimizer to use + */ struct OptimizerSettings { models::ControlConstraints base_constraints{0, 0, 0, 0}; @@ -23,3 +28,5 @@ struct OptimizerSettings }; } // namespace mppi::models + +#endif // MPPIC__MODELS__OPTIMIZER_SETTINGS_HPP_ diff --git a/include/mppic/models/path.hpp b/include/mppic/models/path.hpp index edbcbf86..b7ee50b6 100644 --- a/include/mppic/models/path.hpp +++ b/include/mppic/models/path.hpp @@ -1,25 +1,33 @@ // Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research -#pragma once +#ifndef MPPIC__MODELS__PATH_HPP_ +#define MPPIC__MODELS__PATH_HPP_ #include namespace mppi::models { +/** + * @struct mppi::models::Path + * @brief Path represented as a tensor + */ struct Path { xt::xtensor x; xt::xtensor y; xt::xtensor yaws; + /** + * @brief Reset path data + */ void reset(unsigned int size) { x = xt::zeros({size}); y = xt::zeros({size}); yaws = xt::zeros({size}); } - }; -} -// namespace mppi::models +} // namespace mppi::models + +#endif // MPPIC__MODELS__PATH_HPP_ diff --git a/include/mppic/models/state.hpp b/include/mppic/models/state.hpp index 32719d50..4ad74486 100644 --- a/include/mppic/models/state.hpp +++ b/include/mppic/models/state.hpp @@ -10,6 +10,10 @@ namespace mppi::models { +/** + * @struct mppi::models::State + * @brief State information: velocities, controls, poses, speed + */ struct State { xt::xtensor vx; @@ -23,6 +27,9 @@ struct State geometry_msgs::msg::PoseStamped pose; geometry_msgs::msg::Twist speed; + /** + * @brief Reset state data + */ void reset(unsigned int batch_size, unsigned int time_steps) { vx = xt::zeros({batch_size, time_steps}); @@ -34,7 +41,6 @@ struct State cwz = xt::zeros({batch_size, time_steps}); } }; - -} +} // namespace mppi::models #endif // MPPIC__MODELS__STATE_HPP_ diff --git a/include/mppic/models/trajectories.hpp b/include/mppic/models/trajectories.hpp index 1e7145cb..7cf9bb8c 100644 --- a/include/mppic/models/trajectories.hpp +++ b/include/mppic/models/trajectories.hpp @@ -1,5 +1,6 @@ // Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research -#pragma once +#ifndef MPPIC__MODELS__TRAJECTORIES_HPP_ +#define MPPIC__MODELS__TRAJECTORIES_HPP_ #include #include @@ -7,12 +8,19 @@ namespace mppi::models { +/** + * @class mppi::models::Trajectories + * @brief Candidate Trajectories + */ struct Trajectories { xt::xtensor x; xt::xtensor y; xt::xtensor yaws; + /** + * @brief Reset state data + */ void reset(unsigned int batch_size, unsigned int time_steps) { x = xt::zeros({batch_size, time_steps}); @@ -20,6 +28,10 @@ struct Trajectories yaws = xt::zeros({batch_size, time_steps}); } + /** + * @brief Get the last point in all trajectories + * @return A tensor containing only final points in trajectories + */ inline auto getLastPoints() const { return xt::concatenate( @@ -27,8 +39,8 @@ struct Trajectories xt::view(x, xt::all(), -1, xt::newaxis()), xt::view(y, xt::all(), -1, xt::newaxis())), 1); } - }; -} -// namespace mppi::models +} // namespace mppi::models + +#endif // MPPIC__MODELS__TRAJECTORIES_HPP_ diff --git a/include/mppic/motion_models.hpp b/include/mppic/motion_models.hpp index 37f3b571..75226ebe 100644 --- a/include/mppic/motion_models.hpp +++ b/include/mppic/motion_models.hpp @@ -17,12 +17,27 @@ namespace mppi { +/** + * @class mppi::MotionModel + * @brief Abstract motion model for modeling a vehicle + */ class MotionModel { public: + /** + * @brief Constructor for mppi::MotionModel + */ MotionModel() = default; + + /** + * @brief Destructor for mppi::MotionModel + */ virtual ~MotionModel() = default; + /** + * @brief With input velocities, find the vehicle's output velocities + * @param state Contains control velocities to use to populate vehicle velocities + */ virtual void predict(models::State & state) { using namespace xt::placeholders; // NOLINT @@ -38,24 +53,48 @@ class MotionModel } } + /** + * @brief Whether the motion model is holonomic, using Y axis + * @return Bool If holonomic + */ virtual bool isHolonomic() = 0; - virtual void applyConstraints(models::ControlSequence & /*state*/) {} + + /** + * @brief Apply hard vehicle constraints to a control sequence + * @param control_sequence Control sequence to apply constraints to + */ + virtual void applyConstraints(models::ControlSequence & /*control_sequence*/) {} }; +/** + * @class mppi::AckermannMotionModel + * @brief Ackermann motion model + */ class AckermannMotionModel : public MotionModel { public: + /** + * @brief Constructor for mppi::AckermannMotionModel + */ explicit AckermannMotionModel(ParametersHandler * param_handler) { auto getParam = param_handler->getParamGetter("AckermannConstraints"); getParam(min_turning_r_, "min_turning_r", 0.2); } + /** + * @brief Whether the motion model is holonomic, using Y axis + * @return Bool If holonomic + */ bool isHolonomic() override { return false; } + /** + * @brief Apply hard vehicle constraints to a control sequence + * @param control_sequence Control sequence to apply constraints to + */ void applyConstraints(models::ControlSequence & control_sequence) override { auto & vx = control_sequence.vx; @@ -65,35 +104,60 @@ class AckermannMotionModel : public MotionModel view = xt::sign(vx) / min_turning_r_; } + /** + * @brief Get minimum turning radius of ackermann drive + * @return Minimum turning radius + */ float getMinTurningRadius() {return min_turning_r_;} private: float min_turning_r_{0}; }; +/** + * @class mppi::DiffDriveMotionModel + * @brief Differential drive motion model + */ class DiffDriveMotionModel : public MotionModel { public: + /** + * @brief Constructor for mppi::DiffDriveMotionModel + */ DiffDriveMotionModel() = default; + /** + * @brief Whether the motion model is holonomic, using Y axis + * @return Bool If holonomic + */ bool isHolonomic() override { return false; } }; +/** + * @class mppi::OmniMotionModel + * @brief Omnidirectional motion model + */ class OmniMotionModel : public MotionModel { public: + /** + * @brief Constructor for mppi::OmniMotionModel + */ OmniMotionModel() = default; + /** + * @brief Whether the motion model is holonomic, using Y axis + * @return Bool If holonomic + */ bool isHolonomic() override { return true; } - }; -} // namespace mppi::models +} // namespace mppi #endif // MPPIC__MOTION_MODELS_HPP_ diff --git a/include/mppic/optimizer.hpp b/include/mppic/optimizer.hpp index 3ddd75f4..bb4401dd 100644 --- a/include/mppic/optimizer.hpp +++ b/include/mppic/optimizer.hpp @@ -32,90 +32,191 @@ namespace mppi { +/** + * @class mppi::Optimizer + * @brief Main algorithm optimizer of the MPPI Controller + */ class Optimizer { public: + /** + * @brief Constructor for mppi::Optimizer + */ Optimizer() = default; + /** + * @brief Destructor for mppi::Optimizer + */ ~Optimizer() {shutdown();} + + /** + * @brief Initializes optimizer on startup + * @param parent WeakPtr to node + * @param name Name of plugin + * @param costmap_ros Costmap2DROS object of environment + * @param dynamic_parameter_handler Parameter handler object + */ void initialize( rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name, std::shared_ptr costmap_ros, ParametersHandler * dynamic_parameters_handler); + /** + * @brief Shutdown for optimizer at process end + */ void shutdown(); + /** + * @brief Compute control using MPPI algorithm + * @param robot_pose Pose of the robot at given time + * @param robot_speed Speed of the robot at given time + * @param plan Path plan to track + * @param goal_checker Object to check if goal is completed + * @return TwistStamped of the MPPI control + */ 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); + /** + * @brief Get the trajectories generated in a cycle for visualization + * @return Set of trajectories evaluated in cycle + */ models::Trajectories & getGeneratedTrajectories(); + + /** + * @brief Get the optimal trajectory for a cycle for visualization + * @return Optimal trajectory + */ xt::xtensor getOptimizedTrajectory(); + /** + * @brief Set the maximum speed based on the speed limits callback + * @param speed_limit Limit of the speed for use + * @param percentage Whether the speed limit is absolute or relative + */ void setSpeedLimit(double speed_limit, bool percentage); protected: + /** + * @brief Main function to generate, score, and return trajectories + */ void optimize(); + /** + * @brief Prepare state information on new request for trajectory rollouts + * @param robot_pose Pose of the robot at given time + * @param robot_speed Speed of the robot at given time + * @param plan Path plan to track + * @param goal_checker Object to check if goal is completed + */ 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); + /** + * @brief Obtain the main controller's parameters + */ void getParams(); + + /** + * @brief Reset the optimization problem to initial conditions + */ void reset(); + /** + * @brief Set the motion model of the vehicle platform + * @param model Model string to use + */ void setMotionModel(const std::string & model); + /** + * @brief Shift the optimal control sequence after processing for + * next iterations initial conditions after execution + */ void shiftControlSequence(); /** - * - * @brief updates generated_trajectories_ + * @brief updates generated trajectories with noised trajectories + * from the last cycle's optimal control */ void generateNoisedTrajectories(); + /** + * @brief Apply hard vehicle constraints on control sequence + */ void applyControlSequenceConstraints(); /** - * @brief Update velocities in state_ - * - * @param twist current robot speed - * @param state[out] fill state with velocities on each step + * @brief Update velocities in state + * @param state fill state with velocities on each step */ void updateStateVelocities(models::State & state) const; + /** + * @brief Update initial velocity in state + * @param state fill state + */ void updateInitialStateVelocities(models::State & state) const; /** - * @brief predict velocities in state_ using model - * for time horizont equal to time_steps_ + * @brief predict velocities in state using model + * for time horizon equal to timesteps + * @param state fill state */ void propagateStateVelocitiesFromInitials(models::State & state) const; + /** + * @brief Rollout velocities in state to poses + * @param trajectories to rollout + * @param state fill state + */ void integrateStateVelocities( models::Trajectories & trajectories, const models::State & state) const; + /** + * @brief Rollout velocities in state to poses + * @param trajectories to rollout + * @param state fill state + */ void integrateStateVelocities( xt::xtensor & trajectories, const xt::xtensor & state) const; /** - * @brief Update control_sequence_ with state controls weighted by costs + * @brief Update control sequence with state controls weighted by costs * using softmax function */ void updateControlSequence(); + /** + * @brief Convert control sequence to a twist commant + * @param stamp Timestamp to use + * @return TwistStamped of command to send to robot base + */ geometry_msgs::msg::TwistStamped getControlFromSequenceAsTwist(const builtin_interfaces::msg::Time & stamp); + /** + * @brief Whether the motion model is holonomic + * @return Bool if holonomic to populate `y` axis of state + */ inline bool isHolonomic() const; + /** + * @brief Using control frequence and time step size, determine if trajectory + * offset should be used to populate initial state of the next cycle + */ void setOffset(double controller_frequency); + /** + * @brief Perform fallback behavior to try to recover from a set of trajectories in collision + * @param fail Whether the system failed to recover from + */ bool fallback(bool fail); protected: @@ -141,7 +242,7 @@ class Optimizer CriticData critics_data_ = {state_, generated_trajectories_, path_, costs_, settings_.model_dt, false, nullptr, nullptr, - std::nullopt}; /// Caution, keep references + std::nullopt}; /// Caution, keep references rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; }; diff --git a/include/mppic/tools/noise_generator.hpp b/include/mppic/tools/noise_generator.hpp index 5348b6ad..1abd3fe0 100644 --- a/include/mppic/tools/noise_generator.hpp +++ b/include/mppic/tools/noise_generator.hpp @@ -1,7 +1,7 @@ // Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research -#ifndef MPPIC__NOISE_GENERATOR_HPP_ -#define MPPIC__NOISE_GENERATOR_HPP_ +#ifndef MPPIC__TOOLS__NOISE_GENERATOR_HPP_ +#define MPPIC__TOOLS__NOISE_GENERATOR_HPP_ #include #include @@ -19,9 +19,16 @@ namespace mppi { +/** + * @class mppi::NoiseGenerator + * @brief Generates noise trajectories from optimal trajectory + */ class NoiseGenerator { public: + /** + * @brief Constructor for mppi::NoiseGenerator + */ NoiseGenerator() = default; /** @@ -85,4 +92,4 @@ class NoiseGenerator } // namespace mppi -#endif // MPPIC__NOISE_GENERATOR_HPP_ +#endif // MPPIC__TOOLS__NOISE_GENERATOR_HPP_ diff --git a/include/mppic/tools/parameters_handler.hpp b/include/mppic/tools/parameters_handler.hpp index 38023a37..a2a8c98e 100644 --- a/include/mppic/tools/parameters_handler.hpp +++ b/include/mppic/tools/parameters_handler.hpp @@ -1,6 +1,7 @@ // Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research -#pragma once +#ifndef MPPIC__TOOLS__PARAMETERS_HANDLER_HPP_ +#define MPPIC__TOOLS__PARAMETERS_HANDLER_HPP_ #include #include @@ -16,8 +17,15 @@ namespace mppi { +/** + * @class Parameter Type enum + */ enum class ParameterType { Dynamic, Static }; +/** + * @class mppi::ParametersHandler + * @brief Handles getting parameters and dynamic parmaeter changes + */ class ParametersHandler { public: @@ -25,43 +33,104 @@ class ParametersHandler using post_callback_t = void (); using pre_callback_t = void (); + /** + * @brief Constructor for mppi::ParametersHandler + */ ParametersHandler() = default; + + /** + * @brief Constructor for mppi::ParametersHandler + * @param parent Weak ptr to node + */ explicit ParametersHandler( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent); + /** + * @brief Starts processing dynamic parameter changes + */ void start(); + /** + * @brief Dynamic parameter callback + * @param parameter Parameter changes to process + * @return Set Parameter Result + */ rcl_interfaces::msg::SetParametersResult dynamicParamsCallback( std::vector parameters); + /** + * @brief Get an object to retreive parameters + * @param ns Namespace to get parameters within + * @return Parameter getter object + */ inline auto getParamGetter(const std::string & ns); + /** + * @brief Gets parameter + * @param setting Return Parameter type + * @param name Parameter name + * @param default_value Default parameter value + * @param param_type Type of parameter (dynamic or static) + */ template void getParam( SettingT & setting, const std::string & name, ParamT default_value, ParameterType param_type = ParameterType::Dynamic); + /** + * @brief Set a callback to process after parameter changes + * @param callback Callback function + */ template void addPostCallback(T && callback); + /** + * @brief Set a callback to process before parameter changes + * @param callback Callback function + */ template void addPreCallback(T && callback); + /** + * @brief Set a parameter + * @param setting Return Parameter type + * @param name Parameter name + * @param node Node to set parameter via + */ template void setParam(SettingT & setting, const std::string & name, NodeT node) const; + /** + * @brief Set a parameter to a dynamic parameter callback + * @param setting Parameter + * @param name Name of parameter + */ template void setDynamicParamCallback(T & setting, const std::string & name); + /** + * @brief Get mutex lock for changing parameters + * @return Pointer to mutex + */ std::mutex * getLock() { return ¶meters_change_mutex_; } protected: + /** + * @brief Set a parameter to a dynamic parameter callback + * @param name Name of parameter + * @param callback Parameter callback + */ template void addDynamicParamCallback(const std::string & name, T && callback); + /** + * @brief Converts parameter type to real types + * @param parameter Parameter to convert into real type + * @return parameter as a functional type + */ template static auto as(const rclcpp::Parameter & parameter); @@ -181,3 +250,5 @@ auto ParametersHandler::as(const rclcpp::Parameter & parameter) } } // namespace mppi + +#endif // MPPIC__TOOLS__PARAMETERS_HANDLER_HPP_ diff --git a/include/mppic/tools/path_handler.hpp b/include/mppic/tools/path_handler.hpp index cd965769..ddc0c476 100644 --- a/include/mppic/tools/path_handler.hpp +++ b/include/mppic/tools/path_handler.hpp @@ -1,5 +1,6 @@ // Copyright 2022 FastSense, Samsung Research -#pragma once +#ifndef MPPIC__TOOLS__PATH_HANDLER_HPP_ +#define MPPIC__TOOLS__PATH_HANDLER_HPP_ #include #include @@ -22,46 +23,106 @@ namespace mppi using PathIterator = std::vector::iterator; using PathRange = std::pair; +/** + * @class mppi::PathHandler + * @brief Manager of incoming reference paths for transformation and processing + */ + class PathHandler { public: + /** + * @brief Constructor for mppi::PathHandler + */ PathHandler() = default; + + /** + * @brief Destructor for mppi::PathHandler + */ ~PathHandler() = default; + /** + * @brief Initialize path handler on bringup + * @param parent WeakPtr to node + * @param name Name of plugin + * @param costmap_ros Costmap2DROS object of environment + * @param tf TF buffer for transformations + * @param dynamic_parameter_handler Parameter handler object + */ void initialize( rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name, std::shared_ptr, std::shared_ptr, ParametersHandler *); + /** + * @brief Set new reference path + * @param Plan Path to use + */ void setPath(const nav_msgs::msg::Path & plan); + /** + * @brief Get reference path + * @return Path + */ nav_msgs::msg::Path & getPath(); /** * @brief transform global plan to local applying constraints, - * then prune global_plan_ - * + * then prune global plan + * @param robot_pose Pose of robot * @return global plan in local frame */ nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose); protected: + /** + * @brief Transform a pose to another frame + * @param frame Frame to transform to + * @param in_pose Input pose + * @param out_pose Output pose + * @return Bool if successful + */ bool transformPose( const std::string & frame, const geometry_msgs::msg::PoseStamped & in_pose, geometry_msgs::msg::PoseStamped & out_pose) const; + /** + * @brief Get largest dimension of costmap (radially) + * @return Max distance from center of costmap to edge + */ double getMaxCostmapDist(); + /** + * @brief Transform a pose to the global reference frame + * @param pose Current pose + * @return output poose in global reference frame + */ geometry_msgs::msg::PoseStamped transformToGlobalPlanFrame(const geometry_msgs::msg::PoseStamped & pose); + /** + * @brief Transform a plan to the costmap reference frame + * @param begin Start of path to transform + * @param end End of path to transform + * @param stamp Timestamp to use for transformation + * @return output path in costmap reference frame + */ nav_msgs::msg::Path transformPlanPosesToCostmapFrame( PathIterator begin, PathIterator end, const builtin_interfaces::msg::Time & stamp); + /** + * @brief Get global plan within window of the local costmap size + * @param global_pose Robot pose + * @return Range of path iterators belonging to the path within costmap window + */ PathRange getGlobalPlanConsideringBounds(const geometry_msgs::msg::PoseStamped & global_pose); + /** + * @brief Prune global path to only interesting portions + * @param end Final path iterator + */ void pruneGlobalPlan(const PathIterator end); std::string name_; @@ -77,3 +138,5 @@ class PathHandler double transform_tolerance_{0}; }; } // namespace mppi + +#endif // MPPIC__TOOLS__PATH_HANDLER_HPP_ diff --git a/include/mppic/tools/trajectory_visualizer.hpp b/include/mppic/tools/trajectory_visualizer.hpp index d6263fab..3af740e9 100644 --- a/include/mppic/tools/trajectory_visualizer.hpp +++ b/include/mppic/tools/trajectory_visualizer.hpp @@ -1,6 +1,6 @@ // Copyright 2022 FastSense, Samsung Research -#ifndef MPPIC__TRAJECTORY_VISUALIZER_HPP_ -#define MPPIC__TRAJECTORY_VISUALIZER_HPP_ +#ifndef MPPIC__TOOLS__TRAJECTORY_VISUALIZER_HPP_ +#define MPPIC__TOOLS__TRAJECTORY_VISUALIZER_HPP_ #include #include @@ -18,21 +18,65 @@ namespace mppi { +/** + * @class mppi::TrajectoryVisualizer + * @brief Visualizes trajectories for debugging + */ class TrajectoryVisualizer { public: + /** + * @brief Constructor for mppi::TrajectoryVisualizer + */ TrajectoryVisualizer() = default; + /** + * @brief Configure trajectory visualizer + * @param parent WeakPtr to node + * @param name Name of plugin + * @param frame_id Frame to publish trajectories in + * @param dynamic_parameter_handler Parameter handler object + */ void on_configure( rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name, const std::string & frame_id, ParametersHandler * parameters_handler); + + /** + * @brief Cleanup object on shutdown + */ void on_cleanup(); + + /** + * @brief Activate object + */ void on_activate(); + + /** + * @brief Deactivate object + */ void on_deactivate(); + /** + * @brief Add an optimal trajectory to visualize + * @param trajectory Optimal trajectory + */ void add(const xt::xtensor & trajectory); + + /** + * @brief Add candidate trajectories to visualize + * @param trajectories Candidate trajectories + */ void add(const models::Trajectories & trajectories); + + /** + * @brief Visualize the plan + * @param plan Plan to visualize + */ void visualize(const nav_msgs::msg::Path & plan); + + /** + * @brief Reset object + */ void reset(); protected: @@ -54,4 +98,4 @@ class TrajectoryVisualizer } // namespace mppi -#endif // MPPIC__TRAJECTORY_VISUALIZER_HPP_ +#endif // MPPIC__TOOLS__TRAJECTORY_VISUALIZER_HPP_ diff --git a/include/mppic/tools/utils.hpp b/include/mppic/tools/utils.hpp index 0609d80d..638a1389 100644 --- a/include/mppic/tools/utils.hpp +++ b/include/mppic/tools/utils.hpp @@ -1,11 +1,11 @@ // Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research -#ifndef MPPIC__UTILS_HPP_ -#define MPPIC__UTILS_HPP_ +#ifndef MPPIC__TOOLS__UTILS_HPP_ +#define MPPIC__TOOLS__UTILS_HPP_ #include #include #include - +#include #include #include @@ -36,6 +36,13 @@ namespace mppi::utils { using xt::evaluation_strategy::immediate; +/** + * @brief Convert data into TwistStamped + * @param vx X velocity + * @param wz Angular velocity + * @param stamp Timestamp + * @param frame Reference frame to use + */ inline geometry_msgs::msg::TwistStamped toTwistStamped( float vx, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame) { @@ -48,6 +55,14 @@ inline geometry_msgs::msg::TwistStamped toTwistStamped( return twist; } +/** + * @brief Convert data into TwistStamped + * @param vx X velocity + * @param vy Y velocity + * @param wz Angular velocity + * @param stamp Timestamp + * @param frame Reference frame to use + */ inline geometry_msgs::msg::TwistStamped toTwistStamped( float vx, float vy, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame) @@ -58,6 +73,11 @@ inline geometry_msgs::msg::TwistStamped toTwistStamped( return twist; } +/** + * @brief Convert path to a tensor + * @param path Path to convert + * @return Path tensor + */ inline models::Path toTensor(const nav_msgs::msg::Path & path) { auto result = models::Path{}; @@ -72,6 +92,13 @@ inline models::Path toTensor(const nav_msgs::msg::Path & path) return result; } +/** + * @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 + * @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, @@ -101,6 +128,13 @@ 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 geometry_msgs::msg::Pose & robot, @@ -126,10 +160,10 @@ inline bool withinPositionGoalTolerance( /** * @brief normalize - * * Normalizes the angle to be -M_PI circle to +M_PI circle * It takes and returns radians. - * + * @param angles Angles to normalize + * @return normalized angles */ template auto normalize_angles(const T & angles) @@ -147,7 +181,9 @@ auto normalize_angles(const T & angles) * The result * would always be -pi <= result <= pi. Adding the result * to "from" will always get you an equivelent angle to "to". - * + * @param from Start angle + * @param to End angle + * @return Shortest distance between angles */ template auto shortest_angular_distance( @@ -160,6 +196,8 @@ auto shortest_angular_distance( /** * @brief Evaluate furthest point idx of data.path which is * nearset to some trajectory in data.trajectories + * @param data Data to use + * @return Idx of furthest path point reached by a set of trajectories */ inline size_t findPathFurthestReachedPoint(const CriticData & data) { @@ -190,6 +228,7 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) /** * @brief evaluate path furthest point if it is not set + * @param data Data to use */ inline void setPathFurthestPointIfNotSet(CriticData & data) { @@ -200,6 +239,10 @@ inline void setPathFurthestPointIfNotSet(CriticData & data) /** * @brief evaluate angle from pose (have angle) to point (no angle) + * @param pose pose + * @param point_x Point to find angle relative to X axis + * @param point_y Point to find angle relative to Y axis + * @return Angle between two points */ inline double posePointAngle(const geometry_msgs::msg::Pose & pose, double point_x, double point_y) { @@ -213,6 +256,8 @@ inline double posePointAngle(const geometry_msgs::msg::Pose & pose, double point /** * @brief Evaluate ratio of data.path which reached by all trajectories in data.trajectories + * @param data Data to use + * @return ratio of path reached */ inline float getPathRatioReached(const CriticData & data) { @@ -225,7 +270,12 @@ inline float getPathRatioReached(const CriticData & data) return furthest_reached_path_point / path_points_count; } - +/** + * @brief Apply Savisky-Golay filter to optimal trajectory + * @param control_sequence Sequence to apply filter to + * @param control_history Recent set of controls for edge-case handling + * @param Settings Settings to use + */ inline void savitskyGolayFilter( models::ControlSequence & control_sequence, std::array & control_history, @@ -250,7 +300,8 @@ inline void savitskyGolayFilter( [&](xt::xtensor & sequence, const float hist_0, const float hist_1) -> void { unsigned int idx = 0; - sequence(idx) = applyFilter({ + sequence(idx) = applyFilter( + { hist_0, hist_1, sequence(idx), @@ -258,7 +309,8 @@ inline void savitskyGolayFilter( sequence(idx + 2)}); idx++; - sequence(idx) = applyFilter({ + sequence(idx) = applyFilter( + { hist_1, sequence(idx - 1), sequence(idx), @@ -266,7 +318,8 @@ inline void savitskyGolayFilter( sequence(idx + 2)}); for (idx = 2; idx != num_sequences - 3; idx++) { - sequence(idx) = applyFilter({ + sequence(idx) = applyFilter( + { sequence(idx - 2), sequence(idx - 1), sequence(idx), @@ -275,7 +328,8 @@ inline void savitskyGolayFilter( } idx++; - sequence(idx) = applyFilter({ + sequence(idx) = applyFilter( + { sequence(idx - 2), sequence(idx - 1), sequence(idx), @@ -283,7 +337,8 @@ inline void savitskyGolayFilter( sequence(idx + 1)}); idx++; - sequence(idx) = applyFilter({ + sequence(idx) = applyFilter( + { sequence(idx - 2), sequence(idx - 1), sequence(idx), @@ -307,4 +362,4 @@ inline void savitskyGolayFilter( } // namespace mppi::utils -#endif // MPPIC__UTILS_HPP_ +#endif // MPPIC__TOOLS__UTILS_HPP_ diff --git a/src/critic_manager.cpp b/src/critic_manager.cpp index 617c5697..63c947e5 100644 --- a/src/critic_manager.cpp +++ b/src/critic_manager.cpp @@ -39,7 +39,9 @@ void CriticManager::loadCritics() auto instance = std::unique_ptr( loader_->createUnmanagedInstance(fullname)); critics_.push_back(std::move(instance)); - critics_.back()->on_configure(parent_, name_, name_ + "." + name, costmap_ros_, parameters_handler_); + critics_.back()->on_configure( + parent_, name_, name_ + "." + name, costmap_ros_, + parameters_handler_); RCLCPP_INFO(logger_, "Critic loaded : %s", fullname.c_str()); } } diff --git a/src/critics/constraint_critic.cpp b/src/critics/constraint_critic.cpp index 29b1524f..e4bef0b2 100644 --- a/src/critics/constraint_critic.cpp +++ b/src/critics/constraint_critic.cpp @@ -37,7 +37,7 @@ void ConstraintCritic::score(CriticData & data) auto out_of_max_bounds_motion = xt::maximum(data.state.vx - max_vel_, 0); auto out_of_min_bounds_motion = xt::maximum(min_vel_ - data.state.vx, 0); - auto acker = dynamic_cast(data.motion_model.get()); + auto acker = dynamic_cast(data.motion_model.get()); if (acker != nullptr) { auto & vx = data.state.vx; auto & wz = data.state.wz; @@ -47,16 +47,16 @@ void ConstraintCritic::score(CriticData & data) data.costs += xt::pow( xt::sum( (std::move(out_of_max_bounds_motion) + - std::move(out_of_min_bounds_motion) + - std::move(out_of_turning_rad_motion)) - * data.model_dt, {1}, immediate) * weight_, power_); + std::move(out_of_min_bounds_motion) + + std::move(out_of_turning_rad_motion)) * + data.model_dt, {1}, immediate) * weight_, power_); } data.costs += xt::pow( xt::sum( (std::move(out_of_max_bounds_motion) + - std::move(out_of_min_bounds_motion)) - * data.model_dt, {1}, immediate) * weight_, power_); + std::move(out_of_min_bounds_motion)) * + data.model_dt, {1}, immediate) * weight_, power_); } } // namespace mppi::critics diff --git a/src/critics/path_align_critic.cpp b/src/critics/path_align_critic.cpp index a0c658ef..92e58bb4 100644 --- a/src/critics/path_align_critic.cpp +++ b/src/critics/path_align_critic.cpp @@ -27,7 +27,8 @@ void PathAlignCritic::initialize() void PathAlignCritic::score(CriticData & data) { - if (!enabled_ || utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + if (!enabled_ || + utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) { return; } diff --git a/src/critics/path_angle_critic.cpp b/src/critics/path_angle_critic.cpp index bbaccbec..24112ca5 100644 --- a/src/critics/path_angle_critic.cpp +++ b/src/critics/path_angle_critic.cpp @@ -1,7 +1,7 @@ // Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research #include "mppic/critics/path_angle_critic.hpp" -#include "math.h" +#include namespace mppi::critics { diff --git a/src/critics/twirling_critic.cpp b/src/critics/twirling_critic.cpp index 7ea6c2c2..481de138 100644 --- a/src/critics/twirling_critic.cpp +++ b/src/critics/twirling_critic.cpp @@ -17,7 +17,6 @@ void TwirlingCritic::initialize() void TwirlingCritic::score(CriticData & data) { - using xt::evaluation_strategy::immediate; if (!enabled_) { return; diff --git a/src/optimizer.cpp b/src/optimizer.cpp index 2201e85c..ca317aa3 100644 --- a/src/optimizer.cpp +++ b/src/optimizer.cpp @@ -342,17 +342,17 @@ void Optimizer::updateControlSequence() auto bounded_noises_wz = state_.cwz - control_sequence_.wz; xt::noalias(costs_) += s.gamma / std::pow(s.sampling_std.vx, 2) * xt::sum( - xt::view(control_sequence_.vx, xt::newaxis(), xt::all()) * bounded_noises_vx, 1, immediate); + xt::view(control_sequence_.vx, xt::newaxis(), xt::all()) * bounded_noises_vx, 1, immediate); xt::noalias(costs_) += s.gamma / std::pow(s.sampling_std.wz, 2) * xt::sum( - xt::view(control_sequence_.wz, xt::newaxis(), xt::all()) * bounded_noises_wz, 1, immediate); + xt::view(control_sequence_.wz, xt::newaxis(), xt::all()) * bounded_noises_wz, 1, immediate); if (isHolonomic()) { auto bounded_noises_vy = state_.cvy - control_sequence_.vy; xt::noalias(costs_) += s.gamma / std::pow(s.sampling_std.vy, 2) * xt::sum( - xt::view(control_sequence_.vy, xt::newaxis(), xt::all()) * bounded_noises_vy, - 1, immediate); + xt::view(control_sequence_.vy, xt::newaxis(), xt::all()) * bounded_noises_vy, + 1, immediate); } auto && costs_normalized = costs_ - xt::amin(costs_, immediate); diff --git a/test/utils/models.hpp b/test/utils/models.hpp index a7f31962..3c53ca00 100644 --- a/test/utils/models.hpp +++ b/test/utils/models.hpp @@ -1,6 +1,7 @@ // Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research #pragma once #include +#include #include #include diff --git a/test/utils/utils.hpp b/test/utils/utils.hpp index 372bbcdb..fd1adfe5 100644 --- a/test/utils/utils.hpp +++ b/test/utils/utils.hpp @@ -1,12 +1,14 @@ // Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research #pragma once -#include +#include +#include +#include #include #include #include -#include +#include "tf2_ros/transform_broadcaster.h" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" @@ -14,7 +16,7 @@ #include "models.hpp" #include "factory.hpp" -using namespace std::chrono_literals; +using namespace std::chrono_literals; // NOLINT void waitSome(const std::chrono::nanoseconds & duration, auto & node) { From a12836cda4fbe23e4a5363ae53782be0de394162 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Tue, 29 Nov 2022 14:46:22 -0800 Subject: [PATCH 19/53] adding copyright headers --- benchmark/controller_benchmark.cpp | 14 +++++++++++++- benchmark/optimizer_benchmark.cpp | 14 +++++++++++++- include/mppic/controller.hpp | 15 ++++++++++++++- include/mppic/critic_data.hpp | 14 +++++++++++++- include/mppic/critic_function.hpp | 15 ++++++++++++++- include/mppic/critic_manager.hpp | 14 +++++++++++++- include/mppic/critics/constraint_critic.hpp | 15 ++++++++++++++- include/mppic/critics/goal_angle_critic.hpp | 15 ++++++++++++++- include/mppic/critics/goal_critic.hpp | 15 ++++++++++++++- include/mppic/critics/obstacles_critic.hpp | 15 ++++++++++++++- include/mppic/critics/path_align_critic.hpp | 15 ++++++++++++++- include/mppic/critics/path_angle_critic.hpp | 15 ++++++++++++++- include/mppic/critics/path_follow_critic.hpp | 15 ++++++++++++++- include/mppic/critics/prefer_forward_critic.hpp | 16 ++++++++++++++-- include/mppic/critics/twirling_critic.hpp | 15 ++++++++++++++- include/mppic/models/constraints.hpp | 15 ++++++++++++++- include/mppic/models/control_sequence.hpp | 15 ++++++++++++++- include/mppic/models/optimizer_settings.hpp | 15 ++++++++++++++- include/mppic/models/path.hpp | 15 ++++++++++++++- include/mppic/models/state.hpp | 15 ++++++++++++++- include/mppic/models/trajectories.hpp | 15 ++++++++++++++- include/mppic/motion_models.hpp | 14 +++++++++++++- include/mppic/optimizer.hpp | 14 +++++++++++++- include/mppic/tools/noise_generator.hpp | 14 +++++++++++++- include/mppic/tools/parameters_handler.hpp | 14 +++++++++++++- include/mppic/tools/path_handler.hpp | 15 ++++++++++++++- include/mppic/tools/trajectory_visualizer.hpp | 15 ++++++++++++++- include/mppic/tools/utils.hpp | 15 ++++++++++++++- src/controller.cpp | 15 ++++++++++++++- src/critic_manager.cpp | 15 ++++++++++++++- src/critics/constraint_critic.cpp | 15 ++++++++++++++- src/critics/goal_angle_critic.cpp | 15 ++++++++++++++- src/critics/goal_critic.cpp | 15 ++++++++++++++- src/critics/obstacles_critic.cpp | 15 ++++++++++++++- src/critics/path_align_critic.cpp | 15 ++++++++++++++- src/critics/path_angle_critic.cpp | 15 ++++++++++++++- src/critics/path_follow_critic.cpp | 15 ++++++++++++++- src/critics/prefer_forward_critic.cpp | 14 +++++++++++++- src/critics/twirling_critic.cpp | 15 ++++++++++++++- src/noise_generator.cpp | 15 ++++++++++++++- src/optimizer.cpp | 15 ++++++++++++++- src/parameters_handler.cpp | 15 ++++++++++++++- src/path_handler.cpp | 16 ++++++++++++++-- src/trajectory_visualizer.cpp | 16 ++++++++++++++-- test/controller_test.cpp | 14 +++++++++++++- test/optimizer_test.cpp | 15 ++++++++++++++- test/utils/factory.hpp | 15 ++++++++++++++- test/utils/models.hpp | 15 ++++++++++++++- test/utils/utils.hpp | 15 ++++++++++++++- 49 files changed, 676 insertions(+), 52 deletions(-) diff --git a/benchmark/controller_benchmark.cpp b/benchmark/controller_benchmark.cpp index 17d749e9..b2336bef 100644 --- a/benchmark/controller_benchmark.cpp +++ b/benchmark/controller_benchmark.cpp @@ -1,4 +1,16 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include #include diff --git a/benchmark/optimizer_benchmark.cpp b/benchmark/optimizer_benchmark.cpp index 9cda8d68..6cfebda4 100644 --- a/benchmark/optimizer_benchmark.cpp +++ b/benchmark/optimizer_benchmark.cpp @@ -1,4 +1,16 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include #include diff --git a/include/mppic/controller.hpp b/include/mppic/controller.hpp index 22f5a5aa..acca58c7 100644 --- a/include/mppic/controller.hpp +++ b/include/mppic/controller.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 FastSense, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef MPPIC__CONTROLLER_HPP_ #define MPPIC__CONTROLLER_HPP_ diff --git a/include/mppic/critic_data.hpp b/include/mppic/critic_data.hpp index 2e38e900..075ebe01 100644 --- a/include/mppic/critic_data.hpp +++ b/include/mppic/critic_data.hpp @@ -1,4 +1,16 @@ -// Copyright 2022 @artofnothingness Aleksei Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MPPIC__CRITIC_DATA_HPP_ #define MPPIC__CRITIC_DATA_HPP_ diff --git a/include/mppic/critic_function.hpp b/include/mppic/critic_function.hpp index 3cd31079..e5f5d964 100644 --- a/include/mppic/critic_function.hpp +++ b/include/mppic/critic_function.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef MPPIC__CRITIC_FUNCTION_HPP_ #define MPPIC__CRITIC_FUNCTION_HPP_ diff --git a/include/mppic/critic_manager.hpp b/include/mppic/critic_manager.hpp index 7040e4b6..0f9e66a9 100644 --- a/include/mppic/critic_manager.hpp +++ b/include/mppic/critic_manager.hpp @@ -1,4 +1,16 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MPPIC__CRITIC_MANAGER_HPP_ #define MPPIC__CRITIC_MANAGER_HPP_ diff --git a/include/mppic/critics/constraint_critic.hpp b/include/mppic/critics/constraint_critic.hpp index f3874332..ab37c27b 100644 --- a/include/mppic/critics/constraint_critic.hpp +++ b/include/mppic/critics/constraint_critic.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 FastSense, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #pragma once #include "mppic/critic_function.hpp" diff --git a/include/mppic/critics/goal_angle_critic.hpp b/include/mppic/critics/goal_angle_critic.hpp index da3bf704..86ab09fb 100644 --- a/include/mppic/critics/goal_angle_critic.hpp +++ b/include/mppic/critics/goal_angle_critic.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 FastSense, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #pragma once #include "mppic/critic_function.hpp" diff --git a/include/mppic/critics/goal_critic.hpp b/include/mppic/critics/goal_critic.hpp index 70cc4037..3b93f811 100644 --- a/include/mppic/critics/goal_critic.hpp +++ b/include/mppic/critics/goal_critic.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 FastSense, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #pragma once #include "mppic/critic_function.hpp" diff --git a/include/mppic/critics/obstacles_critic.hpp b/include/mppic/critics/obstacles_critic.hpp index c9f5a5fa..ec3fd61e 100644 --- a/include/mppic/critics/obstacles_critic.hpp +++ b/include/mppic/critics/obstacles_critic.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 FastSense, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #pragma once #include #include "nav2_costmap_2d/footprint_collision_checker.hpp" diff --git a/include/mppic/critics/path_align_critic.hpp b/include/mppic/critics/path_align_critic.hpp index 58d76b67..5e72ead3 100644 --- a/include/mppic/critics/path_align_critic.hpp +++ b/include/mppic/critics/path_align_critic.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 FastSense, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #pragma once #include "mppic/critic_function.hpp" diff --git a/include/mppic/critics/path_angle_critic.hpp b/include/mppic/critics/path_angle_critic.hpp index a2c2e7b3..f1db4da2 100644 --- a/include/mppic/critics/path_angle_critic.hpp +++ b/include/mppic/critics/path_angle_critic.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 FastSense, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #pragma once #include "mppic/critic_function.hpp" diff --git a/include/mppic/critics/path_follow_critic.hpp b/include/mppic/critics/path_follow_critic.hpp index afac54ae..e308f5df 100644 --- a/include/mppic/critics/path_follow_critic.hpp +++ b/include/mppic/critics/path_follow_critic.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #pragma once #include "mppic/critic_function.hpp" diff --git a/include/mppic/critics/prefer_forward_critic.hpp b/include/mppic/critics/prefer_forward_critic.hpp index 8f67d051..0fcd8a97 100644 --- a/include/mppic/critics/prefer_forward_critic.hpp +++ b/include/mppic/critics/prefer_forward_critic.hpp @@ -1,6 +1,18 @@ -// Copyright 2022 FastSense, Samsung Research -#pragma once +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once #include "mppic/critic_function.hpp" #include "mppic/tools/utils.hpp" diff --git a/include/mppic/critics/twirling_critic.hpp b/include/mppic/critics/twirling_critic.hpp index 5768a0ef..9e4e3870 100644 --- a/include/mppic/critics/twirling_critic.hpp +++ b/include/mppic/critics/twirling_critic.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 FastSense, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #pragma once #include "mppic/critic_function.hpp" diff --git a/include/mppic/models/constraints.hpp b/include/mppic/models/constraints.hpp index 8d9832e1..b16e71fe 100644 --- a/include/mppic/models/constraints.hpp +++ b/include/mppic/models/constraints.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef MPPIC__MODELS__CONSTRAINTS_HPP_ #define MPPIC__MODELS__CONSTRAINTS_HPP_ diff --git a/include/mppic/models/control_sequence.hpp b/include/mppic/models/control_sequence.hpp index 17a60e15..b698a159 100644 --- a/include/mppic/models/control_sequence.hpp +++ b/include/mppic/models/control_sequence.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef MPPIC__MODELS__CONTROL_SEQUENCE_HPP_ #define MPPIC__MODELS__CONTROL_SEQUENCE_HPP_ diff --git a/include/mppic/models/optimizer_settings.hpp b/include/mppic/models/optimizer_settings.hpp index d14711ae..02d08a8d 100644 --- a/include/mppic/models/optimizer_settings.hpp +++ b/include/mppic/models/optimizer_settings.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef MPPIC__MODELS__OPTIMIZER_SETTINGS_HPP_ #define MPPIC__MODELS__OPTIMIZER_SETTINGS_HPP_ diff --git a/include/mppic/models/path.hpp b/include/mppic/models/path.hpp index b7ee50b6..8894dd21 100644 --- a/include/mppic/models/path.hpp +++ b/include/mppic/models/path.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef MPPIC__MODELS__PATH_HPP_ #define MPPIC__MODELS__PATH_HPP_ diff --git a/include/mppic/models/state.hpp b/include/mppic/models/state.hpp index 4ad74486..39eb3999 100644 --- a/include/mppic/models/state.hpp +++ b/include/mppic/models/state.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef MPPIC__MODELS__STATE_HPP_ #define MPPIC__MODELS__STATE_HPP_ diff --git a/include/mppic/models/trajectories.hpp b/include/mppic/models/trajectories.hpp index 7cf9bb8c..c596e735 100644 --- a/include/mppic/models/trajectories.hpp +++ b/include/mppic/models/trajectories.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef MPPIC__MODELS__TRAJECTORIES_HPP_ #define MPPIC__MODELS__TRAJECTORIES_HPP_ diff --git a/include/mppic/motion_models.hpp b/include/mppic/motion_models.hpp index 75226ebe..467c6c4b 100644 --- a/include/mppic/motion_models.hpp +++ b/include/mppic/motion_models.hpp @@ -1,4 +1,16 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MPPIC__MOTION_MODELS_HPP_ #define MPPIC__MOTION_MODELS_HPP_ diff --git a/include/mppic/optimizer.hpp b/include/mppic/optimizer.hpp index bb4401dd..c7f3d1cc 100644 --- a/include/mppic/optimizer.hpp +++ b/include/mppic/optimizer.hpp @@ -1,4 +1,16 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MPPIC__OPTIMIZER_HPP_ #define MPPIC__OPTIMIZER_HPP_ diff --git a/include/mppic/tools/noise_generator.hpp b/include/mppic/tools/noise_generator.hpp index 1abd3fe0..c5cd1c8a 100644 --- a/include/mppic/tools/noise_generator.hpp +++ b/include/mppic/tools/noise_generator.hpp @@ -1,4 +1,16 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MPPIC__TOOLS__NOISE_GENERATOR_HPP_ #define MPPIC__TOOLS__NOISE_GENERATOR_HPP_ diff --git a/include/mppic/tools/parameters_handler.hpp b/include/mppic/tools/parameters_handler.hpp index a2a8c98e..bdc32ec0 100644 --- a/include/mppic/tools/parameters_handler.hpp +++ b/include/mppic/tools/parameters_handler.hpp @@ -1,4 +1,16 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MPPIC__TOOLS__PARAMETERS_HANDLER_HPP_ #define MPPIC__TOOLS__PARAMETERS_HANDLER_HPP_ diff --git a/include/mppic/tools/path_handler.hpp b/include/mppic/tools/path_handler.hpp index ddc0c476..b1ed0178 100644 --- a/include/mppic/tools/path_handler.hpp +++ b/include/mppic/tools/path_handler.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 FastSense, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef MPPIC__TOOLS__PATH_HANDLER_HPP_ #define MPPIC__TOOLS__PATH_HANDLER_HPP_ diff --git a/include/mppic/tools/trajectory_visualizer.hpp b/include/mppic/tools/trajectory_visualizer.hpp index 3af740e9..fc6d5fb4 100644 --- a/include/mppic/tools/trajectory_visualizer.hpp +++ b/include/mppic/tools/trajectory_visualizer.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 FastSense, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef MPPIC__TOOLS__TRAJECTORY_VISUALIZER_HPP_ #define MPPIC__TOOLS__TRAJECTORY_VISUALIZER_HPP_ diff --git a/include/mppic/tools/utils.hpp b/include/mppic/tools/utils.hpp index 638a1389..2d4c15d9 100644 --- a/include/mppic/tools/utils.hpp +++ b/include/mppic/tools/utils.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef MPPIC__TOOLS__UTILS_HPP_ #define MPPIC__TOOLS__UTILS_HPP_ diff --git a/src/controller.cpp b/src/controller.cpp index dbe70e4a..025fb09c 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -1,4 +1,17 @@ -// Copyright 2022 FastSense, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include "mppic/controller.hpp" diff --git a/src/critic_manager.cpp b/src/critic_manager.cpp index 63c947e5..b3c2e84c 100644 --- a/src/critic_manager.cpp +++ b/src/critic_manager.cpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "mppic/critic_manager.hpp" namespace mppi diff --git a/src/critics/constraint_critic.cpp b/src/critics/constraint_critic.cpp index e4bef0b2..f567e248 100644 --- a/src/critics/constraint_critic.cpp +++ b/src/critics/constraint_critic.cpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "mppic/critics/constraint_critic.hpp" namespace mppi::critics diff --git a/src/critics/goal_angle_critic.cpp b/src/critics/goal_angle_critic.cpp index 559187b7..f94d0630 100644 --- a/src/critics/goal_angle_critic.cpp +++ b/src/critics/goal_angle_critic.cpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "mppic/critics/goal_angle_critic.hpp" namespace mppi::critics diff --git a/src/critics/goal_critic.cpp b/src/critics/goal_critic.cpp index b3e92edd..ef3367ec 100644 --- a/src/critics/goal_critic.cpp +++ b/src/critics/goal_critic.cpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "mppic/critics/goal_critic.hpp" namespace mppi::critics diff --git a/src/critics/obstacles_critic.cpp b/src/critics/obstacles_critic.cpp index 6ef629b2..1fd829b4 100644 --- a/src/critics/obstacles_critic.cpp +++ b/src/critics/obstacles_critic.cpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include "mppic/critics/obstacles_critic.hpp" diff --git a/src/critics/path_align_critic.cpp b/src/critics/path_align_critic.cpp index 92e58bb4..d5d86226 100644 --- a/src/critics/path_align_critic.cpp +++ b/src/critics/path_align_critic.cpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "mppic/critics/path_align_critic.hpp" #include diff --git a/src/critics/path_angle_critic.cpp b/src/critics/path_angle_critic.cpp index 24112ca5..9dffded1 100644 --- a/src/critics/path_angle_critic.cpp +++ b/src/critics/path_angle_critic.cpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "mppic/critics/path_angle_critic.hpp" #include diff --git a/src/critics/path_follow_critic.cpp b/src/critics/path_follow_critic.cpp index 784bf1b0..c9f48a6a 100644 --- a/src/critics/path_follow_critic.cpp +++ b/src/critics/path_follow_critic.cpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "mppic/critics/path_follow_critic.hpp" #include diff --git a/src/critics/prefer_forward_critic.cpp b/src/critics/prefer_forward_critic.cpp index fef7a068..f4d56364 100644 --- a/src/critics/prefer_forward_critic.cpp +++ b/src/critics/prefer_forward_critic.cpp @@ -1,4 +1,16 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include "mppic/critics/prefer_forward_critic.hpp" diff --git a/src/critics/twirling_critic.cpp b/src/critics/twirling_critic.cpp index 481de138..b0c4fbdf 100644 --- a/src/critics/twirling_critic.cpp +++ b/src/critics/twirling_critic.cpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "mppic/critics/twirling_critic.hpp" namespace mppi::critics diff --git a/src/noise_generator.cpp b/src/noise_generator.cpp index a0a273bd..43c97594 100644 --- a/src/noise_generator.cpp +++ b/src/noise_generator.cpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "mppic/tools/noise_generator.hpp" #include diff --git a/src/optimizer.cpp b/src/optimizer.cpp index ca317aa3..05b794a4 100644 --- a/src/optimizer.cpp +++ b/src/optimizer.cpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "mppic/optimizer.hpp" #include diff --git a/src/parameters_handler.cpp b/src/parameters_handler.cpp index 00102391..f665a454 100644 --- a/src/parameters_handler.cpp +++ b/src/parameters_handler.cpp @@ -1,4 +1,17 @@ -// Copyright 2022 FastSense, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "mppic/tools/parameters_handler.hpp" namespace mppi diff --git a/src/path_handler.cpp b/src/path_handler.cpp index d51fd9da..9f3eaa44 100644 --- a/src/path_handler.cpp +++ b/src/path_handler.cpp @@ -1,6 +1,18 @@ -// Copyright 2022 FastSense, Samsung Research -#include "mppic/tools/path_handler.hpp" +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "mppic/tools/path_handler.hpp" #include "mppic/tools/utils.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" diff --git a/src/trajectory_visualizer.cpp b/src/trajectory_visualizer.cpp index 80a13f37..cc20af87 100644 --- a/src/trajectory_visualizer.cpp +++ b/src/trajectory_visualizer.cpp @@ -1,6 +1,18 @@ -// Copyright 2022 FastSense, Samsung Research -#include +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include #include "mppic/tools/trajectory_visualizer.hpp" namespace mppi diff --git a/test/controller_test.cpp b/test/controller_test.cpp index e876d072..5a0c01de 100644 --- a/test/controller_test.cpp +++ b/test/controller_test.cpp @@ -1,4 +1,16 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include "gtest/gtest.h" #include diff --git a/test/optimizer_test.cpp b/test/optimizer_test.cpp index e82613b5..e9def50a 100644 --- a/test/optimizer_test.cpp +++ b/test/optimizer_test.cpp @@ -1,4 +1,17 @@ -// Copyright 2022 FastSense, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "gtest/gtest.h" #include #include diff --git a/test/utils/factory.hpp b/test/utils/factory.hpp index 22022151..81a6a0db 100644 --- a/test/utils/factory.hpp +++ b/test/utils/factory.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #pragma once #include diff --git a/test/utils/models.hpp b/test/utils/models.hpp index 3c53ca00..fa819efe 100644 --- a/test/utils/models.hpp +++ b/test/utils/models.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #pragma once #include #include diff --git a/test/utils/utils.hpp b/test/utils/utils.hpp index fd1adfe5..4c59089e 100644 --- a/test/utils/utils.hpp +++ b/test/utils/utils.hpp @@ -1,4 +1,17 @@ -// Copyright 2022 @artofnothingness Alexey Budyakov, Samsung Research +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #pragma once #include From 6173f34c0cf378e1c309532014567a2c503c7b73 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Tue, 29 Nov 2022 15:51:10 -0800 Subject: [PATCH 20/53] adding header gaurds to critics --- include/mppic/critics/constraint_critic.hpp | 5 ++++- include/mppic/critics/goal_angle_critic.hpp | 5 ++++- include/mppic/critics/goal_critic.hpp | 5 ++++- include/mppic/critics/obstacles_critic.hpp | 6 +++++- include/mppic/critics/path_align_critic.hpp | 5 ++++- include/mppic/critics/path_angle_critic.hpp | 5 ++++- include/mppic/critics/path_follow_critic.hpp | 5 ++++- include/mppic/critics/prefer_forward_critic.hpp | 5 ++++- include/mppic/critics/twirling_critic.hpp | 5 ++++- 9 files changed, 37 insertions(+), 9 deletions(-) diff --git a/include/mppic/critics/constraint_critic.hpp b/include/mppic/critics/constraint_critic.hpp index ab37c27b..7f8c3972 100644 --- a/include/mppic/critics/constraint_critic.hpp +++ b/include/mppic/critics/constraint_critic.hpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef MPPIC__CRITICS__CONSTRAINT_CRITIC_HPP_ +#define MPPIC__CRITICS__CONSTRAINT_CRITIC_HPP_ #include "mppic/critic_function.hpp" #include "mppic/models/state.hpp" @@ -48,3 +49,5 @@ class ConstraintCritic : public CriticFunction }; } // namespace mppi::critics + +#endif // MPPIC__CRITICS__CONSTRAINT_CRITIC_HPP_ diff --git a/include/mppic/critics/goal_angle_critic.hpp b/include/mppic/critics/goal_angle_critic.hpp index 86ab09fb..229d6005 100644 --- a/include/mppic/critics/goal_angle_critic.hpp +++ b/include/mppic/critics/goal_angle_critic.hpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef MPPIC__CRITICS__GOAL_ANGLE_CRITIC_HPP_ +#define MPPIC__CRITICS__GOAL_ANGLE_CRITIC_HPP_ #include "mppic/critic_function.hpp" #include "mppic/models/state.hpp" @@ -48,3 +49,5 @@ class GoalAngleCritic : public CriticFunction }; } // namespace mppi::critics + +#endif // MPPIC__CRITICS__GOAL_ANGLE_CRITIC_HPP_ diff --git a/include/mppic/critics/goal_critic.hpp b/include/mppic/critics/goal_critic.hpp index 3b93f811..98686560 100644 --- a/include/mppic/critics/goal_critic.hpp +++ b/include/mppic/critics/goal_critic.hpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef MPPIC__CRITICS__GOAL_CRITIC_HPP_ +#define MPPIC__CRITICS__GOAL_CRITIC_HPP_ #include "mppic/critic_function.hpp" #include "mppic/models/state.hpp" @@ -46,3 +47,5 @@ class GoalCritic : public CriticFunction }; } // namespace mppi::critics + +#endif // MPPIC__CRITICS__GOAL_CRITIC_HPP_ diff --git a/include/mppic/critics/obstacles_critic.hpp b/include/mppic/critics/obstacles_critic.hpp index ec3fd61e..353da079 100644 --- a/include/mppic/critics/obstacles_critic.hpp +++ b/include/mppic/critics/obstacles_critic.hpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef MPPIC__CRITICS__OBSTACLES_CRITIC_HPP_ +#define MPPIC__CRITICS__OBSTACLES_CRITIC_HPP_ + #include #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" @@ -110,3 +112,5 @@ class ObstaclesCritic : public CriticFunction }; } // namespace mppi::critics + +#endif // MPPIC__CRITICS__OBSTACLES_CRITIC_HPP_ diff --git a/include/mppic/critics/path_align_critic.hpp b/include/mppic/critics/path_align_critic.hpp index 5e72ead3..03a9810a 100644 --- a/include/mppic/critics/path_align_critic.hpp +++ b/include/mppic/critics/path_align_critic.hpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef MPPIC__CRITICS__PATH_ALIGN_CRITIC_HPP_ +#define MPPIC__CRITICS__PATH_ALIGN_CRITIC_HPP_ #include "mppic/critic_function.hpp" #include "mppic/models/state.hpp" @@ -50,3 +51,5 @@ class PathAlignCritic : public CriticFunction }; } // namespace mppi::critics + +#endif // MPPIC__CRITICS__PATH_ALIGN_CRITIC_HPP_ diff --git a/include/mppic/critics/path_angle_critic.hpp b/include/mppic/critics/path_angle_critic.hpp index f1db4da2..e359f8d7 100644 --- a/include/mppic/critics/path_angle_critic.hpp +++ b/include/mppic/critics/path_angle_critic.hpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef MPPIC__CRITICS__PATH_ANGLE_CRITIC_HPP_ +#define MPPIC__CRITICS__PATH_ANGLE_CRITIC_HPP_ #include "mppic/critic_function.hpp" #include "mppic/models/state.hpp" @@ -52,3 +53,5 @@ class PathAngleCritic : public CriticFunction }; } // namespace mppi::critics + +#endif // MPPIC__CRITICS__PATH_ANGLE_CRITIC_HPP_ diff --git a/include/mppic/critics/path_follow_critic.hpp b/include/mppic/critics/path_follow_critic.hpp index e308f5df..566bc851 100644 --- a/include/mppic/critics/path_follow_critic.hpp +++ b/include/mppic/critics/path_follow_critic.hpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef MPPIC__CRITICS__PATH_FOLLOW_CRITIC_HPP_ +#define MPPIC__CRITICS__PATH_FOLLOW_CRITIC_HPP_ #include "mppic/critic_function.hpp" #include "mppic/models/state.hpp" @@ -50,3 +51,5 @@ class PathFollowCritic : public CriticFunction }; } // namespace mppi::critics + +#endif // MPPIC__CRITICS__PATH_FOLLOW_CRITIC_HPP_ diff --git a/include/mppic/critics/prefer_forward_critic.hpp b/include/mppic/critics/prefer_forward_critic.hpp index 0fcd8a97..9c53fe63 100644 --- a/include/mppic/critics/prefer_forward_critic.hpp +++ b/include/mppic/critics/prefer_forward_critic.hpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef MPPIC__CRITICS__PREFER_FORWARD_CRITIC_HPP_ +#define MPPIC__CRITICS__PREFER_FORWARD_CRITIC_HPP_ #include "mppic/critic_function.hpp" #include "mppic/tools/utils.hpp" @@ -47,3 +48,5 @@ class PreferForwardCritic : public CriticFunction }; } // namespace mppi::critics + +#endif // MPPIC__CRITICS__PREFER_FORWARD_CRITIC_HPP_ diff --git a/include/mppic/critics/twirling_critic.hpp b/include/mppic/critics/twirling_critic.hpp index 9e4e3870..2fce81a0 100644 --- a/include/mppic/critics/twirling_critic.hpp +++ b/include/mppic/critics/twirling_critic.hpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef MPPIC__CRITICS__TWIRLING_CRITIC_HPP_ +#define MPPIC__CRITICS__TWIRLING_CRITIC_HPP_ #include "mppic/critic_function.hpp" #include "mppic/tools/utils.hpp" @@ -46,3 +47,5 @@ class TwirlingCritic : public CriticFunction }; } // namespace mppi::critics + +#endif // MPPIC__CRITICS__TWIRLING_CRITIC_HPP_ From bafa9f2afbf76e3856caba3cb7273a16fac3cdd4 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Tue, 29 Nov 2022 16:23:50 -0800 Subject: [PATCH 21/53] rename controller / plugin to be less abstract --- README.md | 2 +- include/mppic/controller.hpp | 8 ++++---- mppic.xml | 2 +- src/controller.cpp | 18 +++++++++--------- 4 files changed, 15 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index 6d3a8389..d5b1f42f 100644 --- a/README.md +++ b/README.md @@ -158,7 +158,7 @@ controller_server: ros__parameters: controller_frequency: 30.0 FollowPath: - plugin: "mppi::Controller" + plugin: "mppi::MPPIController" time_steps: 56 model_dt: 0.05 batch_size: 2000 diff --git a/include/mppic/controller.hpp b/include/mppic/controller.hpp index acca58c7..9e382dbe 100644 --- a/include/mppic/controller.hpp +++ b/include/mppic/controller.hpp @@ -32,16 +32,16 @@ namespace mppi { /** - * @class mppi::Controller + * @class mppi::MPPIController * @brief Main plugin controller for MPPI Controller */ -class Controller : public nav2_core::Controller +class MPPIController : public nav2_core::Controller { public: /** - * @brief Constructor for mppi::Controller + * @brief Constructor for mppi::MPPIController */ - Controller() = default; + MPPIController() = default; /** * @brief Configure controller on bringup diff --git a/mppic.xml b/mppic.xml index c2b571b0..b9b3c42a 100644 --- a/mppic.xml +++ b/mppic.xml @@ -1,6 +1,6 @@ - + MPPI Controller for Nav2 diff --git a/src/controller.cpp b/src/controller.cpp index 025fb09c..fa0aa607 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -22,7 +22,7 @@ namespace mppi { -void Controller::configure( +void MPPIController::configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, const std::shared_ptr tf, const std::shared_ptr costmap_ros) @@ -48,7 +48,7 @@ void Controller::configure( RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str()); } -void Controller::cleanup() +void MPPIController::cleanup() { optimizer_.shutdown(); trajectory_visualizer_.on_cleanup(); @@ -56,20 +56,20 @@ void Controller::cleanup() RCLCPP_INFO(logger_, "Cleaned up MPPI Controller: %s", name_.c_str()); } -void Controller::activate() +void MPPIController::activate() { trajectory_visualizer_.on_activate(); parameters_handler_->start(); RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str()); } -void Controller::deactivate() +void MPPIController::deactivate() { trajectory_visualizer_.on_deactivate(); RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str()); } -geometry_msgs::msg::TwistStamped Controller::computeVelocityCommands( +geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, nav2_core::GoalChecker * goal_checker) @@ -97,19 +97,19 @@ geometry_msgs::msg::TwistStamped Controller::computeVelocityCommands( return cmd; } -void Controller::visualize(nav_msgs::msg::Path transformed_plan) +void MPPIController::visualize(nav_msgs::msg::Path transformed_plan) { trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories()); trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory()); trajectory_visualizer_.visualize(std::move(transformed_plan)); } -void Controller::setPlan(const nav_msgs::msg::Path & path) +void MPPIController::setPlan(const nav_msgs::msg::Path & path) { path_handler_.setPath(path); } -void Controller::setSpeedLimit(const double & speed_limit, const bool & percentage) +void MPPIController::setSpeedLimit(const double & speed_limit, const bool & percentage) { optimizer_.setSpeedLimit(speed_limit, percentage); } @@ -117,4 +117,4 @@ void Controller::setSpeedLimit(const double & speed_limit, const bool & percenta } // namespace mppi #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(mppi::Controller, nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(mppi::MPPIController, nav2_core::Controller) From 9b74143dc0f6f1aadbeb37775b4f82af6003d533 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Tue, 29 Nov 2022 16:31:42 -0800 Subject: [PATCH 22/53] fix test --- test/utils/factory.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/utils/factory.hpp b/test/utils/factory.hpp index 81a6a0db..bb0d6eb8 100644 --- a/test/utils/factory.hpp +++ b/test/utils/factory.hpp @@ -168,9 +168,9 @@ mppi::PathHandler getDummyPathHandler( return path_handler; } -std::shared_ptr getDummyController(auto node, auto tf_buffer, auto costmap_ros) +std::shared_ptr getDummyController(auto node, auto tf_buffer, auto costmap_ros) { - std::shared_ptr controller = std::make_shared(); + std::shared_ptr controller = std::make_shared(); std::weak_ptr weak_ptr_node{node}; controller->configure(weak_ptr_node, node->get_name(), tf_buffer, costmap_ros); From 73f06ab0c1a0600d47da2d3664eb377f809fa479 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 30 Nov 2022 12:10:43 -0800 Subject: [PATCH 23/53] add in refactors to readme --- README.md | 182 +++++++++++++++++++++++++++++------------------------- 1 file changed, 99 insertions(+), 83 deletions(-) diff --git a/README.md b/README.md index d5b1f42f..8555612c 100644 --- a/README.md +++ b/README.md @@ -4,17 +4,17 @@ ## Overview -This is a controller (local trajectory planner) that implements the [Model Predictive Path Integral (MPPI)](https://ieeexplore.ieee.org/document/7487277) algorithm to track a path with adaptive collision avoidance. It contains plugin-based critic functions to impact the behavior of the algorithm. It was created by [Aleksei Budyakov](https://www.linkedin.com/in/aleksei-budyakov-334889224/) and adapted for Nav2 by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/). +This is a predictive controller (local trajectory planner) that implements the [Model Predictive Path Integral (MPPI)](https://ieeexplore.ieee.org/document/7487277) algorithm to track a path with adaptive collision avoidance. It contains plugin-based critic functions to impact the behavior of the algorithm. It was created by [Aleksei Budyakov](https://www.linkedin.com/in/aleksei-budyakov-334889224/) and adapted & developed for Nav2 by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/). This plugin implements the ``nav2_core::Controller`` interface allowing it to be used across the navigation stack as a local trajectory planner in the controller server's action server (``controller_server``). -This controller is measured to run at 45 Hz on a modest Intel processor (4th gen i5). See its Configuration Guide Page for additional parameter descriptions. +This controller is measured to run at 50+ Hz on a modest Intel processor (4th gen i5). See its Configuration Guide Page for additional parameter descriptions. -It works currently with Differential and Omnidirectional robots, with support for Ackermann planned. +It works currently with Differential, Omnidirectional, and Ackermann robots. ## MPPI Description -The MPPI algorithm finds a control velocity for the robot using an iterative approach. Using the previous time step's best control solution and the robot's current state, a set of randomly sampled perturbations from a Gaussian distribution are applied. These noised controls are forward simulated to generate a set of trajectories within the robot's motion model. +The MPPI algorithm is an MPC variant that finds a control velocity for the robot using an iterative approach. Using the previous time step's best control solution and the robot's current state, a set of randomly sampled perturbations from a Gaussian distribution are applied. These noised controls are forward simulated to generate a set of trajectories within the robot's motion model. Next, these trajectories are scored using a set of plugin-based critic functions to find the best trajectory in the batch. The output scores are used to set the best control with a soft max function. @@ -41,7 +41,6 @@ cd build cmake .. sudo make install -# Optional git clone git@github.com:xtensor-stack/xsimd.git -b 8.0.5 cd xsimd mkdir build @@ -49,108 +48,125 @@ cmake .. sudo make install ``` +## Features + +- Utilizes plugin-based critics which can be swapped out, tuned, or replaced easily by the user +- Highly optimized CPU-only performance using vectorization and tensor operations +- Supports a number of common motion models, including Ackermann, Differential-Drive, and Omni-directional +- Includes fallback mechanisms to handle soft-failures before escalating to recovery behaviors +- High-quality code implementation with Doxygen, high unit test coverage, documentation, and parameter guide +- Easily extensible to support modern research variants of MPPI + ## Configuration -### Controller params +### Controller | Parameter | Type | Definition | - | --------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | - | motion_model | string | Type of model [DiffDrive, Omni, Ackermann] | - | critics | string | Critics (plugins) names | - | iteration_count | int | Iteration count in MPPI algorithm | - | max_robot_pose_search_dist | double | Upper bound on integrated distance along the global plan to search for the closest pose to the robot pose. This should be left as the default unless there are paths with loops and intersections that do not leave the local costmap, in which case making this value smaller is necessary to prevent shortcutting. | - | transform_tolerance | double | TF tolerance to transform poses | - | batch_size | int | Count of randomly sampled trajectories | - | time_steps | int | Number of time steps (points) in each sampled trajectory | - | model_dt | double | Time interval between two sampled points in trajectories | - | vx_std | double | Sampling standart deviation for VX | - | vy_std | double | Sampling standart deviation for VY | - | wx_std | double | Sampling standart deviation for WX | - | vx_max | double | Max VX | - | vy_max | double | Max VY | - | vx_min | double | Min VX | - | wz_max | double | Max WZ | - | temperature | double | Selectiveness of trajectories by their costs (The closer this value to 0, the "more" we take in considiration controls with less cost), 0 mean use control with best cost, huge value will lead to just taking mean of all trajectories without cost consideration | - | gamma | double | A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default of `0.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information. | - | visualize | bool | Use visualization | - | retry_attempt_limit | int | Number of attempts to find feasible trajectory before failure | - - -#### Ackermann Motion Model params - | Parameter | Type | Definition | - | -------------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | min_turning_r | double | minimum turning radius for ackermann motion model | + | --------------------- | ------ | -------------------------------------------------------------------------------------------------------- | + | motion_model | string | Default: DiffDrive. Type of model [DiffDrive, Omni, Ackermann]. | + | critics | string | Default: None. Critics (plugins) names | + | iteration_count | int | Default 1. Iteration count in MPPI algorithm. Recommend to keep as 1 and prefer more batches. | + | batch_size | int | Default 400. Count of randomly sampled candidate trajectories | + | time_steps | int | Default 15. Number of time steps (points) in each sampled trajectory | + | model_dt | double | Default: 0.1. Time interval (s) between two sampled points in trajectories. | + | vx_std | double | Default 0.2. Sampling standart deviation for VX | + | vy_std | double | Default 0.2. Sampling standart deviation for VY | + | wx_std | double | Default 1.0. Sampling standart deviation for WX | + | vx_max | double | Default 0.5. Max VX (m/s) | + | vy_max | double | Default 0.5. Max VY in either direction, if holonomic. (m/s) | + | vx_min | double | Default -0.35. Min VX (m/s) | + | wz_max | double | Default 1.3. Max WZ (rad/s) | + | temperature | double | Default: 0.35. Selectiveness of trajectories by their costs (The closer this value to 0, the "more" we take in considiration controls with less cost), 0 mean use control with best cost, huge value will lead to just taking mean of all trajectories without cost consideration | + | gamma | double | Default: 0.1. A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default of `0.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information. | + | visualize | bool | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging. | + | retry_attempt_limit | int | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure. | +#### Trajectory Visualizer + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | trajectory_step | int | Default: 5. The step between trajectories to visualize to downsample candidate trajectory pool. | + | time_step | int | Default: 3. The step between points on trajectories to visualize to downsample trajectory density. | +#### Path Handler + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | max_robot_pose_search_dist | double | Default: Costmap half-size. Max integrated distance ahead of robot pose to search for nearest path point in case of path looping. | + | prune_distance | double | Default: 1.5. Distance ahead of nearest point on path to robot to prune path to. | + | transform_tolerance | double | Default: 0.1. Time tolerance for data transformations with TF. | -#### GoalCritic params +#### Ackermann Motion Model | Parameter | Type | Definition | | -------------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | cost_weight | double | | - | cost_power | int | | - -#### GoalAngleCritic params - | Parameter | Type | Definition | - | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | cost_weight | double | | - | cost_power | int | | - | threshold_to_consider | double | Minimal distance between robot and goal above which angle goal cost considered | - + | min_turning_r | double | minimum turning radius for ackermann motion model | -#### PathFollowCritic params +#### Constraint Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | cost_weight | double | | - | cost_power | int | | - | offset_from_furthest | int | | - | max_path_ratio | float | | + | cost_weight | double | Default 4.0. Weight to apply to critic term. | + | cost_power | int | Default 1. Power order to apply to term. -#### PathAngleCritic params - | Parameter | Type | Definition | - | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | cost_weight | double | | - | cost_power | int | | - | threshold_to_consider | double | Distance between robot and goal above which path angle cost stops being considered | +#### Goal Angle Critic + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | cost_weight | double | Default 1.0. Weight to apply to critic term. | + | cost_power | int | Default 1. Power order to apply to term. | + | threshold_to_consider | double | Default 0.40. Minimal distance between robot and goal above which angle goal cost considered. | + +#### Goal Critic + | Parameter | Type | Definition | + | -------------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | cost_weight | double | Default 5.0. Weight to apply to critic term. | + | cost_power | int | Default 1. Power order to apply to term. | -#### PathAlignCritic params +#### Obstacles Critic + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. | + | cost_weight | double | Default 2.0. Weight to apply to critic term. | + | cost_power | int | Default 2. Power order to apply to term. | + | collision_cost | double | Default 2000.0. Cost to apply to a true collision in a trajectory. | + | collision_margin_distance | double | Default 0.12. Margin distance from collision to apply severe penalty. Between 0.05-0.2 is reasonable. | + | trajectory_penalty_distance | double | Default 1.0. Minimum trajectory distance from obstacle to apply a preferential penalty to incentivize navigating farther away from obstacles. | + | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term (e.g. `trajectory_penalty_distance` term) to allow robot to smoothly converge to goal pose in close proximity to obstacles. + +#### Path Align Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | cost_weight | double | | - | cost_power | int | | - | enable_nearest_goal_critic | bool | enable critic that scores by mean distance from generated trajectories to nearest to generated trajectories path points | - | path_point_step | int | Consider path points with given step | - | trajectory_point_step | int | Consider generated trajectories points with given step | - | threshold_to_consider | double | Distance between robot and goal above which path align cost stops being considered | + | cost_weight | double | Default 2.0. Weight to apply to critic term. | + | cost_power | int | Default 1. Power order to apply to term. | + | path_point_step | int | Default 2. Consider every N path points for alignment to speed up critic. | + | trajectory_point_step | int | Default 3. Consider every N generated trajectories points to speed up critic evaluation. | + | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which path align cost stops being considered | +#### Path Angle Critic + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | cost_weight | double | Default 2.0. Weight to apply to critic term. | + | cost_power | int | Default 1. Power order to apply to term. | + | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which path angle cost stops being considered | + | offset_from_furthest | int | Default 4. Number of path points after furthest one any trajectory achieves to compute path angle relative to. | + | max_angle_to_furthest | double | Default PI/2. Distance between robot and goal above which path angle cost starts being considered | -#### ObstaclesCritic params - | Parameter | Type | Definition | - | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | consider_footprint | bool | | - | cost_weight | double | | - | cost_power | int | | - | collision_margin_distance | double | Margin distance from collision to apply severe penalty. Between 0.05-0.2 is reasonable. | - | trajectory_penalty_distance | double | Minimum trajectory distance from obstacle to apply a preferential penalty to incentivize navigating farther away from obstacles. | - | near_goal_distance | double | Distance near goal to stop applying preferential obstacle term (e.g. `trajectory_penalty_distance` term) to allow robot to smoothly converge to goal pose in close proximity to obstacles. | - -#### PreferForwardCritic params +#### Path Follow Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | cost_weight | double | | - | cost_power | int | | - | threshold_to_consider | double | Distance between robot and goal above which prefer forward cost stops being considered | - + | cost_weight | double | Default 3.0. Weight to apply to critic term. | + | cost_power | int | Default 1. Power order to apply to term. | + | offset_from_furthest | int | Default 10. Number of path points after furthest one any trajectory achieves to drive path tracking relative to. | + | max_path_ratio | float | Default 0.4. Maximum percentage (0-1.0) of path overlapping with trajectory points to apply critic. Lets goal critics take over on approach to goal. | -#### TwirlingCritic params +#### Prefer Forward Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | cost_weight | double | | - | cost_power | int | | -#### ConstraintsCritic params + | cost_weight | double | Default 3.0. Weight to apply to critic term. | + | cost_power | int | Default 1. Power order to apply to term. | + | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which prefer forward cost stops being considered | + + +#### Twirling Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | cost_weight | double | | - | cost_power | int | - + | cost_weight | double | Default 10.0. Weight to apply to critic term. | + | cost_power | int | Default 1. Power order to apply to term. | ### XML configuration example ``` From 9ee8b767d513934dae7f1e3f9c59487b14155a8f Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 30 Nov 2022 12:11:37 -0800 Subject: [PATCH 24/53] remove deps, going to merge into 22.04 --- README.md | 28 ---------------------------- 1 file changed, 28 deletions(-) diff --git a/README.md b/README.md index 8555612c..982f8292 100644 --- a/README.md +++ b/README.md @@ -20,34 +20,6 @@ Next, these trajectories are scored using a set of plugin-based critic functions This process is then repeated a number of times and returns a converged solution. This solution is then used as the basis of the next time step's initial control. -## Dependencies - -This uses the usual ROS tools for dependency management, so please use ``rosdep`` to install the dependencies. - -Note: If running on Ubuntu 20.04 or other OS's that `xtensor` is not released in binary form, please manually install `xtensor` v 0.24.0 and `xtl` v 0.7.0. These are simply headers so the install process is trivially short, unfortunately the `xtensor` project isn't available in package managers in some common-place operating systems (albeit, all necessary ROS OS versions) so you may be required to do this yourself if building from source. - -``` -git clone git@github.com:xtensor-stack/xtensor.git -b 0.24.0 -cd xtensor -mkdir build -cd build -cmake .. -sudo make install - -git clone git@github.com:xtensor-stack/xtl.git -b 0.7.0 -cd xtl -mkdir build -cd build -cmake .. -sudo make install - -git clone git@github.com:xtensor-stack/xsimd.git -b 8.0.5 -cd xsimd -mkdir build -cmake .. -sudo make install -``` - ## Features - Utilizes plugin-based critics which can be swapped out, tuned, or replaced easily by the user From b1116ab06db2e734a85a1b7fd648d333671d224e Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 30 Nov 2022 12:12:52 -0800 Subject: [PATCH 25/53] feature bullet --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 982f8292..00200f2c 100644 --- a/README.md +++ b/README.md @@ -22,6 +22,7 @@ This process is then repeated a number of times and returns a converged solution ## Features +- Predictive MPC trajectory planner - Utilizes plugin-based critics which can be swapped out, tuned, or replaced easily by the user - Highly optimized CPU-only performance using vectorization and tensor operations - Supports a number of common motion models, including Ackermann, Differential-Drive, and Omni-directional From 73a9a71ba4e0aaba8b4c351b92df49dc77f48a6b Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 1 Dec 2022 17:09:29 -0800 Subject: [PATCH 26/53] adding a ton of unit testing --- include/mppic/models/trajectories.hpp | 12 -- include/mppic/tools/parameters_handler.hpp | 47 +++--- src/noise_generator.cpp | 4 +- test/CMakeLists.txt | 5 +- ...p => controller_state_transition_test.cpp} | 5 +- test/models_test.cpp | 149 +++++++++++++++++ test/noise_generator_test.cpp | 121 ++++++++++++++ test/optimizer_test.cpp | 2 + test/parameter_handler_test.cpp | 155 ++++++++++++++++++ test/utils/factory.hpp | 5 +- 10 files changed, 465 insertions(+), 40 deletions(-) rename test/{controller_test.cpp => controller_state_transition_test.cpp} (94%) create mode 100644 test/models_test.cpp create mode 100644 test/noise_generator_test.cpp create mode 100644 test/parameter_handler_test.cpp diff --git a/include/mppic/models/trajectories.hpp b/include/mppic/models/trajectories.hpp index c596e735..2d31d926 100644 --- a/include/mppic/models/trajectories.hpp +++ b/include/mppic/models/trajectories.hpp @@ -40,18 +40,6 @@ struct Trajectories y = xt::zeros({batch_size, time_steps}); yaws = xt::zeros({batch_size, time_steps}); } - - /** - * @brief Get the last point in all trajectories - * @return A tensor containing only final points in trajectories - */ - inline auto getLastPoints() const - { - return xt::concatenate( - xtuple( - xt::view(x, xt::all(), -1, xt::newaxis()), - xt::view(y, xt::all(), -1, xt::newaxis())), 1); - } }; } // namespace mppi::models diff --git a/include/mppic/tools/parameters_handler.hpp b/include/mppic/tools/parameters_handler.hpp index bdc32ec0..26a7a394 100644 --- a/include/mppic/tools/parameters_handler.hpp +++ b/include/mppic/tools/parameters_handler.hpp @@ -77,18 +77,6 @@ class ParametersHandler */ inline auto getParamGetter(const std::string & ns); - /** - * @brief Gets parameter - * @param setting Return Parameter type - * @param name Parameter name - * @param default_value Default parameter value - * @param param_type Type of parameter (dynamic or static) - */ - template - void getParam( - SettingT & setting, const std::string & name, ParamT default_value, - ParameterType param_type = ParameterType::Dynamic); - /** * @brief Set a callback to process after parameter changes * @param callback Callback function @@ -103,15 +91,6 @@ class ParametersHandler template void addPreCallback(T && callback); - /** - * @brief Set a parameter - * @param setting Return Parameter type - * @param name Parameter name - * @param node Node to set parameter via - */ - template - void setParam(SettingT & setting, const std::string & name, NodeT node) const; - /** * @brief Set a parameter to a dynamic parameter callback * @param setting Parameter @@ -129,7 +108,6 @@ class ParametersHandler return ¶meters_change_mutex_; } -protected: /** * @brief Set a parameter to a dynamic parameter callback * @param name Name of parameter @@ -138,6 +116,28 @@ class ParametersHandler template void addDynamicParamCallback(const std::string & name, T && callback); +protected: + /** + * @brief Gets parameter + * @param setting Return Parameter type + * @param name Parameter name + * @param default_value Default parameter value + * @param param_type Type of parameter (dynamic or static) + */ + template + void getParam( + SettingT & setting, const std::string & name, ParamT default_value, + ParameterType param_type = ParameterType::Dynamic); + + /** + * @brief Set a parameter + * @param setting Return Parameter type + * @param name Parameter name + * @param node Node to set parameter via + */ + template + void setParam(SettingT & setting, const std::string & name, NodeT node) const; + /** * @brief Converts parameter type to real types * @param parameter Parameter to convert into real type @@ -173,7 +173,6 @@ inline auto ParametersHandler::getParamGetter(const std::string & ns) }; } - template void ParametersHandler::addDynamicParamCallback(const std::string & name, T && callback) { @@ -258,6 +257,8 @@ auto ParametersHandler::as(const rclcpp::Parameter & parameter) return parameter.as_double_array(); } else if constexpr (std::is_same_v>) { return parameter.as_string_array(); + } else if constexpr (std::is_same_v>) { + return parameter.as_bool_array(); } } diff --git a/src/noise_generator.cpp b/src/noise_generator.cpp index 43c97594..58ece1a4 100644 --- a/src/noise_generator.cpp +++ b/src/noise_generator.cpp @@ -36,7 +36,9 @@ void NoiseGenerator::shutdown() active_ = false; ready_ = true; noise_cond_.notify_all(); - noise_thread_.join(); + if (noise_thread_.joinable()) { + noise_thread_.join(); + } } void NoiseGenerator::generateNextNoises() diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 07dbe278..abf71ddf 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,6 +1,9 @@ set(TEST_NAMES optimizer_test - controller_test + controller_state_transition_test + models_test + noise_generator_test + parameter_handler_test ) foreach(name IN LISTS TEST_NAMES) diff --git a/test/controller_test.cpp b/test/controller_state_transition_test.cpp similarity index 94% rename from test/controller_test.cpp rename to test/controller_state_transition_test.cpp index 5a0c01de..16d4457b 100644 --- a/test/controller_test.cpp +++ b/test/controller_state_transition_test.cpp @@ -32,10 +32,11 @@ class RosLockGuard }; RosLockGuard g_rclcpp; -TEST(ControllerTest, ControllerNotFail) +// Tests basic transition from configure->active->process->deactive->cleanup + +TEST(ControllerStateTransitionTest, ControllerNotFail) { const bool visualize = true; - TestCostmapSettings costmap_settings{}; // Node Options diff --git a/test/models_test.cpp b/test/models_test.cpp new file mode 100644 index 00000000..d05e48f1 --- /dev/null +++ b/test/models_test.cpp @@ -0,0 +1,149 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "mppic/models/control_sequence.hpp" +#include "mppic/models/path.hpp" +#include "mppic/models/state.hpp" +#include "mppic/models/trajectories.hpp" + +// Tests model classes with methods + +class RosLockGuard +{ +public: + RosLockGuard() {rclcpp::init(0, nullptr);} + ~RosLockGuard() {rclcpp::shutdown();} +}; +RosLockGuard g_rclcpp; + +using namespace mppi::models; // NOLINT + +TEST(ModelsTest, ControlSequenceTest) +{ + // populate the object + ControlSequence sequence; + sequence.vx = xt::ones({10}); + sequence.vy = xt::ones({10}); + sequence.wz = xt::ones({10}); + + // Show you can get contents + EXPECT_EQ(sequence.vx(4), 1); + EXPECT_EQ(sequence.vy(4), 1); + EXPECT_EQ(sequence.wz(4), 1); + + sequence.reset(20); + + // Show contents are gone and new size + EXPECT_EQ(sequence.vx(4), 0); + EXPECT_EQ(sequence.vy(4), 0); + EXPECT_EQ(sequence.wz(4), 0); + EXPECT_EQ(sequence.vx.shape(0), 20u); + EXPECT_EQ(sequence.vy.shape(0), 20u); + EXPECT_EQ(sequence.wz.shape(0), 20u); +} + +TEST(ModelsTest, PathTest) +{ + // populate the object + Path path; + path.x = xt::ones({10}); + path.y = xt::ones({10}); + path.yaws = xt::ones({10}); + + // Show you can get contents + EXPECT_EQ(path.x(4), 1); + EXPECT_EQ(path.y(4), 1); + EXPECT_EQ(path.yaws(4), 1); + + path.reset(20); + + // Show contents are gone and new size + EXPECT_EQ(path.x(4), 0); + EXPECT_EQ(path.y(4), 0); + EXPECT_EQ(path.yaws(4), 0); + EXPECT_EQ(path.x.shape(0), 20u); + EXPECT_EQ(path.y.shape(0), 20u); + EXPECT_EQ(path.yaws.shape(0), 20u); +} + +TEST(ModelsTest, StateTest) +{ + // populate the object + State state; + state.vx = xt::ones({10, 10}); + state.vy = xt::ones({10, 10}); + state.wz = xt::ones({10, 10}); + state.cvx = xt::ones({10, 10}); + state.cvy = xt::ones({10, 10}); + state.cwz = xt::ones({10, 10}); + + // Show you can get contents + EXPECT_EQ(state.cvx(4), 1); + EXPECT_EQ(state.cvy(4), 1); + EXPECT_EQ(state.cwz(4), 1); + EXPECT_EQ(state.vx(4), 1); + EXPECT_EQ(state.vy(4), 1); + EXPECT_EQ(state.wz(4), 1); + + state.reset(20, 40); + + // Show contents are gone and new size + EXPECT_EQ(state.cvx(4), 0); + EXPECT_EQ(state.cvy(4), 0); + EXPECT_EQ(state.cwz(4), 0); + EXPECT_EQ(state.vx(4), 0); + EXPECT_EQ(state.vy(4), 0); + EXPECT_EQ(state.wz(4), 0); + EXPECT_EQ(state.cvx.shape(0), 20u); + EXPECT_EQ(state.cvy.shape(0), 20u); + EXPECT_EQ(state.cwz.shape(0), 20u); + EXPECT_EQ(state.cvx.shape(1), 40u); + EXPECT_EQ(state.cvy.shape(1), 40u); + EXPECT_EQ(state.cwz.shape(1), 40u); + EXPECT_EQ(state.vx.shape(0), 20u); + EXPECT_EQ(state.vy.shape(0), 20u); + EXPECT_EQ(state.wz.shape(0), 20u); + EXPECT_EQ(state.vx.shape(1), 40u); + EXPECT_EQ(state.vy.shape(1), 40u); + EXPECT_EQ(state.wz.shape(1), 40u); +} + +TEST(ModelsTest, TrajectoriesTest) +{ + // populate the object + Trajectories trajectories; + trajectories.x = xt::ones({10, 10}); + trajectories.y = xt::ones({10, 10}); + trajectories.yaws = xt::ones({10, 10}); + + // Show you can get contents + EXPECT_EQ(trajectories.x(4), 1); + EXPECT_EQ(trajectories.y(4), 1); + EXPECT_EQ(trajectories.yaws(4), 1); + + trajectories.reset(20, 40); + + // Show contents are gone and new size + EXPECT_EQ(trajectories.x(4), 0); + EXPECT_EQ(trajectories.y(4), 0); + EXPECT_EQ(trajectories.yaws(4), 0); + EXPECT_EQ(trajectories.x.shape(0), 20u); + EXPECT_EQ(trajectories.y.shape(0), 20u); + EXPECT_EQ(trajectories.yaws.shape(0), 20u); + EXPECT_EQ(trajectories.x.shape(1), 40u); + EXPECT_EQ(trajectories.y.shape(1), 40u); + EXPECT_EQ(trajectories.yaws.shape(1), 40u); +} diff --git a/test/noise_generator_test.cpp b/test/noise_generator_test.cpp new file mode 100644 index 00000000..b918ba6e --- /dev/null +++ b/test/noise_generator_test.cpp @@ -0,0 +1,121 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "mppic/tools/noise_generator.hpp" +#include "mppic/models/optimizer_settings.hpp" +#include "mppic/models/state.hpp" +#include "mppic/models/control_sequence.hpp" + +// Tests noise generator object + +class RosLockGuard +{ +public: + RosLockGuard() {rclcpp::init(0, nullptr);} + ~RosLockGuard() {rclcpp::shutdown();} +}; +RosLockGuard g_rclcpp; + +using namespace mppi; // NOLINT + +TEST(NoiseGeneratorTest, NoiseGeneratorLifecycle) +{ + // Tests shuts down internal thread cleanly + NoiseGenerator generator; + mppi::models::OptimizerSettings settings; + settings.batch_size = 100; + settings.time_steps = 25; + + generator.initialize(settings, false); + generator.shutdown(); +} + +TEST(NoiseGeneratorTest, NoiseGeneratorMain) +{ + // Tests shuts down internal thread cleanly + NoiseGenerator generator; + mppi::models::OptimizerSettings settings; + settings.batch_size = 100; + settings.time_steps = 25; + settings.sampling_std.vx = 0.1; + settings.sampling_std.vy = 0.1; + settings.sampling_std.wz = 0.1; + + // Populate a potential control sequence + mppi::models::ControlSequence control_sequence; + control_sequence.reset(25); + for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) { + control_sequence.vx(i) = i; + control_sequence.vy(i) = i; + control_sequence.wz(i) = i; + } + + mppi::models::State state; + state.reset(settings.batch_size, settings.time_steps); + + // Request an update with no noise yet generated, should result in identical outputs + generator.initialize(settings, false); + generator.reset(settings, false); // sets initial sizing and zeros out noises + generator.setNoisedControls(state, control_sequence); + EXPECT_EQ(state.cvx(0), 0); + EXPECT_EQ(state.cvy(0), 0); + EXPECT_EQ(state.cwz(0), 0); + EXPECT_EQ(state.cvx(9), 9); + EXPECT_EQ(state.cvy(9), 9); + EXPECT_EQ(state.cwz(9), 9); + + // Request an update with noise requested + generator.generateNextNoises(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + generator.setNoisedControls(state, control_sequence); + EXPECT_NE(state.cvx(0), 0); + EXPECT_EQ(state.cvy(0), 0); // Not populated in non-holonomic + EXPECT_NE(state.cwz(0), 0); + EXPECT_NE(state.cvx(9), 9); + EXPECT_EQ(state.cvy(9), 9); // Not populated in non-holonomic + EXPECT_NE(state.cwz(9), 9); + + EXPECT_NEAR(state.cvx(0), 0, 0.3); + EXPECT_NEAR(state.cvy(0), 0, 0.3); + EXPECT_NEAR(state.cwz(0), 0, 0.3); + EXPECT_NEAR(state.cvx(9), 9, 0.3); + EXPECT_NEAR(state.cvy(9), 9, 0.3); + EXPECT_NEAR(state.cwz(9), 9, 0.3); + + // Test holonomic setting + generator.reset(settings, true); // Now holonomically + generator.generateNextNoises(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + generator.setNoisedControls(state, control_sequence); + EXPECT_NE(state.cvx(0), 0); + EXPECT_NE(state.cvy(0), 0); // Now populated in non-holonomic + EXPECT_NE(state.cwz(0), 0); + EXPECT_NE(state.cvx(9), 9); + EXPECT_NE(state.cvy(9), 9); // Now populated in non-holonomic + EXPECT_NE(state.cwz(9), 9); + + EXPECT_NEAR(state.cvx(0), 0, 0.3); + EXPECT_NEAR(state.cvy(0), 0, 0.3); + EXPECT_NEAR(state.cwz(0), 0, 0.3); + EXPECT_NEAR(state.cvx(9), 9, 0.3); + EXPECT_NEAR(state.cvy(9), 9, 0.3); + EXPECT_NEAR(state.cwz(9), 9, 0.3); + + generator.shutdown(); +} diff --git a/test/optimizer_test.cpp b/test/optimizer_test.cpp index e9def50a..271772bd 100644 --- a/test/optimizer_test.cpp +++ b/test/optimizer_test.cpp @@ -40,6 +40,8 @@ class RosLockGuard }; RosLockGuard g_rclcpp; +// Smoke tests the optimizer + class OptimizerSuite : public ::testing::TestWithParam, bool>> {}; diff --git a/test/parameter_handler_test.cpp b/test/parameter_handler_test.cpp new file mode 100644 index 00000000..0c4c81bf --- /dev/null +++ b/test/parameter_handler_test.cpp @@ -0,0 +1,155 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "mppic/tools/parameters_handler.hpp" + +// Tests parameter handler object + +class RosLockGuard +{ +public: + RosLockGuard() {rclcpp::init(0, nullptr);} + ~RosLockGuard() {rclcpp::shutdown();} +}; + +RosLockGuard g_rclcpp; + +using namespace mppi; // NOLINT + +class ParametersHandlerWrapper : public ParametersHandler +{ +public: + ParametersHandlerWrapper() = default; + + template + auto asWrapped(rclcpp::Parameter parameter) + { + return ParametersHandler::as(parameter); + } +}; + +using namespace mppi; // NOLINT + +TEST(ParameterHandlerTest, asTypeConversionTest) +{ + ParametersHandlerWrapper a; + rclcpp::Parameter int_p("int_parameter", rclcpp::ParameterValue(1)); + rclcpp::Parameter double_p("double_parameter", rclcpp::ParameterValue(10.0)); + rclcpp::Parameter bool_p("bool_parameter", rclcpp::ParameterValue(false)); + rclcpp::Parameter string_p("string_parameter", rclcpp::ParameterValue(std::string("hello"))); + + rclcpp::Parameter intv_p("intv_parameter", rclcpp::ParameterValue(std::vector{1})); + rclcpp::Parameter doublev_p( + "doublev_parameter", rclcpp::ParameterValue(std::vector{10.0})); + rclcpp::Parameter boolv_p("boolv_parameter", rclcpp::ParameterValue(std::vector{false})); + rclcpp::Parameter stringv_p( + "stringv_parameter", rclcpp::ParameterValue(std::vector{std::string("hello")})); + + EXPECT_EQ(a.asWrapped(int_p), 1); + EXPECT_EQ(a.asWrapped(double_p), 10.0); + EXPECT_EQ(a.asWrapped(bool_p), false); + EXPECT_EQ(a.asWrapped(string_p), std::string("hello")); + + EXPECT_EQ(a.asWrapped>(intv_p)[0], 1); + EXPECT_EQ(a.asWrapped>(doublev_p)[0], 10.0); + EXPECT_EQ(a.asWrapped>(boolv_p)[0], false); + EXPECT_EQ(a.asWrapped>(stringv_p)[0], std::string("hello")); +} + +TEST(ParameterHandlerTest, PrePostDynamicCallbackTest) +{ + bool pre_triggered = false, post_triggered = false, dynamic_triggered = false; + auto preCb = [&]() { + if (post_triggered) { + throw std::runtime_error("Post-callback triggered before pre-callback!"); + } + pre_triggered = true; + }; + + auto postCb = [&]() { + if (!pre_triggered) { + throw std::runtime_error("Pre-callback was not triggered before post-callback!"); + } + post_triggered = true; + }; + + auto dynamicCb = [&](const rclcpp::Parameter & /*param*/) { + dynamic_triggered = true; + }; + + rclcpp::Parameter random_param("blah_blah", rclcpp::ParameterValue(true)); + rclcpp::Parameter random_param2("use_sim_time", rclcpp::ParameterValue(true)); + bool val = false; + + ParametersHandlerWrapper a; + a.addPreCallback(preCb); + a.addPostCallback(postCb); + a.addDynamicParamCallback("use_sim_time", dynamicCb); + a.setDynamicParamCallback(val, "blah_blah"); + + // Dynamic callback should not trigger, wrong parameter, but val should be updated + a.dynamicParamsCallback(std::vector{random_param}); + EXPECT_FALSE(dynamic_triggered); + EXPECT_TRUE(pre_triggered); + EXPECT_TRUE(post_triggered); + EXPECT_TRUE(val); + + // Now dynamic parameter bool should be updated, right param called! + pre_triggered = false, post_triggered = false; + a.dynamicParamsCallback(std::vector{random_param2}); + EXPECT_TRUE(dynamic_triggered); + EXPECT_TRUE(pre_triggered); + EXPECT_TRUE(post_triggered); +} + +TEST(ParameterHandlerTest, GetSystemParamsTest) +{ + auto node = std::make_shared("my_node"); + node->declare_parameter("param1", rclcpp::ParameterValue(true)); + node->declare_parameter("ns.param2", rclcpp::ParameterValue(7)); + + // Get parameters in global namespace and in subnamespaces + ParametersHandler handler(node); + auto getParamer = handler.getParamGetter(""); + bool p1 = false; + int p2 = 0; + getParamer(p1, "param1", false); + getParamer(p2, "ns.param2", 0); + EXPECT_EQ(p1, true); + EXPECT_EQ(p2, 7); + + // Get parameters in subnamespaces using name semantics of getter + auto getParamer2 = handler.getParamGetter("ns"); + p2 = 0; + getParamer2(p2, "param2", 0); + EXPECT_EQ(p2, 7); +} + +TEST(ParameterHandlerTest, DynamicParametersTest) +{ + auto node = std::make_shared("my_node"); + node->declare_parameter("param1", rclcpp::ParameterValue(true)); + node->declare_parameter("ns.param2", rclcpp::ParameterValue(7)); + ParametersHandler handler(node); + handler.start(); + + // Change parameters from external client, see they update properly + + // Set some parameters as static, should not be updated +} diff --git a/test/utils/factory.hpp b/test/utils/factory.hpp index bb0d6eb8..7d86c61f 100644 --- a/test/utils/factory.hpp +++ b/test/utils/factory.hpp @@ -95,6 +95,7 @@ geometry_msgs::msg::Point getDummyPoint(double x, double y) return point; } + std::shared_ptr getDummyCostmapRos() { auto costmap_ros = std::make_shared("cost_map_node"); @@ -168,7 +169,9 @@ mppi::PathHandler getDummyPathHandler( return path_handler; } -std::shared_ptr getDummyController(auto node, auto tf_buffer, auto costmap_ros) +std::shared_ptr getDummyController( + auto node, auto tf_buffer, + auto costmap_ros) { std::shared_ptr controller = std::make_shared(); std::weak_ptr weak_ptr_node{node}; From 178c0db0b16de22756849d5a976ed52f3a95c408 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 1 Dec 2022 17:23:16 -0800 Subject: [PATCH 27/53] complete parameter dynamic handler tests --- test/parameter_handler_test.cpp | 38 +++++++++++++++++++++++++++------ 1 file changed, 32 insertions(+), 6 deletions(-) diff --git a/test/parameter_handler_test.cpp b/test/parameter_handler_test.cpp index 0c4c81bf..43557230 100644 --- a/test/parameter_handler_test.cpp +++ b/test/parameter_handler_test.cpp @@ -37,6 +37,10 @@ class ParametersHandlerWrapper : public ParametersHandler public: ParametersHandlerWrapper() = default; + explicit ParametersHandlerWrapper( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent) + : ParametersHandler(parent) {} + template auto asWrapped(rclcpp::Parameter parameter) { @@ -141,15 +145,37 @@ TEST(ParameterHandlerTest, GetSystemParamsTest) EXPECT_EQ(p2, 7); } -TEST(ParameterHandlerTest, DynamicParametersTest) +TEST(ParameterHandlerTest, DynamicAndStaticParametersTest) { auto node = std::make_shared("my_node"); - node->declare_parameter("param1", rclcpp::ParameterValue(true)); - node->declare_parameter("ns.param2", rclcpp::ParameterValue(7)); - ParametersHandler handler(node); + node->declare_parameter("dynamic_int", rclcpp::ParameterValue(7)); + node->declare_parameter("static_int", rclcpp::ParameterValue(7)); + ParametersHandlerWrapper handler(node); handler.start(); - // Change parameters from external client, see they update properly + // Get parameters and check they have initial values + auto getParamer = handler.getParamGetter(""); + int p1 = 0, p2 = 0; + getParamer(p1, "dynamic_int", 0, ParameterType::Dynamic); + getParamer(p2, "static_int", 0, ParameterType::Static); + EXPECT_EQ(p1, 7); + EXPECT_EQ(p2, 7); + + // Now change them both via dynamic parameters + auto rec_param = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("dynamic_int", 10), + rclcpp::Parameter("static_int", 10)}); - // Set some parameters as static, should not be updated + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results); + + // Now, only param1 should change, param 2 should be the same + EXPECT_EQ(p1, 10); + EXPECT_EQ(p2, 7); } From c1eadc9b868a9a3a78314dbcfcc18ba16ba4fe29 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 2 Dec 2022 16:33:53 -0800 Subject: [PATCH 28/53] adding motion model tests + fix to ackermann --- include/mppic/motion_models.hpp | 4 +- test/CMakeLists.txt | 1 + test/motion_model_tests.cpp | 179 ++++++++++++++++++++++++++++++++ 3 files changed, 182 insertions(+), 2 deletions(-) create mode 100644 test/motion_model_tests.cpp diff --git a/include/mppic/motion_models.hpp b/include/mppic/motion_models.hpp index 467c6c4b..7efdc90e 100644 --- a/include/mppic/motion_models.hpp +++ b/include/mppic/motion_models.hpp @@ -112,8 +112,8 @@ class AckermannMotionModel : public MotionModel auto & vx = control_sequence.vx; auto & wz = control_sequence.wz; - auto view = xt::masked_view(wz, vx / wz > min_turning_r_); - view = xt::sign(vx) / min_turning_r_; + auto view = xt::masked_view(wz, xt::fabs(vx) / xt::fabs(wz) > min_turning_r_); + view = xt::sign(wz) * vx / min_turning_r_; } /** diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index abf71ddf..2c2076ee 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -4,6 +4,7 @@ set(TEST_NAMES models_test noise_generator_test parameter_handler_test + motion_model_tests ) foreach(name IN LISTS TEST_NAMES) diff --git a/test/motion_model_tests.cpp b/test/motion_model_tests.cpp new file mode 100644 index 00000000..37093fab --- /dev/null +++ b/test/motion_model_tests.cpp @@ -0,0 +1,179 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "mppic/motion_models.hpp" +#include "mppic/models/state.hpp" +#include "mppic/models/control_sequence.hpp" + +// Tests noise generator object + +class RosLockGuard +{ +public: + RosLockGuard() {rclcpp::init(0, nullptr);} + ~RosLockGuard() {rclcpp::shutdown();} +}; +RosLockGuard g_rclcpp; + +using namespace mppi; // NOLINT + +TEST(MotionModelTests, DiffDriveTest) +{ + models::ControlSequence control_sequence; + models::State state; + int batches = 1000; + int timesteps = 50; + control_sequence.reset(timesteps); // populates with zeros + state.reset(batches, timesteps); // populates with zeros + std::unique_ptr model = + std::make_unique(); + + // Check that predict properly populates the trajectory velocities with the control velocities + state.cvx = 10 * xt::ones({batches, timesteps}); + state.cvy = 5 * xt::ones({batches, timesteps}); + state.cwz = 1 * xt::ones({batches, timesteps}); + + // Manually set state index 0 from initial conditions which would be the speed of the robot + xt::view(state.vx, xt::all(), 0) = 10; + xt::view(state.wz, xt::all(), 0) = 1; + + model->predict(state); + + EXPECT_EQ(state.vx, state.cvx); + EXPECT_EQ(state.vy, xt::zeros({batches, timesteps})); // non-holonomic + EXPECT_EQ(state.wz, state.cwz); + + // Check that application of constraints are empty for Diff Drive + for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) { + control_sequence.vx(i) = i * i * i; + control_sequence.wz(i) = i * i * i; + } + + models::ControlSequence initial_control_sequence = control_sequence; + model->applyConstraints(control_sequence); + EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); + EXPECT_EQ(initial_control_sequence.vy, control_sequence.vy); + EXPECT_EQ(initial_control_sequence.wz, control_sequence.wz); + + // Check that Diff Drive is properly non-holonomic + EXPECT_EQ(model->isHolonomic(), false); + + // Check it cleanly destructs + model.reset(); +} + +TEST(MotionModelTests, OmniTest) +{ + models::ControlSequence control_sequence; + models::State state; + int batches = 1000; + int timesteps = 50; + control_sequence.reset(timesteps); // populates with zeros + state.reset(batches, timesteps); // populates with zeros + std::unique_ptr model = + std::make_unique(); + + // Check that predict properly populates the trajectory velocities with the control velocities + state.cvx = 10 * xt::ones({batches, timesteps}); + state.cvy = 5 * xt::ones({batches, timesteps}); + state.cwz = 1 * xt::ones({batches, timesteps}); + + // Manually set state index 0 from initial conditions which would be the speed of the robot + xt::view(state.vx, xt::all(), 0) = 10; + xt::view(state.vy, xt::all(), 0) = 5; + xt::view(state.wz, xt::all(), 0) = 1; + + model->predict(state); + + EXPECT_EQ(state.vx, state.cvx); + EXPECT_EQ(state.vy, state.cvy); // holonomic + EXPECT_EQ(state.wz, state.cwz); + + // Check that application of constraints are empty for Omni Drive + for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) { + control_sequence.vx(i) = i * i * i; + control_sequence.vy(i) = i * i * i; + control_sequence.wz(i) = i * i * i; + } + + models::ControlSequence initial_control_sequence = control_sequence; + model->applyConstraints(control_sequence); + EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); + EXPECT_EQ(initial_control_sequence.vy, control_sequence.vy); + EXPECT_EQ(initial_control_sequence.wz, control_sequence.wz); + + // Check that Omni Drive is properly holonomic + EXPECT_EQ(model->isHolonomic(), true); + + // Check it cleanly destructs + model.reset(); +} + +TEST(MotionModelTests, AckermannTest) +{ + models::ControlSequence control_sequence; + models::State state; + int batches = 1000; + int timesteps = 50; + control_sequence.reset(timesteps); // populates with zeros + state.reset(batches, timesteps); // populates with zeros + auto node = std::make_shared("my_node"); + ParametersHandler param_handler(node); + std::unique_ptr model = + std::make_unique(¶m_handler); + + // Check that predict properly populates the trajectory velocities with the control velocities + state.cvx = 10 * xt::ones({batches, timesteps}); + state.cvy = 5 * xt::ones({batches, timesteps}); + state.cwz = 1 * xt::ones({batches, timesteps}); + + // Manually set state index 0 from initial conditions which would be the speed of the robot + xt::view(state.vx, xt::all(), 0) = 10; + xt::view(state.wz, xt::all(), 0) = 1; + + model->predict(state); + + EXPECT_EQ(state.vx, state.cvx); + EXPECT_EQ(state.vy, xt::zeros({batches, timesteps})); // non-holonomic + EXPECT_EQ(state.wz, state.cwz); + + // Check that application of constraints are non-empty for Ackermann Drive + for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) { + control_sequence.vx(i) = i * i * i; + control_sequence.wz(i) = i * i * i; + } + + models::ControlSequence initial_control_sequence = control_sequence; + model->applyConstraints(control_sequence); + // VX equal since this doesn't change, the WZ is reduced if breaking the constraint + EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); + EXPECT_NE(initial_control_sequence.wz, control_sequence.wz); + + // Now, check the specifics of the minimum curvature constraint TODO(SM) + EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6); + for (unsigned int i = 1; i != control_sequence.vx.shape(0); i++) { + EXPECT_TRUE(fabs(control_sequence.vx(i)) / fabs(control_sequence.wz(i)) <= 0.2); + } + + // Check that Ackermann Drive is properly non-holonomic and parameterized + EXPECT_EQ(model->isHolonomic(), false); + + // Check it cleanly destructs + model.reset(); +} From 6076a3f943cfda7a8112d158db705b9679a11b70 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Mon, 5 Dec 2022 16:24:36 -0800 Subject: [PATCH 29/53] adding unit testing for trajectory visualization and partial coverage for utils --- include/mppic/tools/trajectory_visualizer.hpp | 2 +- include/mppic/tools/utils.hpp | 84 +++++++- src/trajectory_visualizer.cpp | 75 +------ test/CMakeLists.txt | 2 + test/trajectory_visualizer_tests.cpp | 155 ++++++++++++++ test/utils_test.cpp | 198 ++++++++++++++++++ 6 files changed, 450 insertions(+), 66 deletions(-) create mode 100644 test/trajectory_visualizer_tests.cpp create mode 100644 test/utils_test.cpp diff --git a/include/mppic/tools/trajectory_visualizer.hpp b/include/mppic/tools/trajectory_visualizer.hpp index fc6d5fb4..9e1c0ea4 100644 --- a/include/mppic/tools/trajectory_visualizer.hpp +++ b/include/mppic/tools/trajectory_visualizer.hpp @@ -23,9 +23,9 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "visualization_msgs/msg/marker_array.hpp" #include "mppic/tools/parameters_handler.hpp" +#include "mppic/tools/utils.hpp" #include "mppic/models/trajectories.hpp" namespace mppi diff --git a/include/mppic/tools/utils.hpp b/include/mppic/tools/utils.hpp index 2d4c15d9..9101a36a 100644 --- a/include/mppic/tools/utils.hpp +++ b/include/mppic/tools/utils.hpp @@ -32,6 +32,7 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/path.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -49,6 +50,88 @@ namespace mppi::utils { using xt::evaluation_strategy::immediate; +/** + * @brief Convert data into pose + * @param x X position + * @param y Y position + * @param z Z position + * @return Pose object + */ +inline geometry_msgs::msg::Pose createPose(double x, double y, double z) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.position.z = z; + pose.orientation.w = 1; + pose.orientation.x = 0; + pose.orientation.y = 0; + pose.orientation.z = 0; + return pose; +} + +/** + * @brief Convert data into scale + * @param x X scale + * @param y Y scale + * @param z Z scale + * @return Scale object + */ +inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z) +{ + geometry_msgs::msg::Vector3 scale; + scale.x = x; + scale.y = y; + scale.z = z; + return scale; +} + +/** + * @brief Convert data into color + * @param r Red component + * @param g Green component + * @param b Blue component + * @param a Alpha component (transparency) + * @return Color object + */ +inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a) +{ + std_msgs::msg::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +/** + * @brief Convert data into a Maarker + * @param id Marker ID + * @param pose Marker pose + * @param scale Marker scale + * @param color Marker color + * @param frame Reference frame to use + * @return Visualization Marker + */ +inline visualization_msgs::msg::Marker createMarker( + int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale, + const std_msgs::msg::ColorRGBA & color, const std::string & frame_id) +{ + using visualization_msgs::msg::Marker; + Marker marker; + marker.header.frame_id = frame_id; + marker.header.stamp = rclcpp::Time(0, 0); + marker.ns = "MarkerNS"; + marker.id = id; + marker.type = Marker::SPHERE; + marker.action = Marker::ADD; + + marker.pose = pose; + marker.scale = scale; + marker.color = color; + return marker; +} + /** * @brief Convert data into TwistStamped * @param vx X velocity @@ -238,7 +321,6 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) return max_id_by_trajectories; } - /** * @brief evaluate path furthest point if it is not set * @param data Data to use diff --git a/src/trajectory_visualizer.cpp b/src/trajectory_visualizer.cpp index cc20af87..8f2ebfd6 100644 --- a/src/trajectory_visualizer.cpp +++ b/src/trajectory_visualizer.cpp @@ -18,62 +18,6 @@ namespace mppi { -namespace -{ - -inline geometry_msgs::msg::Pose createPose(double x, double y, double z) -{ - geometry_msgs::msg::Pose pose; - pose.position.x = x; - pose.position.y = y; - pose.position.z = z; - pose.orientation.w = 1; - pose.orientation.x = 0; - pose.orientation.y = 0; - pose.orientation.z = 0; - return pose; -} - -inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z) -{ - geometry_msgs::msg::Vector3 scale; - scale.x = x; - scale.y = y; - scale.z = z; - return scale; -} - -inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a) -{ - std_msgs::msg::ColorRGBA color; - color.r = r; - color.g = g; - color.b = b; - color.a = a; - return color; -} - -inline visualization_msgs::msg::Marker createMarker( - int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale, - const std_msgs::msg::ColorRGBA & color, const std::string & frame_id) -{ - using visualization_msgs::msg::Marker; - Marker marker; - marker.header.frame_id = frame_id; - marker.header.stamp = rclcpp::Time(0, 0); - marker.ns = "MarkerNS"; - marker.id = id; - marker.type = Marker::SPHERE; - marker.action = Marker::ADD; - - marker.pose = pose; - marker.scale = scale; - marker.color = color; - return marker; -} - -} // namespace - void TrajectoryVisualizer::on_configure( rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name, const std::string & frame_id, ParametersHandler * parameters_handler) @@ -122,10 +66,13 @@ void TrajectoryVisualizer::add(const xt::xtensor & trajectory) auto add_marker = [&](auto i) { float component = static_cast(i) / static_cast(size); - auto pose = createPose(trajectory(i, 0), trajectory(i, 1), 0.06); - auto scale = i != size - 1 ? createScale(0.03, 0.03, 0.07) : createScale(0.07, 0.07, 0.09); - auto color = createColor(0, component, component, 1); - auto marker = createMarker(marker_id_++, pose, scale, color, frame_id_); + auto pose = utils::createPose(trajectory(i, 0), trajectory(i, 1), 0.06); + auto scale = + i != size - 1 ? + utils::createScale(0.03, 0.03, 0.07) : + utils::createScale(0.07, 0.07, 0.09); + auto color = utils::createColor(0, component, component, 1); + auto marker = utils::createMarker(marker_id_++, pose, scale, color, frame_id_); points_->markers.push_back(marker); }; @@ -147,10 +94,10 @@ void TrajectoryVisualizer::add( float blue_component = 1.0f - j_flt / shape_1; float green_component = j_flt / shape_1; - auto pose = createPose(trajectories.x(i, j), trajectories.y(i, j), 0.03); - auto scale = createScale(0.03, 0.03, 0.03); - auto color = createColor(0, green_component, blue_component, 1); - auto marker = createMarker(marker_id_++, pose, scale, color, frame_id_); + auto pose = utils::createPose(trajectories.x(i, j), trajectories.y(i, j), 0.03); + auto scale = utils::createScale(0.03, 0.03, 0.03); + auto color = utils::createColor(0, green_component, blue_component, 1); + auto marker = utils::createMarker(marker_id_++, pose, scale, color, frame_id_); points_->markers.push_back(marker); } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 2c2076ee..4e2ba1e7 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -5,6 +5,8 @@ set(TEST_NAMES noise_generator_test parameter_handler_test motion_model_tests + trajectory_visualizer_tests + utils_test ) foreach(name IN LISTS TEST_NAMES) diff --git a/test/trajectory_visualizer_tests.cpp b/test/trajectory_visualizer_tests.cpp new file mode 100644 index 00000000..c19286d7 --- /dev/null +++ b/test/trajectory_visualizer_tests.cpp @@ -0,0 +1,155 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "mppic/tools/trajectory_visualizer.hpp" + +// Tests noise generator object + +class RosLockGuard +{ +public: + RosLockGuard() {rclcpp::init(0, nullptr);} + ~RosLockGuard() {rclcpp::shutdown();} +}; +RosLockGuard g_rclcpp; + +using namespace mppi; // NOLINT + +TEST(TrajectoryVisualizerTests, StateTransition) +{ + auto node = std::make_shared("my_node"); + auto parameters_handler = std::make_unique(node); + + TrajectoryVisualizer vis; + vis.on_configure(node, "my_name", "map", parameters_handler.get()); + vis.on_activate(); + vis.on_deactivate(); + vis.on_cleanup(); +} + +TEST(TrajectoryVisualizerTests, VisPathRepub) +{ + auto node = std::make_shared("my_node"); + auto parameters_handler = std::make_unique(node); + nav_msgs::msg::Path recieved_path; + nav_msgs::msg::Path pub_path; + pub_path.header.frame_id = "fake_frame"; + pub_path.poses.resize(5); + + auto my_sub = node->create_subscription( + "transformed_global_plan", 10, + [&](const nav_msgs::msg::Path msg) {recieved_path = msg;}); + + TrajectoryVisualizer vis; + vis.on_configure(node, "my_name", "map", parameters_handler.get()); + vis.on_activate(); + vis.visualize(pub_path); + + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_EQ(recieved_path.poses.size(), 5u); + EXPECT_EQ(recieved_path.header.frame_id, "fake_frame"); +} + +TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) +{ + auto node = std::make_shared("my_node"); + auto parameters_handler = std::make_unique(node); + + visualization_msgs::msg::MarkerArray recieved_msg; + auto my_sub = node->create_subscription( + "/trajectories", 10, + [&](const visualization_msgs::msg::MarkerArray msg) {recieved_msg = msg;}); + + // optimal_trajectory empty, should fail to publish + xt::xtensor optimal_trajectory; + TrajectoryVisualizer vis; + vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); + vis.on_activate(); + vis.add(optimal_trajectory); + nav_msgs::msg::Path bogus_path; + vis.visualize(bogus_path); + + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_EQ(recieved_msg.markers.size(), 0u); + + // Now populated with content, should publish + optimal_trajectory = xt::ones({20, 2}); + vis.add(optimal_trajectory); + vis.visualize(bogus_path); + + rclcpp::spin_some(node->get_node_base_interface()); + + // Should have 20 trajectory points in the map frame + EXPECT_EQ(recieved_msg.markers.size(), 20u); + EXPECT_EQ(recieved_msg.markers[0].header.frame_id, "fkmap"); + + // Check IDs are properly populated + EXPECT_EQ(recieved_msg.markers[0].id, 0); + EXPECT_EQ(recieved_msg.markers[1].id, 1); + EXPECT_EQ(recieved_msg.markers[10].id, 10); + + // Check poses are correct + EXPECT_EQ(recieved_msg.markers[0].pose.position.x, 1); + EXPECT_EQ(recieved_msg.markers[0].pose.position.y, 1); + EXPECT_EQ(recieved_msg.markers[0].pose.position.z, 0.06); + + // Check that scales are rational + EXPECT_EQ(recieved_msg.markers[0].scale.x, 0.03); + EXPECT_EQ(recieved_msg.markers[0].scale.y, 0.03); + EXPECT_EQ(recieved_msg.markers[0].scale.z, 0.07); + + EXPECT_EQ(recieved_msg.markers[19].scale.x, 0.07); + EXPECT_EQ(recieved_msg.markers[19].scale.y, 0.07); + EXPECT_EQ(recieved_msg.markers[19].scale.z, 0.09); + + // Check that the colors are rational + for (unsigned int i = 0; i != recieved_msg.markers.size() - 1; i++) { + EXPECT_LT(recieved_msg.markers[i].color.g, recieved_msg.markers[i + 1].color.g); + EXPECT_LT(recieved_msg.markers[i].color.b, recieved_msg.markers[i + 1].color.b); + EXPECT_EQ(recieved_msg.markers[i].color.r, recieved_msg.markers[i + 1].color.r); + EXPECT_EQ(recieved_msg.markers[i].color.a, recieved_msg.markers[i + 1].color.a); + } +} + +TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) +{ + auto node = std::make_shared("my_node"); + auto parameters_handler = std::make_unique(node); + + visualization_msgs::msg::MarkerArray recieved_msg; + auto my_sub = node->create_subscription( + "/trajectories", 10, + [&](const visualization_msgs::msg::MarkerArray msg) {recieved_msg = msg;}); + + models::Trajectories candidate_trajectories; + candidate_trajectories.x = xt::ones({200, 12}); + candidate_trajectories.y = xt::ones({200, 12}); + candidate_trajectories.yaws = xt::ones({200, 12}); + + TrajectoryVisualizer vis; + vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); + vis.on_activate(); + vis.add(candidate_trajectories); + nav_msgs::msg::Path bogus_path; + vis.visualize(bogus_path); + + rclcpp::spin_some(node->get_node_base_interface()); + // 40 * 4, for 5 trajectory steps + 3 point steps + EXPECT_EQ(recieved_msg.markers.size(), 160u); +} diff --git a/test/utils_test.cpp b/test/utils_test.cpp new file mode 100644 index 00000000..82519a6e --- /dev/null +++ b/test/utils_test.cpp @@ -0,0 +1,198 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "mppic/tools/utils.hpp" +#include "mppic/models/path.hpp" + +// Tests noise generator object + +class RosLockGuard +{ +public: + RosLockGuard() {rclcpp::init(0, nullptr);} + ~RosLockGuard() {rclcpp::shutdown();} +}; +RosLockGuard g_rclcpp; + +using namespace mppi::utils; // NOLINT +using namespace mppi; // NOLINT + +class TestGoalChecker : public nav2_core::GoalChecker +{ +public: + TestGoalChecker() {} + + virtual void initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & /*parent*/, + const std::string & /*plugin_name*/, + const std::shared_ptr/*costmap_ros*/) {} + + virtual void reset() {} + + virtual bool isGoalReached( + const geometry_msgs::msg::Pose & /*query_pose*/, + const geometry_msgs::msg::Pose & /*goal_pose*/, + const geometry_msgs::msg::Twist & /*velocity*/) {return false;} + + virtual bool getTolerances( + geometry_msgs::msg::Pose & pose_tolerance, + geometry_msgs::msg::Twist & /*vel_tolerance*/) + { + pose_tolerance.position.x = 0.25; + pose_tolerance.position.y = 0.25; + return true; + } +}; + +TEST(UtilsTests, MarkerPopulationUtils) +{ + auto pose = createPose(1.0, 2.0, 3.0); + EXPECT_EQ(pose.position.x, 1.0); + EXPECT_EQ(pose.position.y, 2.0); + EXPECT_EQ(pose.position.z, 3.0); + EXPECT_EQ(pose.orientation.w, 1.0); + + auto scale = createScale(1.0, 2.0, 3.0); + EXPECT_EQ(scale.x, 1.0); + EXPECT_EQ(scale.y, 2.0); + EXPECT_EQ(scale.z, 3.0); + + auto color = createColor(1.0, 2.0, 3.0, 0.0); + EXPECT_EQ(color.r, 1.0); + EXPECT_EQ(color.g, 2.0); + EXPECT_EQ(color.b, 3.0); + EXPECT_EQ(color.a, 0.0); + + auto marker = createMarker(999, pose, scale, color, "map"); + EXPECT_EQ(marker.header.frame_id, "map"); + EXPECT_EQ(marker.id, 999); + EXPECT_EQ(marker.pose, pose); + EXPECT_EQ(marker.scale, scale); + EXPECT_EQ(marker.color, color); +} + +TEST(UtilsTests, ConversionTests) +{ + geometry_msgs::msg::TwistStamped output; + builtin_interfaces::msg::Time time; + + // Check population is correct + output = toTwistStamped(0.5, 0.3, time, "map"); + EXPECT_NEAR(output.twist.linear.x, 0.5, 1e-6); + EXPECT_NEAR(output.twist.linear.y, 0.0, 1e-6); + EXPECT_NEAR(output.twist.angular.z, 0.3, 1e-6); + EXPECT_EQ(output.header.frame_id, "map"); + EXPECT_EQ(output.header.stamp, time); + + output = toTwistStamped(0.5, 0.4, 0.3, time, "map"); + EXPECT_NEAR(output.twist.linear.x, 0.5, 1e-6); + EXPECT_NEAR(output.twist.linear.y, 0.4, 1e-6); + EXPECT_NEAR(output.twist.angular.z, 0.3, 1e-6); + EXPECT_EQ(output.header.frame_id, "map"); + EXPECT_EQ(output.header.stamp, time); + + nav_msgs::msg::Path path; + path.poses.resize(5); + path.poses[2].pose.position.x = 5; + path.poses[2].pose.position.y = 50; + models::Path path_t = toTensor(path); + + // Check population is correct + EXPECT_EQ(path_t.x.shape(0), 5u); + EXPECT_EQ(path_t.y.shape(0), 5u); + EXPECT_EQ(path_t.yaws.shape(0), 5u); + EXPECT_EQ(path_t.x(2), 5); + EXPECT_EQ(path_t.y(2), 50); + EXPECT_NEAR(path_t.yaws(2), 0.0, 1e-6); +} + +TEST(UtilsTests, WithTolTests) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = 10.0; + pose.position.y = 1.0; + + nav2_core::GoalChecker * goal_checker = new TestGoalChecker; + + // Test not in tolerance + nav_msgs::msg::Path path; + path.poses.resize(2); + 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)); + + // 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)); + + 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)); + + 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)); + + delete goal_checker; + goal_checker = nullptr; + EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, path_t)); +} + +TEST(UtilsTests, AnglesTests) +{ + // Test angle normalization by creating insane angles + xt::xtensor angles, zero_angles; + angles = xt::ones({100}); + for (unsigned int i = 0; i != angles.shape(0); i++) { + angles(i) = i * i; + if (i % 2 == 0) { + angles(i) *= -1; + } + } + + auto norm_ang = normalize_angles(angles); + for (unsigned int i = 0; i != norm_ang.shape(0); i++) { + EXPECT_TRUE((norm_ang(i) >= -M_PI) && (norm_ang(i) <= M_PI)); + } + + // Test shortest angular distance + zero_angles = xt::zeros({100}); + auto ang_dist = shortest_angular_distance(angles, zero_angles); + for (unsigned int i = 0; i != ang_dist.shape(0); i++) { + EXPECT_TRUE((ang_dist(i) >= -M_PI) && (ang_dist(i) <= M_PI)); + } + + // Test point-pose angle + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + pose.orientation.w = 1.0; + double point_x = 1.0, point_y = 0.0; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y), 0.0, 1e-6); +} + From 36da4c8042f1325665f5f6c1894087c6d1978417 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 6 Dec 2022 15:38:04 -0800 Subject: [PATCH 30/53] Fixing comparison for finding path point closest to final trajectories --- include/mppic/tools/utils.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/mppic/tools/utils.hpp b/include/mppic/tools/utils.hpp index fe84df30..ec0a5619 100644 --- a/include/mppic/tools/utils.hpp +++ b/include/mppic/tools/utils.hpp @@ -175,7 +175,7 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) for (size_t i = 0; i < dists.shape(0); i++) { size_t min_id_by_path = 0; for (size_t j = 0; j < dists.shape(1); j++) { - if (min_distance_by_path < dists(i, j)) { + if (min_distance_by_path > dists(i, j)) { min_distance_by_path = dists(i, j); min_id_by_path = j; } From 0740ec9eedb089604b423675fa8dad8c96954a30 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Tue, 6 Dec 2022 17:11:07 -0800 Subject: [PATCH 31/53] adding more tests --- test/CMakeLists.txt | 1 + test/motion_model_tests.cpp | 2 +- test/path_handler_test.cpp | 170 +++++++++++++++++++++++++++ test/trajectory_visualizer_tests.cpp | 2 +- test/utils_test.cpp | 98 +++++++++++++++ 5 files changed, 271 insertions(+), 2 deletions(-) create mode 100644 test/path_handler_test.cpp diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 4e2ba1e7..5e34c5ea 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -7,6 +7,7 @@ set(TEST_NAMES motion_model_tests trajectory_visualizer_tests utils_test + path_handler_test ) foreach(name IN LISTS TEST_NAMES) diff --git a/test/motion_model_tests.cpp b/test/motion_model_tests.cpp index 37093fab..d76b3a58 100644 --- a/test/motion_model_tests.cpp +++ b/test/motion_model_tests.cpp @@ -21,7 +21,7 @@ #include "mppic/models/state.hpp" #include "mppic/models/control_sequence.hpp" -// Tests noise generator object +// Tests motion models class RosLockGuard { diff --git a/test/path_handler_test.cpp b/test/path_handler_test.cpp new file mode 100644 index 00000000..b502737a --- /dev/null +++ b/test/path_handler_test.cpp @@ -0,0 +1,170 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "mppic/tools/path_handler.hpp" + +// Tests path handling + +class RosLockGuard +{ +public: + RosLockGuard() {rclcpp::init(0, nullptr);} + ~RosLockGuard() {rclcpp::shutdown();} +}; +RosLockGuard g_rclcpp; + +using namespace mppi; // NOLINT + +class PathHandlerWrapper : public PathHandler +{ +public: + PathHandlerWrapper() + : PathHandler() {} + + void pruneGlobalPlanWrapper(const PathIterator end) + { + return pruneGlobalPlan(end); + } + + double getMaxCostmapDistWrapper() + { + return getMaxCostmapDist(); + } + + PathRange getGlobalPlanConsideringBoundsWrapper(const geometry_msgs::msg::PoseStamped & pose) + { + return getGlobalPlanConsideringBounds(pose); + } + + bool transformPoseWrapper( + const std::string & frame, const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const + { + return transformPose(frame, in_pose, out_pose); + } + + nav_msgs::msg::Path transformPlanPosesToCostmapFrameWrapper( + PathIterator begin, PathIterator end, const builtin_interfaces::msg::Time & stamp) + { + return transformPlanPosesToCostmapFrame(begin, end, stamp); + } + + geometry_msgs::msg::PoseStamped transformToGlobalPlanFrameWrapper( + const geometry_msgs::msg::PoseStamped & pose) + { + return transformToGlobalPlanFrame(pose); + } +}; + +TEST(PathHandlerTests, GetAndPrunePath) +{ + nav_msgs::msg::Path path; + PathHandlerWrapper handler; + + path.header.frame_id = "fkframe"; + path.poses.resize(11); + + handler.setPath(path); + auto & rtn_path = handler.getPath(); + EXPECT_EQ(path.header.frame_id, rtn_path.header.frame_id); + EXPECT_EQ(path.poses.size(), rtn_path.poses.size()); + + PathIterator it = rtn_path.poses.begin() + 5; + handler.pruneGlobalPlanWrapper(it); + auto rtn2_path = handler.getPath(); + EXPECT_EQ(rtn2_path.poses.size(), 6u); +} + +TEST(PathHandlerTests, TestBounds) +{ + PathHandlerWrapper handler; + auto node = std::make_shared("my_node"); + node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State state; + costmap_ros->on_configure(state); + + // Test initialization and getting costmap basic metadata + handler.initialize(node, "dummy", costmap_ros, costmap_ros->getTfBuffer(), ¶m_handler); + EXPECT_EQ(handler.getMaxCostmapDistWrapper(), 2.5); + + // Test getting the global plans within a bounds window + nav_msgs::msg::Path path; + path.header.frame_id = "odom"; + path.poses.resize(100); + for (unsigned int i = 0; i != path.poses.size(); i++) { + path.poses[i].pose.position.x = i; + } + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = "odom"; + robot_pose.pose.position.x = 25.0; + + handler.setPath(path); + auto [closest, furthest] = handler.getGlobalPlanConsideringBoundsWrapper(robot_pose); + auto & path_in = handler.getPath(); + EXPECT_EQ(closest - path_in.poses.begin(), 25); + EXPECT_EQ(furthest - path_in.poses.begin(), 27); + handler.pruneGlobalPlanWrapper(closest); + auto & path_pruned = handler.getPath(); + EXPECT_EQ(path_pruned.poses.size(), 75u); +} + +TEST(PathHandlerTests, TestTransforms) +{ + PathHandlerWrapper handler; + auto node = std::make_shared("my_node"); + node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State state; + costmap_ros->on_configure(state); + + // Test basic transformations and path handling + handler.initialize(node, "dummy", costmap_ros, costmap_ros->getTfBuffer(), ¶m_handler); + + nav_msgs::msg::Path path; + path.header.frame_id = "map"; + path.poses.resize(100); + for (unsigned int i = 0; i != path.poses.size(); i++) { + path.poses[i].pose.position.x = i; + } + + geometry_msgs::msg::PoseStamped robot_pose, output_pose; + robot_pose.header.frame_id = "map"; + robot_pose.pose.position.x = 25.0; + + EXPECT_TRUE(handler.transformPoseWrapper("map", robot_pose, output_pose)); + EXPECT_EQ(output_pose.pose.position.x, 25.0); + + EXPECT_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose), std::runtime_error); + handler.setPath(path); + EXPECT_NO_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose)); + + auto [closest, furthest] = handler.getGlobalPlanConsideringBoundsWrapper(robot_pose); + + builtin_interfaces::msg::Time stamp; + auto path_out = handler.transformPlanPosesToCostmapFrameWrapper(closest, furthest, stamp); + + // Put it all together + auto final_path = handler.transformPath(robot_pose); + EXPECT_EQ(final_path.poses.size(), path_out.poses.size()); +} diff --git a/test/trajectory_visualizer_tests.cpp b/test/trajectory_visualizer_tests.cpp index c19286d7..22dfbfa5 100644 --- a/test/trajectory_visualizer_tests.cpp +++ b/test/trajectory_visualizer_tests.cpp @@ -19,7 +19,7 @@ #include "rclcpp/rclcpp.hpp" #include "mppic/tools/trajectory_visualizer.hpp" -// Tests noise generator object +// Tests trajectory visualization class RosLockGuard { diff --git a/test/utils_test.cpp b/test/utils_test.cpp index 82519a6e..86da88ce 100644 --- a/test/utils_test.cpp +++ b/test/utils_test.cpp @@ -15,6 +15,7 @@ #include #include +#include #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "mppic/tools/utils.hpp" @@ -196,3 +197,100 @@ TEST(UtilsTests, AnglesTests) EXPECT_NEAR(posePointAngle(pose, point_x, point_y), 0.0, 1e-6); } +TEST(UtilsTests, FurthestReachedPoint) +{ + models::State state; + models::Trajectories generated_trajectories; + models::Path path; + xt::xtensor costs; + float model_dt = 0.1; + + CriticData data = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + std::nullopt}; /// Caution, keep references + + // Attempt to set furthest point if notionally set, should not change + data.furthest_reached_path_point = 99999; + setPathFurthestPointIfNotSet(data); + EXPECT_EQ(data.furthest_reached_path_point, 99999); + + // 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, + std::nullopt}; /// Caution, keep references + setPathFurthestPointIfNotSet(data2); + EXPECT_EQ(data2.furthest_reached_path_point, 0); + + // Test the actual computation of the path point reached + generated_trajectories.x = xt::ones({100, 2}); + generated_trajectories.y = xt::zeros({100, 2}); + generated_trajectories.yaws = xt::zeros({100, 2}); + + nav_msgs::msg::Path plan; + plan.poses.resize(10); + for (unsigned int i = 0; i != plan.poses.size(); i++) { + plan.poses[i].pose.position.x = 0.2 * i; + plan.poses[i].pose.position.y = 0.0; + } + path = toTensor(plan); + + CriticData data3 = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + std::nullopt}; /// Caution, keep references + EXPECT_EQ(findPathFurthestReachedPoint(data3), 5u); + data3.furthest_reached_path_point = 5u; + EXPECT_EQ(getPathRatioReached(data3), 0.5); +} + +TEST(UtilsTests, SmootherTest) +{ + models::ControlSequence noisey_sequence, sequence_init; + noisey_sequence.vx = 0.2 * xt::ones({30}); + noisey_sequence.vy = 0.0 * xt::ones({30}); + noisey_sequence.wz = 0.3 * xt::ones({30}); + + // Make the sequence noisey + auto noises = xt::random::randn({30}, 0.0, 0.2); + noisey_sequence.vx += noises; + noisey_sequence.vy += noises; + noisey_sequence.wz += noises; + sequence_init = noisey_sequence; + + std::array history, history_init; + history[1].vx = 0.1; + history[1].vy = 0.0; + history[1].wz = 0.3; + history[0].vx = 0.0; + history[0].vy = 0.0; + history[0].wz = 0.0; + history_init = history; + + models::OptimizerSettings settings; + settings.shift_control_sequence = false; // so result stores 0th value in history + + savitskyGolayFilter(noisey_sequence, history, settings); + + // Check history is propogated backward + EXPECT_NEAR(history_init[1].vx, history[0].vx, 0.02); + EXPECT_NEAR(history_init[1].vy, history[0].vy, 0.02); + EXPECT_NEAR(history_init[1].wz, history[0].wz, 0.02); + + // Check history element is updated for first command + EXPECT_NEAR(history[1].vx, 0.2, 0.05); + EXPECT_NEAR(history[1].vy, 0.0, 0.02); + EXPECT_NEAR(history[1].wz, 0.23, 0.02); + + // Check that path is smoother + float smoothed_val, original_val; + for (unsigned int i = 0; i != noisey_sequence.vx.shape(0); i++) { + smoothed_val += fabs(noisey_sequence.vx(i) - 0.2); + smoothed_val += fabs(noisey_sequence.vy(i) - 0.0); + smoothed_val += fabs(noisey_sequence.wz(i) - 0.3); + + original_val += fabs(sequence_init.vx(i) - 0.2); + original_val += fabs(sequence_init.vy(i) - 0.0); + original_val += fabs(sequence_init.wz(i) - 0.3); + } + + EXPECT_LT(smoothed_val, original_val); +} From 462a6510ed36a0dac54afd0cc03f28418ddb3984 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 7 Dec 2022 16:47:24 -0800 Subject: [PATCH 32/53] moooooooor unit tests --- include/mppic/critic_manager.hpp | 2 +- include/mppic/critics/constraint_critic.hpp | 3 + src/critics/constraint_critic.cpp | 6 +- src/critics/goal_angle_critic.cpp | 13 +- test/CMakeLists.txt | 9 + test/critic_manager_test.cpp | 137 ++++++++++ test/critics_tests.cpp | 289 ++++++++++++++++++++ 7 files changed, 449 insertions(+), 10 deletions(-) create mode 100644 test/critic_manager_test.cpp create mode 100644 test/critics_tests.cpp diff --git a/include/mppic/critic_manager.hpp b/include/mppic/critic_manager.hpp index 0f9e66a9..c19b7945 100644 --- a/include/mppic/critic_manager.hpp +++ b/include/mppic/critic_manager.hpp @@ -73,7 +73,7 @@ class CriticManager /** * @brief Load the critic plugins */ - void loadCritics(); + virtual void loadCritics(); /** * @brief Get full-name namespaced critic IDs diff --git a/include/mppic/critics/constraint_critic.hpp b/include/mppic/critics/constraint_critic.hpp index 7f8c3972..b140a648 100644 --- a/include/mppic/critics/constraint_critic.hpp +++ b/include/mppic/critics/constraint_critic.hpp @@ -41,6 +41,9 @@ class ConstraintCritic : public CriticFunction */ void score(CriticData & data) override; + float getMaxVelConstraint() {return max_vel_;} + float getMinVelConstraint() {return min_vel_;} + protected: unsigned int power_{0}; float weight_{0}; diff --git a/src/critics/constraint_critic.cpp b/src/critics/constraint_critic.cpp index f567e248..56a609ef 100644 --- a/src/critics/constraint_critic.cpp +++ b/src/critics/constraint_critic.cpp @@ -47,8 +47,10 @@ void ConstraintCritic::score(CriticData & data) return; } - auto out_of_max_bounds_motion = xt::maximum(data.state.vx - max_vel_, 0); - auto out_of_min_bounds_motion = xt::maximum(min_vel_ - data.state.vx, 0); + auto sgn = xt::where(data.state.vx > 0.0, 1.0, -1.0); + auto vel_total = sgn * xt::sqrt(data.state.vx * data.state.vx + data.state.vy * data.state.vy); + auto out_of_max_bounds_motion = xt::maximum(vel_total - max_vel_, 0); + auto out_of_min_bounds_motion = xt::maximum(min_vel_ - vel_total, 0); auto acker = dynamic_cast(data.motion_model.get()); if (acker != nullptr) { diff --git a/src/critics/goal_angle_critic.cpp b/src/critics/goal_angle_critic.cpp index f94d0630..bdafc0ea 100644 --- a/src/critics/goal_angle_critic.cpp +++ b/src/critics/goal_angle_critic.cpp @@ -41,17 +41,16 @@ void GoalAngleCritic::score(CriticData & data) 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 float goal_x = data.path.x(goal_idx); + const float goal_y = data.path.y(goal_idx); - const auto dx = data.state.pose.pose.position.x - goal_x; - const auto dy = data.state.pose.pose.position.y - goal_y; + const float dx = data.state.pose.pose.position.x - goal_x; + const float dy = data.state.pose.pose.position.y - goal_y; - const auto dist = std::sqrt(dx * dx + dy * dy); + const float dist = std::sqrt(dx * dx + dy * dy); if (dist < threshold_to_consider_) { - const auto goal_yaw = data.path.yaws(goal_idx); - + const float goal_yaw = data.path.yaws(goal_idx); data.costs += xt::pow( xt::mean(xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) * weight_, power_); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 5e34c5ea..ebd5257f 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -8,6 +8,7 @@ set(TEST_NAMES trajectory_visualizer_tests utils_test path_handler_test + critic_manager_test ) foreach(name IN LISTS TEST_NAMES) @@ -28,3 +29,11 @@ foreach(name IN LISTS TEST_NAMES) endif() endforeach() + +# This is a special case requiring linking against the critics library +ament_add_gtest(critics_tests critics_tests.cpp) +ament_target_dependencies(critics_tests ${dependencies_pkgs}) +target_link_libraries(critics_tests mppic critics) +if(${TEST_DEBUG_INFO}) + target_compile_definitions(critics_tests PUBLIC -DTEST_DEBUG_INFO) +endif() diff --git a/test/critic_manager_test.cpp b/test/critic_manager_test.cpp new file mode 100644 index 00000000..48c1b3fc --- /dev/null +++ b/test/critic_manager_test.cpp @@ -0,0 +1,137 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "mppic/critic_manager.hpp" + +// Tests critic manager + +class RosLockGuard +{ +public: + RosLockGuard() {rclcpp::init(0, nullptr);} + ~RosLockGuard() {rclcpp::shutdown();} +}; +RosLockGuard g_rclcpp; + +using namespace mppi; // NOLINT +using namespace mppi::critics; // NOLINT + +class DummyCritic : public CriticFunction +{ +public: + virtual void initialize() {initialized_ = true;} + virtual void score(CriticData & /*data*/) {scored_ = true;} + bool initialized_{false}, scored_{false}; +}; + +class CriticManagerWrapper : public CriticManager +{ +public: + CriticManagerWrapper() + : CriticManager() {} + + virtual void loadCritics() + { + critics_.clear(); + auto instance = std::unique_ptr(new DummyCritic); + critics_.push_back(std::move(instance)); + critics_.back()->on_configure( + parent_, name_, name_ + "." + "DummyCritic", costmap_ros_, + parameters_handler_); + } + + std::string getFullNameWrapper(const std::string & name) + { + return getFullName(name); + } + + bool getDummyCriticInitialized() + { + return dynamic_cast(critics_[0].get())->initialized_; + } + + bool getDummyCriticScored() + { + return dynamic_cast(critics_[0].get())->scored_; + } +}; + +class CriticManagerWrapperEnum : public CriticManager +{ +public: + CriticManagerWrapperEnum() + : CriticManager() {} + + unsigned int getCriticNum() + { + return critics_.size(); + } +}; + +TEST(CriticManagerTests, BasicCriticOperations) +{ + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + // Configuration should get parameters and initialize critic functions + CriticManagerWrapper critic_manager; + critic_manager.on_configure(node, "critic_manager", costmap_ros, ¶m_handler); + EXPECT_TRUE(critic_manager.getDummyCriticInitialized()); + + // Evaluation of critics should score them, but only if failure flag is not set + models::State state; + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + models::Path path; + xt::xtensor costs; + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt}; + + data.fail_flag = true; + EXPECT_FALSE(critic_manager.getDummyCriticScored()); + data.fail_flag = false; + critic_manager.evalTrajectoriesScores(data); + EXPECT_TRUE(critic_manager.getDummyCriticScored()); + + // This should get the full namespaced name of the critics + EXPECT_EQ(critic_manager.getFullNameWrapper("name"), std::string("mppi::critics::name")); +} + +TEST(CriticManagerTests, CriticLoadingTest) +{ + auto node = std::make_shared("my_node"); + node->declare_parameter( + "critic_manager.critics", + rclcpp::ParameterValue(std::vector{"ConstraintCritic", "PreferForwardCritic"})); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State state; + costmap_ros->on_configure(state); + + // This should grab the critics parameter and load the 2 requested plugins + CriticManagerWrapperEnum critic_manager; + critic_manager.on_configure(node, "critic_manager", costmap_ros, ¶m_handler); + EXPECT_EQ(critic_manager.getCriticNum(), 2u); +} diff --git a/test/critics_tests.cpp b/test/critics_tests.cpp new file mode 100644 index 00000000..951f90ee --- /dev/null +++ b/test/critics_tests.cpp @@ -0,0 +1,289 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "mppic/tools/utils.hpp" +#include "mppic/motion_models.hpp" +#include "mppic/critics/constraint_critic.hpp" +#include "mppic/critics/goal_angle_critic.hpp" +#include "mppic/critics/goal_critic.hpp" +#include "mppic/critics/obstacles_critic.hpp" +#include "mppic/critics/path_align_critic.hpp" +#include "mppic/critics/path_angle_critic.hpp" +#include "mppic/critics/path_follow_critic.hpp" +#include "mppic/critics/prefer_forward_critic.hpp" +#include "mppic/critics/twirling_critic.hpp" +#include "utils_test.cpp" // NOLINT + +// Tests the various critic plugin functions + +// ROS lock used from utils_test.cpp + +using namespace mppi; // NOLINT +using namespace mppi::critics; // NOLINT +using namespace mppi::utils; // NOLINT +using xt::evaluation_strategy::immediate; + +TEST(CriticTests, ConstraintsCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + models::Path path; + 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}; + data.motion_model = std::make_shared(); + + // Initialization testing + + // Make sure initializes correctly and that defaults are reasonable + ConstraintCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + EXPECT_TRUE(critic.getMaxVelConstraint() > 0.0); + EXPECT_TRUE(critic.getMinVelConstraint() < 0.0); + + // Scoring testing + + // provide velocities in constraints, should not have any costs + state.vx = 0.40 * xt::ones({1000, 30}); + state.vy = xt::zeros({1000, 30}); + state.wz = xt::ones({1000, 30}); + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); + + // provide out of maximum velocity constraint + auto last_batch_traj_in_full = xt::view(state.vx, -1, xt::all()); + last_batch_traj_in_full = 0.60 * xt::ones({30}); + critic.score(data); + EXPECT_GT(xt::sum(costs, immediate)(), 0); + // 4.0 weight * 0.1 model_dt * 0.1 error introduced * 30 timesteps = 1.2 + EXPECT_NEAR(costs(999), 1.2, 0.01); + costs = xt::zeros({1000}); + + // provide out of minimum velocity constraint + auto first_batch_traj_in_full = xt::view(state.vx, 1, xt::all()); + first_batch_traj_in_full = -0.45 * xt::ones({30}); + critic.score(data); + EXPECT_GT(xt::sum(costs, immediate)(), 0); + // 4.0 weight * 0.1 model_dt * 0.1 error introduced * 30 timesteps = 1.2 + EXPECT_NEAR(costs(1), 1.2, 0.01); + costs = xt::zeros({1000}); + + // Now with ackermann, all in constraint so no costs to score + state.vx = 0.40 * xt::ones({1000, 30}); + state.wz = 1.5 * xt::ones({1000, 30}); + data.motion_model = std::make_shared(¶m_handler); + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); + + // Now violating the ackermann constraints + state.wz = 2.5 * xt::ones({1000, 30}); + critic.score(data); + EXPECT_GT(xt::sum(costs, immediate)(), 0); + // 4.0 weight * 0.1 model_dt * (0.2 - 0.4/2.5) * 30 timesteps = 0.48 + EXPECT_NEAR(costs(1), 0.48, 0.01); +} + +TEST(CriticTests, GoalAngleCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + 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}; + data.motion_model = std::make_shared(); + + // Initialization testing + + // Make sure initializes correctly + GoalAngleCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide state poses and path too far from `threshold_to_consider` to consider + state.pose.pose.position.x = 1.0; + path.reset(10); + path.x(9) = 10.0; + path.y(9) = 0.0; + path.yaws(9) = 3.14; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); + + // Lets move it even closer, just to be sure it still doesn't trigger + state.pose.pose.position.x = 9.2; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); + + // provide state pose and path below `threshold_to_consider` to consider + state.pose.pose.position.x = 9.7; + critic.score(data); + EXPECT_GT(xt::sum(costs, immediate)(), 0); + EXPECT_NEAR(costs(0), 9.42, 0.02); // (3.14 - 0.0) * 3.0 weight +} + +TEST(CriticTests, GoalCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + 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}; + data.motion_model = std::make_shared(); + + // Initialization testing + + // Make sure initializes correctly + GoalCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing with all trajectories set to 0 + + // provide state poses and path far + state.pose.pose.position.x = 1.0; + path.reset(10); + path.x(9) = 10.0; + path.y(9) = 0.0; + critic.score(data); + EXPECT_NEAR(costs(2), 50.0, 1e-6); // (sqrt(10.0 * 10.0) * 5.0 weight + EXPECT_NEAR(xt::sum(costs, immediate)(), 50000.0, 1e-6); // Should all be 50 * 1000 + costs = xt::zeros({1000}); + + // provide state pose and path close + path.x(9) = 0.5; + path.y(9) = 0.0; + 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 +} + +TEST(CriticTests, ObstaclesCritic) +{ +} + +TEST(CriticTests, PathAlignCritic) +{ +} + +TEST(CriticTests, PathAngleCritic) +{ +} + +TEST(CriticTests, PathFollowCritic) +{ +} + +TEST(CriticTests, PreferForwardCritic) +{ +} + +TEST(CriticTests, TwirlingCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + state.reset(1000, 30); + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + 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}; + data.motion_model = std::make_shared(); + TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally + data.goal_checker = &goal_checker; + + // Initialization testing + + // Make sure initializes correctly + TwirlingCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide state poses and path far away, not within positional tolerances + state.pose.pose.position.x = 1.0; + path.reset(10); + path.x(9) = 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; + state.wz = xt::zeros({1000, 30}); + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // Provide nearby with some motion + auto traj_view = xt::view(state.wz, 0, xt::all()); + traj_view = 10.0; + critic.score(data); + EXPECT_NEAR(costs(0), 100.0, 1e-6); // (mean(10.0) * 10.0 weight + costs = xt::zeros({1000}); + + // Now try again with some wiggling noise + traj_view = xt::random::randn({30}, 0.0, 0.5); + critic.score(data); + EXPECT_NEAR(costs(0), 3.3, 4e-1); // (mean of noise with mu=0, sigma=0.5 * 10.0 weight +} From 5f9aec13b3329073727b594c17c03eac7f630b8c Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 8 Dec 2022 11:59:44 -0800 Subject: [PATCH 33/53] adding path angle nand prefer forward tests --- test/critics_tests.cpp | 108 +++++++++++++++++++++++++++++++++++++---- 1 file changed, 98 insertions(+), 10 deletions(-) diff --git a/test/critics_tests.cpp b/test/critics_tests.cpp index 951f90ee..f310a73b 100644 --- a/test/critics_tests.cpp +++ b/test/critics_tests.cpp @@ -209,24 +209,112 @@ TEST(CriticTests, GoalCritic) EXPECT_NEAR(xt::sum(costs, immediate)(), 2500.0, 1e-6); // should be 2.5 * 1000 } -TEST(CriticTests, ObstaclesCritic) +TEST(CriticTests, PathAngleCritic) { -} + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); -TEST(CriticTests, PathAlignCritic) -{ -} + models::State state; + state.reset(1000, 30); + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + 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}; + data.motion_model = std::make_shared(); + TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally -TEST(CriticTests, PathAngleCritic) -{ -} + // Initialization testing -TEST(CriticTests, PathFollowCritic) -{ + // Make sure initializes correctly + PathAngleCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide state poses and path close, within pose tolerance so won't do anything + state.pose.pose.position.x = 0.0; + state.pose.pose.position.y = 0.0; + path.reset(10); + path.x(9) = 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; + 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; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = -1.0; // angle between path point and pose > max_angle_to_furthest_ + path.y(6) = 4.0; + critic.score(data); + EXPECT_GT(xt::sum(costs, immediate)(), 0.0); + EXPECT_NEAR(costs(0), 3.6315, 1e-2); // atan2(4,-1) [1.81] * 2.0 weight } TEST(CriticTests, PreferForwardCritic) { + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + state.reset(1000, 30); + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + 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}; + data.motion_model = std::make_shared(); + TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally + + // Initialization testing + + // Make sure initializes correctly + PreferForwardCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide state poses and path far away, not within positional tolerances + state.pose.pose.position.x = 1.0; + path.reset(10); + path.x(9) = 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 all forward motion + path.x(9) = 0.15; + state.vx = xt::ones({1000, 30}); + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path close to trigger behavior but with all reverse motion + state.vx = -1.0 * xt::ones({1000, 30}); + critic.score(data); + EXPECT_GT(xt::sum(costs, immediate)(), 0.0); + EXPECT_NEAR(costs(0), 15.0, 1e-6); // 1.0 * 0.1 model_dt * 5.0 weight * 30 length } TEST(CriticTests, TwirlingCritic) From c2c8511f25219546c6a19f033712419f128590c7 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 8 Dec 2022 16:59:02 -0800 Subject: [PATCH 34/53] half of the required optimizer tests --- include/mppic/optimizer.hpp | 2 +- include/mppic/tools/utils.hpp | 2 +- src/optimizer.cpp | 6 +- test/CMakeLists.txt | 3 +- ...izer_test.cpp => optimizer_smoke_test.cpp} | 0 test/optimizer_unit_tests.cpp | 385 ++++++++++++++++++ 6 files changed, 392 insertions(+), 6 deletions(-) rename test/{optimizer_test.cpp => optimizer_smoke_test.cpp} (100%) create mode 100644 test/optimizer_unit_tests.cpp diff --git a/include/mppic/optimizer.hpp b/include/mppic/optimizer.hpp index c7f3d1cc..8dbd057a 100644 --- a/include/mppic/optimizer.hpp +++ b/include/mppic/optimizer.hpp @@ -217,7 +217,7 @@ class Optimizer * @brief Whether the motion model is holonomic * @return Bool if holonomic to populate `y` axis of state */ - inline bool isHolonomic() const; + bool isHolonomic() const; /** * @brief Using control frequence and time step size, determine if trajectory diff --git a/include/mppic/tools/utils.hpp b/include/mppic/tools/utils.hpp index 9101a36a..4482a9a7 100644 --- a/include/mppic/tools/utils.hpp +++ b/include/mppic/tools/utils.hpp @@ -311,7 +311,7 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) for (size_t i = 0; i < dists.shape(0); i++) { size_t min_id_by_path = 0; for (size_t j = 0; j < dists.shape(1); j++) { - if (min_distance_by_path < dists(i, j)) { + if (min_distance_by_path > dists(i, j)) { min_distance_by_path = dists(i, j); min_id_by_path = j; } diff --git a/src/optimizer.cpp b/src/optimizer.cpp index 05b794a4..a11bdc98 100644 --- a/src/optimizer.cpp +++ b/src/optimizer.cpp @@ -97,14 +97,14 @@ void Optimizer::setOffset(double controller_frequency) const double controller_period = 1.0 / controller_frequency; constexpr double eps = 1e-6; - if (controller_period < settings_.model_dt) { + if ((controller_period + eps) < settings_.model_dt) { RCLCPP_WARN( logger_, "Controller period is less then model dt, consider setting it equal"); } else if (abs(controller_period - settings_.model_dt) < eps) { RCLCPP_INFO( logger_, - "Controller period is equal to model dt. Control seuqence " + "Controller period is equal to model dt. Control sequence " "shifting is ON"); settings_.shift_control_sequence = true; } else { @@ -168,7 +168,7 @@ bool Optimizer::fallback(bool fail) reset(); - if (counter++ > settings_.retry_attempt_limit) { + if (++counter > settings_.retry_attempt_limit) { counter = 0; throw std::runtime_error("Optimizer fail to compute path"); } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index ebd5257f..b94e5d5e 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,5 +1,5 @@ set(TEST_NAMES - optimizer_test + optimizer_smoke_test controller_state_transition_test models_test noise_generator_test @@ -9,6 +9,7 @@ set(TEST_NAMES utils_test path_handler_test critic_manager_test + optimizer_unit_tests ) foreach(name IN LISTS TEST_NAMES) diff --git a/test/optimizer_test.cpp b/test/optimizer_smoke_test.cpp similarity index 100% rename from test/optimizer_test.cpp rename to test/optimizer_smoke_test.cpp diff --git a/test/optimizer_unit_tests.cpp b/test/optimizer_unit_tests.cpp new file mode 100644 index 00000000..9b4cc412 --- /dev/null +++ b/test/optimizer_unit_tests.cpp @@ -0,0 +1,385 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "mppic/optimizer.hpp" + +// Tests main optimizer functions + +// applyControlSequenceConstraints - Check min/max respected. Ackermann rad too. +// updateInitialStateVelocities - Check first values are initials +// propagateStateVelocitiesFromInitials - check pass through on cvx/vx +// getControlFromSequenceAsTwist - set sequence and check returns valid object with right info +// integrateStateVelocities - Give it a couple of easy const traj and check rollout + +class RosLockGuard +{ +public: + RosLockGuard() {rclcpp::init(0, nullptr);} + ~RosLockGuard() {rclcpp::shutdown();} +}; +RosLockGuard g_rclcpp; + +using namespace mppi; // NOLINT +using namespace mppi::critics; // NOLINT +using namespace mppi::utils; // NOLINT +using xt::evaluation_strategy::immediate; + +class OptimizerTester : public Optimizer +{ +public: + OptimizerTester() + : Optimizer() {} + + void testSetDiffModel() + { + EXPECT_EQ(motion_model_.get(), nullptr); + EXPECT_NO_THROW(setMotionModel("DiffDrive")); + EXPECT_NE(motion_model_.get(), nullptr); + EXPECT_TRUE(dynamic_cast(motion_model_.get())); + EXPECT_FALSE(isHolonomic()); + } + + void testSetOmniModel() + { + EXPECT_EQ(motion_model_.get(), nullptr); + EXPECT_NO_THROW(setMotionModel("Omni")); + EXPECT_NE(motion_model_.get(), nullptr); + EXPECT_TRUE(dynamic_cast(motion_model_.get())); + EXPECT_TRUE(isHolonomic()); + } + + void testSetAckModel() + { + EXPECT_EQ(motion_model_.get(), nullptr); + EXPECT_NO_THROW(setMotionModel("Ackermann")); + EXPECT_NE(motion_model_.get(), nullptr); + EXPECT_TRUE(dynamic_cast(motion_model_.get())); + EXPECT_FALSE(isHolonomic()); + } + + void testSetRandModel() + { + EXPECT_EQ(motion_model_.get(), nullptr); + try { + setMotionModel("Random"); + FAIL(); + } catch (...) { + SUCCEED(); + } + EXPECT_EQ(motion_model_.get(), nullptr); + } + + void resetMotionModel() + { + motion_model_.reset(); + } + + void setOffsetWrapper(const double freq) + { + return setOffset(freq); + } + + bool getShiftControlSequence() + { + return settings_.shift_control_sequence; + } + + void fillOptimizerWithGarbage() + { + state_.vx = 0.43432 * xt::ones({1000, 10}); + control_sequence_.vx = 342.0 * xt::ones({30}); + control_history_[0] = {43, 5646, 32432}; + costs_ = 5.32 * xt::ones({56453}); + generated_trajectories_.x = 432.234 * xt::ones({7865, 1}); + } + + void testReset() + { + reset(); + + EXPECT_EQ(state_.vx, xt::zeros({1000, 50})); + EXPECT_EQ(control_sequence_.vx, xt::zeros({50})); + EXPECT_EQ(control_history_[0].vx, 0.0); + EXPECT_EQ(control_history_[0].vy, 0.0); + EXPECT_NEAR(xt::sum(costs_, immediate)(), 0, 1e-6); + EXPECT_EQ(generated_trajectories_.x, xt::zeros({1000, 50})); + } + + bool fallbackWrapper(bool fail) + { + return fallback(fail); + } + + 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) + { + prepare(robot_pose, robot_speed, plan, goal_checker); + + EXPECT_EQ(critics_data_.goal_checker, nullptr); + EXPECT_NEAR(xt::sum(costs_, immediate)(), 0, 1e-6); // should be reset + EXPECT_FALSE(critics_data_.fail_flag); // should be reset + EXPECT_FALSE(critics_data_.motion_model->isHolonomic()); // object is valid + diff drive + EXPECT_EQ(state_.pose.pose.position.x, 999); + EXPECT_EQ(state_.speed.linear.y, 4.0); + EXPECT_EQ(path_.x.shape(0), 17u); + } + + void testShiftControlSequence() + { + control_sequence_.reset({100}); + control_sequence_.vx(0) = 9999; + control_sequence_.vx(1) = 6; + control_sequence_.vx(2) = 888; + control_sequence_.vy(0) = 9999; + control_sequence_.vy(1) = 6; + control_sequence_.vy(2) = 888; + control_sequence_.wz(0) = 9999; + control_sequence_.wz(1) = 6; + control_sequence_.wz(2) = 888; + + resetMotionModel(); + testSetOmniModel(); + shiftControlSequence(); + + EXPECT_EQ(control_sequence_.vx(0), 6); + EXPECT_EQ(control_sequence_.vy(0), 6); + EXPECT_EQ(control_sequence_.wz(0), 6); + EXPECT_EQ(control_sequence_.vx(1), 888); + EXPECT_EQ(control_sequence_.vy(1), 888); + EXPECT_EQ(control_sequence_.wz(1), 888); + EXPECT_EQ(control_sequence_.vx(2), 0); + EXPECT_EQ(control_sequence_.vy(2), 0); + EXPECT_EQ(control_sequence_.wz(2), 0); + } + + void testSpeedLimit() + { + auto & s = settings_; + EXPECT_EQ(s.constraints.vx_max, 0.5); + EXPECT_EQ(s.constraints.vx_min, -0.35); + setSpeedLimit(0, false); + EXPECT_EQ(s.constraints.vx_max, 0.5); + EXPECT_EQ(s.constraints.vx_min, -0.35); + setSpeedLimit(50.0, true); + EXPECT_NEAR(s.constraints.vx_max, 0.5 / 2.0, 1e-3); + EXPECT_NEAR(s.constraints.vx_min, -0.35 / 2.0, 1e-3); + setSpeedLimit(0, true); + EXPECT_EQ(s.constraints.vx_max, 0.5); + EXPECT_EQ(s.constraints.vx_min, -0.35); + setSpeedLimit(0.75, false); + EXPECT_NEAR(s.constraints.vx_max, 0.75, 1e-3); + EXPECT_NEAR(s.constraints.vx_min, -0.5249, 1e-2); + } +}; + +TEST(OptimizerTests, BasicInitializedFunctions) +{ + auto node = std::make_shared("my_node"); + OptimizerTester optimizer_tester; + node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000)); + node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); + node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); + + // Should be empty of size batches x time steps + // and tests getting set params: time_steps, batch_size, controller_frequency + auto trajs = optimizer_tester.getGeneratedTrajectories(); + EXPECT_EQ(trajs.x.shape(0), 1000u); + EXPECT_EQ(trajs.x.shape(1), 50u); + EXPECT_EQ(trajs.x, xt::zeros({1000, 50})); + + optimizer_tester.resetMotionModel(); + optimizer_tester.testSetOmniModel(); + auto traj = optimizer_tester.getOptimizedTrajectory(); + EXPECT_EQ(traj(5, 0), 0.0); // x + EXPECT_EQ(traj(5, 1), 0.0); // y + EXPECT_EQ(traj(5, 2), 0.0); // yaw + EXPECT_EQ(traj.shape(0), 50u); + EXPECT_EQ(traj.shape(1), 3u); + + optimizer_tester.shutdown(); +} + +TEST(OptimizerTests, TestOptimizerMotionModels) +{ + auto node = std::make_shared("my_node"); + OptimizerTester optimizer_tester; + node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); + + // Diff Drive should be non-holonomic + optimizer_tester.resetMotionModel(); + optimizer_tester.testSetDiffModel(); + + // Omni Drive should be holonomic + optimizer_tester.resetMotionModel(); + optimizer_tester.testSetOmniModel(); + + // // Ackermann should be non-holonomic + optimizer_tester.resetMotionModel(); + optimizer_tester.testSetAckModel(); + + // // Rand should fail + optimizer_tester.resetMotionModel(); + optimizer_tester.testSetRandModel(); +} + +TEST(OptimizerTests, setOffsetTests) +{ + auto node = std::make_shared("my_node"); + OptimizerTester optimizer_tester; + node->declare_parameter("mppic.model_dt", rclcpp::ParameterValue(0.1)); + node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); + node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000)); + node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); + + // Test offsets are properly set based on relationship of model_dt and controller frequency + // Also tests getting set model_dt parameter. + EXPECT_THROW(optimizer_tester.setOffsetWrapper(1.0), std::runtime_error); + EXPECT_NO_THROW(optimizer_tester.setOffsetWrapper(30.0)); + EXPECT_FALSE(optimizer_tester.getShiftControlSequence()); + EXPECT_NO_THROW(optimizer_tester.setOffsetWrapper(10.0)); + EXPECT_TRUE(optimizer_tester.getShiftControlSequence()); +} + +TEST(OptimizerTests, resetTests) +{ + auto node = std::make_shared("my_node"); + OptimizerTester optimizer_tester; + node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); + node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000)); + node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); + + // Tests resetting the full state of all the functions after filling with garbage + optimizer_tester.fillOptimizerWithGarbage(); + optimizer_tester.testReset(); +} + +TEST(OptimizerTests, FallbackTests) +{ + auto node = std::make_shared("my_node"); + OptimizerTester optimizer_tester; + node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); + node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000)); + node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); + node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); + + // Test fallback logic, also tests getting set param retry_attempt_limit + // Because retry set to 2, it should attempt soft resets 2x before throwing exception + // for hard reset + EXPECT_FALSE(optimizer_tester.fallbackWrapper(false)); + EXPECT_TRUE(optimizer_tester.fallbackWrapper(true)); + EXPECT_TRUE(optimizer_tester.fallbackWrapper(true)); + EXPECT_THROW(optimizer_tester.fallbackWrapper(true), std::runtime_error); +} + +TEST(OptimizerTests, PrepareTests) +{ + auto node = std::make_shared("my_node"); + OptimizerTester optimizer_tester; + node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); + node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000)); + node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); + node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); + + // Test Prepare function to set the state of the robot pose/speed on new cycle + // Populate the contents with things easily identifiable if correct + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 999; + geometry_msgs::msg::Twist speed; + speed.linear.y = 4.0; + nav_msgs::msg::Path path; + path.poses.resize(17); + + optimizer_tester.testPrepare(pose, speed, path, nullptr); +} + +TEST(OptimizerTests, shiftControlSequenceTests) +{ + auto node = std::make_shared("my_node"); + OptimizerTester optimizer_tester; + node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); + node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000)); + node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); + node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); + + // Test shiftControlSequence by setting the 2nd value to something unique to neighbors + optimizer_tester.testShiftControlSequence(); +} + +TEST(OptimizerTests, SpeedLimitTests) +{ + auto node = std::make_shared("my_node"); + OptimizerTester optimizer_tester; + node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); + node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000)); + node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); + node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); + + // Test Speed limits API + optimizer_tester.testSpeedLimit(); +} From 66f13da89fb1387129e2bf7c50b92b29cc95c633 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 9 Dec 2022 13:15:03 -0800 Subject: [PATCH 35/53] completed testing! --- src/optimizer.cpp | 30 +-- test/optimizer_unit_tests.cpp | 344 +++++++++++++++++++++++++++++----- 2 files changed, 311 insertions(+), 63 deletions(-) diff --git a/src/optimizer.cpp b/src/optimizer.cpp index a11bdc98..8e8ba1c4 100644 --- a/src/optimizer.cpp +++ b/src/optimizer.cpp @@ -246,11 +246,11 @@ void Optimizer::updateStateVelocities( void Optimizer::updateInitialStateVelocities( models::State & state) const { - xt::noalias(xt::view(state.vx, xt::all(), 0)) = state_.speed.linear.x; - xt::noalias(xt::view(state.wz, xt::all(), 0)) = state_.speed.angular.z; + xt::noalias(xt::view(state.vx, xt::all(), 0)) = state.speed.linear.x; + xt::noalias(xt::view(state.wz, xt::all(), 0)) = state.speed.angular.z; if (isHolonomic()) { - xt::noalias(xt::view(state.vy, xt::all(), 0)) = state_.speed.linear.y; + xt::noalias(xt::view(state.vy, xt::all(), 0)) = state.speed.linear.y; } } @@ -262,13 +262,13 @@ void Optimizer::propagateStateVelocitiesFromInitials( void Optimizer::integrateStateVelocities( xt::xtensor & trajectory, - const xt::xtensor & state) const + const xt::xtensor & sequence) const { double initial_yaw = tf2::getYaw(state_.pose.pose.orientation); - const auto vx = xt::view(state, xt::all(), 0); - const auto vy = xt::view(state, xt::all(), 2); - const auto wz = xt::view(state, xt::all(), 1); + const auto vx = xt::view(sequence, xt::all(), 0); + const auto vy = xt::view(sequence, xt::all(), 2); + const auto wz = xt::view(sequence, xt::all(), 1); auto traj_x = xt::view(trajectory, xt::all(), 0); auto traj_y = xt::view(trajectory, xt::all(), 1); @@ -303,7 +303,7 @@ void Optimizer::integrateStateVelocities( models::Trajectories & trajectories, const models::State & state) const { - const double initial_yaw = tf2::getYaw(state_.pose.pose.orientation); + const double initial_yaw = tf2::getYaw(state.pose.pose.orientation); xt::noalias(trajectories.yaws) = utils::normalize_angles(xt::cumsum(state.wz * settings_.model_dt, 1) + initial_yaw); @@ -325,26 +325,26 @@ void Optimizer::integrateStateVelocities( dy = dy + state.vy * yaw_cos; } - xt::noalias(trajectories.x) = state_.pose.pose.position.x + + xt::noalias(trajectories.x) = state.pose.pose.position.x + xt::cumsum(dx * settings_.model_dt, 1); - xt::noalias(trajectories.y) = state_.pose.pose.position.y + + xt::noalias(trajectories.y) = state.pose.pose.position.y + xt::cumsum(dy * settings_.model_dt, 1); } xt::xtensor Optimizer::getOptimizedTrajectory() { - auto && state = + auto && sequence = xt::xtensor::from_shape({settings_.time_steps, isHolonomic() ? 3u : 2u}); auto && trajectories = xt::xtensor::from_shape({settings_.time_steps, 3}); - xt::noalias(xt::view(state, xt::all(), 0)) = control_sequence_.vx; - xt::noalias(xt::view(state, xt::all(), 1)) = control_sequence_.wz; + xt::noalias(xt::view(sequence, xt::all(), 0)) = control_sequence_.vx; + xt::noalias(xt::view(sequence, xt::all(), 1)) = control_sequence_.wz; if (isHolonomic()) { - xt::noalias(xt::view(state, xt::all(), 2)) = control_sequence_.vy; + xt::noalias(xt::view(sequence, xt::all(), 2)) = control_sequence_.vy; } - integrateStateVelocities(trajectories, state); + integrateStateVelocities(trajectories, sequence); return std::move(trajectories); } diff --git a/test/optimizer_unit_tests.cpp b/test/optimizer_unit_tests.cpp index 9b4cc412..19b8b2ff 100644 --- a/test/optimizer_unit_tests.cpp +++ b/test/optimizer_unit_tests.cpp @@ -21,12 +21,6 @@ // Tests main optimizer functions -// applyControlSequenceConstraints - Check min/max respected. Ackermann rad too. -// updateInitialStateVelocities - Check first values are initials -// propagateStateVelocitiesFromInitials - check pass through on cvx/vx -// getControlFromSequenceAsTwist - set sequence and check returns valid object with right info -// integrateStateVelocities - Give it a couple of easy const traj and check rollout - class RosLockGuard { public: @@ -142,51 +136,80 @@ class OptimizerTester : public Optimizer EXPECT_EQ(path_.x.shape(0), 17u); } - void testShiftControlSequence() + void shiftControlSequenceWrapper() { - control_sequence_.reset({100}); - control_sequence_.vx(0) = 9999; - control_sequence_.vx(1) = 6; - control_sequence_.vx(2) = 888; - control_sequence_.vy(0) = 9999; - control_sequence_.vy(1) = 6; - control_sequence_.vy(2) = 888; - control_sequence_.wz(0) = 9999; - control_sequence_.wz(1) = 6; - control_sequence_.wz(2) = 888; - - resetMotionModel(); - testSetOmniModel(); - shiftControlSequence(); - - EXPECT_EQ(control_sequence_.vx(0), 6); - EXPECT_EQ(control_sequence_.vy(0), 6); - EXPECT_EQ(control_sequence_.wz(0), 6); - EXPECT_EQ(control_sequence_.vx(1), 888); - EXPECT_EQ(control_sequence_.vy(1), 888); - EXPECT_EQ(control_sequence_.wz(1), 888); - EXPECT_EQ(control_sequence_.vx(2), 0); - EXPECT_EQ(control_sequence_.vy(2), 0); - EXPECT_EQ(control_sequence_.wz(2), 0); + return shiftControlSequence(); } - void testSpeedLimit() + std::pair getVelLimits() { auto & s = settings_; - EXPECT_EQ(s.constraints.vx_max, 0.5); - EXPECT_EQ(s.constraints.vx_min, -0.35); - setSpeedLimit(0, false); - EXPECT_EQ(s.constraints.vx_max, 0.5); - EXPECT_EQ(s.constraints.vx_min, -0.35); - setSpeedLimit(50.0, true); - EXPECT_NEAR(s.constraints.vx_max, 0.5 / 2.0, 1e-3); - EXPECT_NEAR(s.constraints.vx_min, -0.35 / 2.0, 1e-3); - setSpeedLimit(0, true); - EXPECT_EQ(s.constraints.vx_max, 0.5); - EXPECT_EQ(s.constraints.vx_min, -0.35); - setSpeedLimit(0.75, false); - EXPECT_NEAR(s.constraints.vx_max, 0.75, 1e-3); - EXPECT_NEAR(s.constraints.vx_min, -0.5249, 1e-2); + return {s.constraints.vx_min, s.constraints.vx_max}; + } + + void applyControlSequenceConstraintsWrapper() + { + return applyControlSequenceConstraints(); + } + + models::ControlSequence & grabControlSequence() + { + return control_sequence_; + } + + void testupdateStateVels() + { + // updateInitialStateVelocities + models::State state; + state.reset(1000, 50); + state.speed.linear.x = 5.0; + state.speed.linear.y = 1.0; + state.speed.angular.z = 6.0; + state.cvx = 0.75 * xt::ones({1000, 50}); + state.cvy = 0.5 * xt::ones({1000, 50}); + state.cwz = 0.1 * xt::ones({1000, 50}); + updateInitialStateVelocities(state); + EXPECT_NEAR(state.vx(0, 0), 5.0, 1e-6); + EXPECT_NEAR(state.vy(0, 0), 1.0, 1e-6); + EXPECT_NEAR(state.wz(0, 0), 6.0, 1e-6); + + // propagateStateVelocitiesFromInitials + propagateStateVelocitiesFromInitials(state); + EXPECT_NEAR(state.vx(0, 0), 5.0, 1e-6); + EXPECT_NEAR(state.vy(0, 0), 1.0, 1e-6); + EXPECT_NEAR(state.wz(0, 0), 6.0, 1e-6); + EXPECT_NEAR(state.vx(0, 1), 0.75, 1e-6); + EXPECT_NEAR(state.vy(0, 1), 0.5, 1e-6); + EXPECT_NEAR(state.wz(0, 1), 0.1, 1e-6); + + // Putting them together: updateStateVelocities + state.reset(1000, 50); + state.speed.linear.x = -5.0; + state.speed.linear.y = -1.0; + state.speed.angular.z = -6.0; + state.cvx = -0.75 * xt::ones({1000, 50}); + state.cvy = -0.5 * xt::ones({1000, 50}); + state.cwz = -0.1 * xt::ones({1000, 50}); + updateStateVelocities(state); + EXPECT_NEAR(state.vx(0, 0), -5.0, 1e-6); + EXPECT_NEAR(state.vy(0, 0), -1.0, 1e-6); + EXPECT_NEAR(state.wz(0, 0), -6.0, 1e-6); + EXPECT_NEAR(state.vx(0, 1), -0.75, 1e-6); + EXPECT_NEAR(state.vy(0, 1), -0.5, 1e-6); + EXPECT_NEAR(state.wz(0, 1), -0.1, 1e-6); + } + + geometry_msgs::msg::TwistStamped getControlFromSequenceAsTwistWrapper() + { + builtin_interfaces::msg::Time stamp; + return getControlFromSequenceAsTwist(stamp); + } + + void integrateStateVelocitiesWrapper( + models::Trajectories & traj, + const models::State & state) + { + return integrateStateVelocities(traj, state); } }; @@ -362,7 +385,31 @@ TEST(OptimizerTests, shiftControlSequenceTests) optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); // Test shiftControlSequence by setting the 2nd value to something unique to neighbors - optimizer_tester.testShiftControlSequence(); + auto & sequence = optimizer_tester.grabControlSequence(); + sequence.reset({100}); + sequence.vx(0) = 9999; + sequence.vx(1) = 6; + sequence.vx(2) = 888; + sequence.vy(0) = 9999; + sequence.vy(1) = 6; + sequence.vy(2) = 888; + sequence.wz(0) = 9999; + sequence.wz(1) = 6; + sequence.wz(2) = 888; + + optimizer_tester.resetMotionModel(); + optimizer_tester.testSetOmniModel(); + optimizer_tester.shiftControlSequenceWrapper(); + + EXPECT_EQ(sequence.vx(0), 6); + EXPECT_EQ(sequence.vy(0), 6); + EXPECT_EQ(sequence.wz(0), 6); + EXPECT_EQ(sequence.vx(1), 888); + EXPECT_EQ(sequence.vy(1), 888); + EXPECT_EQ(sequence.wz(1), 888); + EXPECT_EQ(sequence.vx(2), 0); + EXPECT_EQ(sequence.vy(2), 0); + EXPECT_EQ(sequence.wz(2), 0); } TEST(OptimizerTests, SpeedLimitTests) @@ -381,5 +428,206 @@ TEST(OptimizerTests, SpeedLimitTests) optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); // Test Speed limits API - optimizer_tester.testSpeedLimit(); + auto [v_min, v_max] = optimizer_tester.getVelLimits(); + EXPECT_EQ(v_max, 0.5); + EXPECT_EQ(v_min, -0.35); + optimizer_tester.setSpeedLimit(0, false); + auto [v_min2, v_max2] = optimizer_tester.getVelLimits(); + EXPECT_EQ(v_max2, 0.5); + EXPECT_EQ(v_min2, -0.35); + optimizer_tester.setSpeedLimit(50.0, true); + auto [v_min3, v_max3] = optimizer_tester.getVelLimits(); + EXPECT_NEAR(v_max3, 0.5 / 2.0, 1e-3); + EXPECT_NEAR(v_min3, -0.35 / 2.0, 1e-3); + optimizer_tester.setSpeedLimit(0, true); + auto [v_min4, v_max4] = optimizer_tester.getVelLimits(); + EXPECT_EQ(v_max4, 0.5); + EXPECT_EQ(v_min4, -0.35); + optimizer_tester.setSpeedLimit(0.75, false); + auto [v_min5, v_max5] = optimizer_tester.getVelLimits(); + EXPECT_NEAR(v_max5, 0.75, 1e-3); + EXPECT_NEAR(v_min5, -0.5249, 1e-2); +} + +TEST(OptimizerTests, applyControlSequenceConstraintsTests) +{ + auto node = std::make_shared("my_node"); + OptimizerTester optimizer_tester; + node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); + node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000)); + node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); + node->declare_parameter("mppic.vx_max", rclcpp::ParameterValue(1.0)); + node->declare_parameter("mppic.vx_min", rclcpp::ParameterValue(-1.0)); + node->declare_parameter("mppic.vy_max", rclcpp::ParameterValue(0.75)); + node->declare_parameter("mppic.wz_max", rclcpp::ParameterValue(2.0)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); + + // Test constraints being applied to ensure feasibility of trajectories + // Also tests param get of set vx/vy/wz min/maxes + + // Set model to omni to consider holonomic vy elements + // Ack is not tested here because `applyConstraints` is covered in detail + // in motion_models_test.cpp + optimizer_tester.resetMotionModel(); + optimizer_tester.testSetOmniModel(); + auto & sequence = optimizer_tester.grabControlSequence(); + + // Test boundary of limits + sequence.vx = xt::ones({50}); + sequence.vy = 0.75 * xt::ones({50}); + sequence.wz = 2.0 * xt::ones({50}); + optimizer_tester.applyControlSequenceConstraintsWrapper(); + EXPECT_EQ(sequence.vx, xt::ones({50})); + EXPECT_EQ(sequence.vy, 0.75 * xt::ones({50})); + EXPECT_EQ(sequence.wz, 2.0 * xt::ones({50})); + + // Test breaking limits sets to maximum + sequence.vx = 5.0 * xt::ones({50}); + sequence.vy = 5.0 * xt::ones({50}); + sequence.wz = 5.0 * xt::ones({50}); + optimizer_tester.applyControlSequenceConstraintsWrapper(); + EXPECT_EQ(sequence.vx, xt::ones({50})); + EXPECT_EQ(sequence.vy, 0.75 * xt::ones({50})); + EXPECT_EQ(sequence.wz, 2.0 * xt::ones({50})); + + // Test breaking limits sets to minimum + sequence.vx = -5.0 * xt::ones({50}); + sequence.vy = -5.0 * xt::ones({50}); + sequence.wz = -5.0 * xt::ones({50}); + optimizer_tester.applyControlSequenceConstraintsWrapper(); + EXPECT_EQ(sequence.vx, -1.0 * xt::ones({50})); + EXPECT_EQ(sequence.vy, -0.75 * xt::ones({50})); + EXPECT_EQ(sequence.wz, -2.0 * xt::ones({50})); +} + +TEST(OptimizerTests, updateStateVelocitiesTests) +{ + auto node = std::make_shared("my_node"); + OptimizerTester optimizer_tester; + node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); + node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000)); + node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); + node->declare_parameter("mppic.vx_max", rclcpp::ParameterValue(1.0)); + node->declare_parameter("mppic.vx_min", rclcpp::ParameterValue(-1.0)); + node->declare_parameter("mppic.vy_max", rclcpp::ParameterValue(0.60)); + node->declare_parameter("mppic.wz_max", rclcpp::ParameterValue(2.0)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); + + // Test settings of the state to the initial robot speed to start rollout + // Set model to omni to consider holonomic vy elements + optimizer_tester.resetMotionModel(); + optimizer_tester.testSetOmniModel(); + optimizer_tester.testupdateStateVels(); +} + +TEST(OptimizerTests, getControlFromSequenceAsTwistTests) +{ + auto node = std::make_shared("my_node"); + OptimizerTester optimizer_tester; + node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); + node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000)); + node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); + node->declare_parameter("mppic.vx_max", rclcpp::ParameterValue(1.0)); + node->declare_parameter("mppic.vx_min", rclcpp::ParameterValue(-1.0)); + node->declare_parameter("mppic.vy_max", rclcpp::ParameterValue(0.60)); + node->declare_parameter("mppic.wz_max", rclcpp::ParameterValue(2.0)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); + + // Test conversion of control sequence into a Twist command to execute + auto & sequence = optimizer_tester.grabControlSequence(); + sequence.vx = 0.25 * xt::ones({10}); + sequence.vy = 0.5 * xt::ones({10}); + sequence.wz = 0.1 * xt::ones({10}); + + auto diff_t = optimizer_tester.getControlFromSequenceAsTwistWrapper(); + EXPECT_NEAR(diff_t.twist.linear.x, 0.25, 1e-6); + EXPECT_NEAR(diff_t.twist.linear.y, 0.0, 1e-6); // Y should not be populated + EXPECT_NEAR(diff_t.twist.angular.z, 0.1, 1e-6); + + // Set model to omni to consider holonomic vy elements + optimizer_tester.resetMotionModel(); + optimizer_tester.testSetOmniModel(); + auto omni_t = optimizer_tester.getControlFromSequenceAsTwistWrapper(); + EXPECT_NEAR(omni_t.twist.linear.x, 0.25, 1e-6); + EXPECT_NEAR(omni_t.twist.linear.y, 0.5, 1e-6); // Now it should be + EXPECT_NEAR(omni_t.twist.angular.z, 0.1, 1e-6); +} + +TEST(OptimizerTests, integrateStateVelocitiesTests) +{ + auto node = std::make_shared("my_node"); + OptimizerTester optimizer_tester; + node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); + node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000)); + node->declare_parameter("mppic.model_dt", rclcpp::ParameterValue(0.1)); + node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); + optimizer_tester.resetMotionModel(); + optimizer_tester.testSetOmniModel(); + + // Test integration of velocities for trajectory rollout poses + + // Give it a couple of easy const traj and check rollout, start from 0 + models::State state; + state.reset(1000, 50); + models::Trajectories traj; + state.vx = 0.1 * xt::ones({1000, 50}); + xt::view(state.vx, xt::all(), 0) = xt::zeros({1000}); + state.vy = xt::zeros({1000, 50}); + state.wz = xt::zeros({1000, 50}); + + optimizer_tester.integrateStateVelocitiesWrapper(traj, state); + EXPECT_EQ(traj.y, xt::zeros({1000, 50})); + EXPECT_EQ(traj.yaws, xt::zeros({1000, 50})); + for (unsigned int i = 0; i != traj.x.shape(1); i++) { + EXPECT_NEAR(traj.x(1, i), i * 0.1 /*vel*/ * 0.1 /*dt*/, 1e-3); + } + + // Give it a bit of a more complex trajectory to crunch + state.vy = 0.2 * xt::ones({1000, 50}); + xt::view(state.vy, xt::all(), 0) = xt::zeros({1000}); + optimizer_tester.integrateStateVelocitiesWrapper(traj, state); + + EXPECT_EQ(traj.yaws, xt::zeros({1000, 50})); + for (unsigned int i = 0; i != traj.x.shape(1); i++) { + EXPECT_NEAR(traj.x(1, i), i * 0.1 /*vel*/ * 0.1 /*dt*/, 1e-3); + EXPECT_NEAR(traj.y(1, i), i * 0.2 /*vel*/ * 0.1 /*dt*/, 1e-3); + } + + // Lets add some angular motion to the mix + state.vy = xt::zeros({1000, 50}); + state.wz = 0.2 * xt::ones({1000, 50}); + xt::view(state.wz, xt::all(), 0) = xt::zeros({1000}); + optimizer_tester.integrateStateVelocitiesWrapper(traj, state); + + float x = 0; + float y = 0; + for (unsigned int i = 1; i != traj.x.shape(1); i++) { + std::cout << i << std::endl; + x += (0.1 /*vx*/ * cos(0.2 /*wz*/ * 0.1 /*model_dt*/ * (i - 1))) * 0.1 /*model_dt*/; + y += (0.1 /*vx*/ * sin(0.2 /*wz*/ * 0.1 /*model_dt*/ * (i - 1))) * 0.1 /*model_dt*/; + + EXPECT_NEAR(traj.x(1, i), x, 1e-6); + EXPECT_NEAR(traj.y(1, i), y, 1e-6); + } } From 7f33773e1618e1f7e9fa6de5fcb86f2d43c03c7e Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 9 Dec 2022 15:14:56 -0800 Subject: [PATCH 36/53] adding path align to smoke test --- test/optimizer_smoke_test.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/optimizer_smoke_test.cpp b/test/optimizer_smoke_test.cpp index 271772bd..4298e5e3 100644 --- a/test/optimizer_smoke_test.cpp +++ b/test/optimizer_smoke_test.cpp @@ -97,19 +97,19 @@ INSTANTIATE_TEST_SUITE_P( std::make_tuple( "Omni", std::vector( - {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, {"PathAlign"}, {"TwirlingCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}), true), std::make_tuple( "DiffDrive", std::vector( - {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, {"PathAlign"}, {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}), true), std::make_tuple( "Ackermann", std::vector( - {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, {"PathAlign"}, {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}), true)) ); From b684cb7647d64554308ff767acf97663ae4a4f65 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 9 Dec 2022 16:04:45 -0800 Subject: [PATCH 37/53] fixing test --- test/optimizer_smoke_test.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/optimizer_smoke_test.cpp b/test/optimizer_smoke_test.cpp index 4298e5e3..d22ee83e 100644 --- a/test/optimizer_smoke_test.cpp +++ b/test/optimizer_smoke_test.cpp @@ -97,19 +97,19 @@ INSTANTIATE_TEST_SUITE_P( std::make_tuple( "Omni", std::vector( - {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, {"PathAlign"}, + {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, {"PathAlignCritic"}, {"TwirlingCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}), true), std::make_tuple( "DiffDrive", std::vector( - {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, {"PathAlign"}, + {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, {"PathAlignCritic"}, {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}), true), std::make_tuple( "Ackermann", std::vector( - {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, {"PathAlign"}, + {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, {"PathAlignCritic"}, {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}), true)) ); From d5520bef90d0285824a2b3cc3263d09573b2f9b0 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 16 Dec 2022 13:54:25 -0800 Subject: [PATCH 38/53] major updates to improve path tracking and obstacle avoidance INCOMPLETE --- CMakeLists.txt | 2 +- README.md | 7 +- include/mppic/controller.hpp | 5 + include/mppic/critics/goal_critic.hpp | 1 + include/mppic/critics/path_align_critic.hpp | 10 +- include/mppic/critics/path_angle_critic.hpp | 1 + include/mppic/critics/path_follow_critic.hpp | 8 +- include/mppic/optimizer.hpp | 10 +- include/mppic/tools/utils.hpp | 18 +-- src/controller.cpp | 5 + src/critics/goal_angle_critic.cpp | 24 ++-- src/critics/goal_critic.cpp | 8 ++ src/critics/path_align_critic.cpp | 96 +++++--------- src/critics/path_follow_critic.cpp | 17 ++- src/optimizer.cpp | 1 + test/critics_tests.cpp | 125 ++++++++++++++++++- test/motion_model_tests.cpp | 2 +- test/optimizer_smoke_test.cpp | 4 +- test/optimizer_unit_tests.cpp | 1 + test/utils_test.cpp | 2 - 20 files changed, 221 insertions(+), 126 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a426031a..7b3f7024 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,7 +81,7 @@ if(BUILD_TESTING) set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() add_subdirectory(test) - add_subdirectory(benchmark) + # add_subdirectory(benchmark) endif() ament_export_libraries(${libraries}) diff --git a/README.md b/README.md index 00200f2c..83265111 100644 --- a/README.md +++ b/README.md @@ -231,5 +231,10 @@ controller_server: The `model_dt` parameter generally should be set to the duration of your control frequency. So if your control frequency is 20hz, this should be `0.05`. However, you may also set it lower **but not larger**. -Visualization of the trajectories using `visualize` uses compute resources to back out trajectories for visualization and therefore slows compute time. It is not suggested that this parameter is set to `true` during a deployed use, but is a useful debug instrument while tuning the system. +Visualization of the trajectories using `visualize` uses compute resources to back out trajectories for visualization and therefore slows compute time. It is not suggested that this parameter is set to `true` during a deployed use, but is a useful debug instrument while tuning the system, but use sparingly. Visualizating 2000 batches @ 56 points at 30 hz is _alot_. +As this is a predictive planner, there is some relationship between maximum speed, prediction times, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artifically limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the **minimum** required costmap radius is 1.5m - or 3m total width. + +The same applies to the Path Follow and Align offsets from furthest. In the same example if the furthest point we can consider is already at the edge of the costmap, then further offsets are thresholded because they're unusable. So its important while selecting these parameters to make sure that the theoretical offsets can exist on the costmap settings selected with the maximum prediction horizon and velocities desired. + +The Path Follow critic cannot drive velocities greater than the projectable distance of that velocity on the available path on the rolling costmap. The Path Align critic offset represents the number of path points a trajectory passes through while tracking the path. If this is set either absurdly low (e.g. 5) it can trigger when a robot is simply trying to start path tracking causing some suboptimal behaviors and local minima while starting a task. If it is set absurdly high (e.g. 50) relative to the path resolution and costmap size, then the critic may never trigger or only do so when at full-speed. A balance here is wise. A selection of this value to be ~30% of the maximum velocity distance projected is good (e.g. if a planner produces points every 2.5cm, 60 can fit on the 1.5m local costmap radius. If the max speed is 0.5m/s with a 3s prediction time, then 20 points represents 33% of the maximum speed projected over the prediction horizon onto the path). When in doubt, `prediction_horizon_s * max_speed / path_resolution / 3.0` is a good baseline. diff --git a/include/mppic/controller.hpp b/include/mppic/controller.hpp index 9e382dbe..d2a126c4 100644 --- a/include/mppic/controller.hpp +++ b/include/mppic/controller.hpp @@ -70,6 +70,11 @@ class MPPIController : public nav2_core::Controller */ void deactivate() override; + /** + * @brief Reset the controller state between tasks + */ + void reset() override; + /** * @brief Main method to compute velocities using the optimizer * @param robot_pose Robot pose diff --git a/include/mppic/critics/goal_critic.hpp b/include/mppic/critics/goal_critic.hpp index 98686560..006dbb1f 100644 --- a/include/mppic/critics/goal_critic.hpp +++ b/include/mppic/critics/goal_critic.hpp @@ -44,6 +44,7 @@ class GoalCritic : public CriticFunction protected: unsigned int power_{0}; float weight_{0}; + float threshold_to_consider_{0}; }; } // namespace mppi::critics diff --git a/include/mppic/critics/path_align_critic.hpp b/include/mppic/critics/path_align_critic.hpp index 03a9810a..7d5708b4 100644 --- a/include/mppic/critics/path_align_critic.hpp +++ b/include/mppic/critics/path_align_critic.hpp @@ -24,7 +24,10 @@ namespace mppi::critics /** * @class mppi::critics::ConstraintCritic - * @brief Critic objective function for aligning to the path + * @brief Critic objective function for aligning to the path. Note: + * High settings of this will follow the path more precisely, but also makes it + * difficult (or impossible) to deviate in the presense of dynamic obstacles. + * This is an important critic to tune and consider in tandem with Obstacle. */ class PathAlignCritic : public CriticFunction { @@ -42,10 +45,9 @@ class PathAlignCritic : public CriticFunction void score(CriticData & data) override; protected: - unsigned int path_point_step_{0}; - unsigned int trajectory_point_step_{0}; + size_t offset_from_furthest_{0}; + int trajectory_point_step_{0}; float threshold_to_consider_{0}; - unsigned int power_{0}; float weight_{0}; }; diff --git a/include/mppic/critics/path_angle_critic.hpp b/include/mppic/critics/path_angle_critic.hpp index e359f8d7..456b1515 100644 --- a/include/mppic/critics/path_angle_critic.hpp +++ b/include/mppic/critics/path_angle_critic.hpp @@ -25,6 +25,7 @@ namespace mppi::critics /** * @class mppi::critics::ConstraintCritic * @brief Critic objective function for aligning to path in cases of extreme misalignment + * or turning */ class PathAngleCritic : public CriticFunction { diff --git a/include/mppic/critics/path_follow_critic.hpp b/include/mppic/critics/path_follow_critic.hpp index 566bc851..a5937ce9 100644 --- a/include/mppic/critics/path_follow_critic.hpp +++ b/include/mppic/critics/path_follow_critic.hpp @@ -24,7 +24,11 @@ namespace mppi::critics /** * @class mppi::critics::ConstraintCritic - * @brief Critic objective function for following the path + * @brief Critic objective function for following the path approximately + * To allow for deviation from path in case of dynamic obstacles. Path Align + * is what aligns the trajectories to the path more or less precisely, if desireable. + * A higher weight here with an offset > 1 will accelerate the samples to full speed + * faster and push the follow point further ahead, creating some shortcutting. */ class PathFollowCritic : public CriticFunction { @@ -43,7 +47,7 @@ class PathFollowCritic : public CriticFunction void score(CriticData & data) override; protected: - float max_path_ratio_{0}; + float threshold_to_consider_{0}; size_t offset_from_furthest_{0}; unsigned int power_{0}; diff --git a/include/mppic/optimizer.hpp b/include/mppic/optimizer.hpp index 8dbd057a..8bc06602 100644 --- a/include/mppic/optimizer.hpp +++ b/include/mppic/optimizer.hpp @@ -111,6 +111,11 @@ class Optimizer */ void setSpeedLimit(double speed_limit, bool percentage); + /** + * @brief Reset the optimization problem to initial conditions + */ + void reset(); + protected: /** * @brief Main function to generate, score, and return trajectories @@ -134,11 +139,6 @@ class Optimizer */ void getParams(); - /** - * @brief Reset the optimization problem to initial conditions - */ - void reset(); - /** * @brief Set the motion model of the vehicle platform * @param model Model string to use diff --git a/include/mppic/tools/utils.hpp b/include/mppic/tools/utils.hpp index 4482a9a7..2cd8bd2f 100644 --- a/include/mppic/tools/utils.hpp +++ b/include/mppic/tools/utils.hpp @@ -311,7 +311,7 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) for (size_t i = 0; i < dists.shape(0); i++) { size_t min_id_by_path = 0; for (size_t j = 0; j < dists.shape(1); j++) { - if (min_distance_by_path > dists(i, j)) { + if (dists(i, j) < min_distance_by_path) { min_distance_by_path = dists(i, j); min_id_by_path = j; } @@ -349,22 +349,6 @@ inline double posePointAngle(const geometry_msgs::msg::Pose & pose, double point return abs(angles::shortest_angular_distance(yaw, pose_yaw)); } -/** - * @brief Evaluate ratio of data.path which reached by all trajectories in data.trajectories - * @param data Data to use - * @return ratio of path reached - */ -inline float getPathRatioReached(const CriticData & data) -{ - if (!data.furthest_reached_path_point) { - throw std::runtime_error("Furthest point not computed yet"); - } - - auto path_points_count = static_cast(data.path.x.shape(0)); - auto furthest_reached_path_point = static_cast(*data.furthest_reached_path_point); - return furthest_reached_path_point / path_points_count; -} - /** * @brief Apply Savisky-Golay filter to optimal trajectory * @param control_sequence Sequence to apply filter to diff --git a/src/controller.cpp b/src/controller.cpp index fa0aa607..579bbb0c 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -69,6 +69,11 @@ void MPPIController::deactivate() RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str()); } +void MPPIController::reset() +{ + optimizer_.reset(); +} + geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, diff --git a/src/critics/goal_angle_critic.cpp b/src/critics/goal_angle_critic.cpp index bdafc0ea..0a0cd380 100644 --- a/src/critics/goal_angle_critic.cpp +++ b/src/critics/goal_angle_critic.cpp @@ -39,22 +39,18 @@ void GoalAngleCritic::score(CriticData & data) return; } - const auto goal_idx = data.path.x.shape(0) - 1; - - const float goal_x = data.path.x(goal_idx); - const float goal_y = data.path.y(goal_idx); - - const float dx = data.state.pose.pose.position.x - goal_x; - const float dy = data.state.pose.pose.position.y - goal_y; + if (!utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, data.path)) + { + return; + } - const float dist = std::sqrt(dx * dx + dy * dy); + const auto goal_idx = data.path.x.shape(0) - 1; + const float goal_yaw = data.path.yaws(goal_idx); - if (dist < threshold_to_consider_) { - const float goal_yaw = data.path.yaws(goal_idx); - data.costs += xt::pow( - xt::mean(xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) * - weight_, power_); - } + data.costs += xt::pow( + xt::mean(xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) * + weight_, power_); } } // namespace mppi::critics diff --git a/src/critics/goal_critic.cpp b/src/critics/goal_critic.cpp index ef3367ec..4e89ff89 100644 --- a/src/critics/goal_critic.cpp +++ b/src/critics/goal_critic.cpp @@ -23,6 +23,8 @@ void GoalCritic::initialize() getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0); + getParam(threshold_to_consider_, "threshold_to_consider", 1.0); + RCLCPP_INFO( logger_, "GoalCritic instantiated with %d power and %f weight.", power_, weight_); @@ -34,6 +36,12 @@ void GoalCritic::score(CriticData & data) return; } + if (!utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, data.path)) + { + return; + } + const auto goal_idx = data.path.x.shape(0) - 1; const auto goal_x = data.path.x(goal_idx); diff --git a/src/critics/path_align_critic.cpp b/src/critics/path_align_critic.cpp index d5d86226..38dfef4e 100644 --- a/src/critics/path_align_critic.cpp +++ b/src/critics/path_align_critic.cpp @@ -20,14 +20,17 @@ namespace mppi::critics { +using namespace xt::placeholders; // NOLINT +using xt::evaluation_strategy::immediate; + void PathAlignCritic::initialize() { auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 1.0); - getParam(path_point_step_, "path_point_step", 1); - getParam(trajectory_point_step_, "trajectory_point_step", 3); + getParam(offset_from_furthest_, "offset_from_furthest", 20); + getParam(trajectory_point_step_, "trajectory_point_step", 5); getParam( threshold_to_consider_, "threshold_to_consider", 0.40f); @@ -46,83 +49,48 @@ void PathAlignCritic::score(CriticData & data) return; } - using namespace xt::placeholders; // NOLINT - using xt::evaluation_strategy::immediate; - - // see http://paulbourke.net/geometry/pointlineplane/ - - // P3 points from which we calculate distance to segments - const auto & P3_x = data.trajectories.x; - const auto & P3_y = data.trajectories.y; + utils::setPathFurthestPointIfNotSet(data); + if (*data.furthest_reached_path_point < offset_from_furthest_) { + return; + } - const auto P1_x = xt::view(data.path.x, xt::range(_, -1)); // segments start points - const auto P1_y = xt::view(data.path.y, xt::range(_, -1)); // segments start points + const auto & T_x = data.trajectories.x; + const auto & T_y = data.trajectories.y; - const auto P2_x = xt::view(data.path.x, xt::range(1, _)); // segments end points - const auto P2_y = xt::view(data.path.y, xt::range(1, _)); // segments end points + const auto P_x = xt::view(data.path.x, xt::range(_, -1)); // path points + const auto P_y = xt::view(data.path.y, xt::range(_, -1)); // path points - const size_t batch_size = P3_x.shape(0); - const size_t time_steps = P3_x.shape(1); + const size_t batch_size = T_x.shape(0); + const size_t time_steps = T_x.shape(1); + const size_t traj_pts_eval = floor(time_steps / trajectory_point_step_); const size_t path_segments_count = data.path.x.shape(0) - 1; + if (path_segments_count < 1) { + return; + } - auto && cost = xt::xtensor::from_shape({batch_size}); - - const auto P2_P1_dx = P2_x - P1_x; - const auto P2_P1_dy = P2_y - P1_y; - - const auto && P2_P1_norm_sq = xt::eval(P2_P1_dx * P2_P1_dx + P2_P1_dy * P2_P1_dy); - - auto evaluate_u = [&](size_t t, size_t p, size_t s) { - return (((P3_x(t, p) - P1_x(s)) * P2_P1_dx(s)) + ((P3_y(t, p) - P1_y(s)) * P2_P1_dy(s))) / - P2_P1_norm_sq(s); - }; - - const auto segment_short = P2_P1_norm_sq < 1e-3f; - auto evaluate_dist_sq = [&P3_x, &P3_y](const auto & P, - size_t t, size_t p) { - auto dx = P(0) - P3_x(t, p); - auto dy = P(1) - P3_y(t, p); - return dx * dx + dy * dy; - }; + auto && cost = xt::xtensor::from_shape({data.costs.shape(0)}); - size_t traj_pts_eval = floor(time_steps / trajectory_point_step_); - size_t max_s = 0; for (size_t t = 0; t < batch_size; ++t) { - float mean_dist = 0; - for (size_t p = 0; p < time_steps; p += trajectory_point_step_) { + float summed_dist = 0; + for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { double min_dist_sq = std::numeric_limits::max(); - size_t min_s = 0; - for (size_t s = 0; s < path_segments_count; s += path_point_step_) { + // Find closest path segment to the trajectory point + for (size_t s = 0; s < path_segments_count - 1; s++) { xt::xtensor_fixed> P; - if (segment_short(s)) { - P[0] = P1_x(s); - P[1] = P1_y(s); - } else if (auto u = evaluate_u(t, p, s); u <= 0) { - P[0] = P1_x(s); - P[1] = P1_y(s); - } else if (u >= 1) { - P[0] = P2_x(s); - P[1] = P2_y(s); - } else { - P[0] = P1_x(s) + u * P2_P1_dx(s); - P[1] = P1_y(s) + u * P2_P1_dy(s); - } - auto dist = evaluate_dist_sq(P, t, p); - if (dist < min_dist_sq) { - min_s = s; - min_dist_sq = dist; + float dx = P_x(s) - T_x(t, p); + float dy = P_y(s) - T_y(t, p); + float dist_sq = dx * dx + dy * dy; + if (dist_sq < min_dist_sq) { + min_dist_sq = dist_sq; } } - max_s = std::max(max_s, min_s); - mean_dist += std::sqrt(min_dist_sq); + summed_dist += std::sqrt(min_dist_sq); } - - cost(t) = mean_dist / traj_pts_eval; + cost[t] = summed_dist / traj_pts_eval; } - data.furthest_reached_path_point = max_s; - data.costs += xt::pow(cost * weight_, power_); + data.costs += xt::pow(std::move(cost) * weight_, power_); } } // namespace mppi::critics diff --git a/src/critics/path_follow_critic.cpp b/src/critics/path_follow_critic.cpp index c9f48a6a..9466bdf7 100644 --- a/src/critics/path_follow_critic.cpp +++ b/src/critics/path_follow_critic.cpp @@ -25,31 +25,28 @@ void PathFollowCritic::initialize() auto getParam = parameters_handler_->getParamGetter(name_); getParam( - max_path_ratio_, - "max_path_ratio", 0.40f); - + threshold_to_consider_, + "threshold_to_consider", 0.40f); getParam(offset_from_furthest_, "offset_from_furthest", 10); - getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 3.0); } void PathFollowCritic::score(CriticData & data) { - if (!enabled_) { + if (!enabled_ || + utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + { return; } utils::setPathFurthestPointIfNotSet(data); - if (utils::getPathRatioReached(data) > max_path_ratio_) { - return; - } auto offseted_idx = std::min( *data.furthest_reached_path_point + offset_from_furthest_, data.path.x.shape(0) - 1); - const auto path_x = xt::view(data.path.x, offseted_idx); - const auto path_y = xt::view(data.path.y, offseted_idx); + const auto path_x = data.path.x(offseted_idx); + const auto path_y = data.path.y(offseted_idx); const auto last_x = xt::view(data.trajectories.x, xt::all(), -1); const auto last_y = xt::view(data.trajectories.y, xt::all(), -1); diff --git a/src/optimizer.cpp b/src/optimizer.cpp index 8e8ba1c4..d9e19765 100644 --- a/src/optimizer.cpp +++ b/src/optimizer.cpp @@ -189,6 +189,7 @@ void Optimizer::prepare( critics_data_.fail_flag = false; critics_data_.goal_checker = goal_checker; critics_data_.motion_model = motion_model_; + critics_data_.furthest_reached_path_point.reset(); } void Optimizer::shiftControlSequence() diff --git a/test/critics_tests.cpp b/test/critics_tests.cpp index f310a73b..6b6aa904 100644 --- a/test/critics_tests.cpp +++ b/test/critics_tests.cpp @@ -191,14 +191,14 @@ TEST(CriticTests, GoalCritic) // Scoring testing with all trajectories set to 0 - // provide state poses and path far + // provide state poses and path far, should not trigger state.pose.pose.position.x = 1.0; path.reset(10); path.x(9) = 10.0; path.y(9) = 0.0; critic.score(data); - EXPECT_NEAR(costs(2), 50.0, 1e-6); // (sqrt(10.0 * 10.0) * 5.0 weight - EXPECT_NEAR(xt::sum(costs, immediate)(), 50000.0, 1e-6); // Should all be 50 * 1000 + 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 costs = xt::zeros({1000}); // provide state pose and path close @@ -375,3 +375,122 @@ TEST(CriticTests, TwirlingCritic) critic.score(data); EXPECT_NEAR(costs(0), 3.3, 4e-1); // (mean of noise with mu=0, sigma=0.5 * 10.0 weight } + +TEST(CriticTests, PathFollowCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + state.reset(1000, 30); + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + 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}; + data.motion_model = std::make_shared(); + TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally + data.goal_checker = &goal_checker; + + // Initialization testing + + // Make sure initializes correctly + PathFollowCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide state poses and path close within positional tolerances + state.pose.pose.position.x = 1.0; + path.reset(10); + path.x(9) = 0.85; + 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(9) = 0.15; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 450.0, 1e-2); // 0.15 * 3 weight * 1000 +} + + +TEST(CriticTests, PathAlignCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + state.reset(1000, 30); + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + 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}; + data.motion_model = std::make_shared(); + TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally + data.goal_checker = &goal_checker; + + // Initialization testing + + // Make sure initializes correctly + PathAlignCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide state poses and path close within positional tolerances + state.pose.pose.position.x = 1.0; + path.reset(10); + path.x(9) = 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; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path far enough to enable, with data to pass condition + // but with empty trajectories and paths, should still be zero + *data.furthest_reached_path_point = 21; + path.x(9) = 0.15; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path far enough to enable, with data to pass condition + state.pose.pose.position.x = 0.0; + path.x(0) = 0; + path.x(1) = 0.1; + path.x(2) = 0.2; + path.x(3) = 0.3; + path.x(4) = 0.4; + path.x(5) = 0.5; + path.x(6) = 0.6; + path.x(7) = 0.7; + path.x(8) = 0.8; + path.x(9) = 0.9; + generated_trajectories.x = 0.66 * xt::ones({1000, 30}); + critic.score(data); + // 0.04 * 1000 * 1 weight * 6 num pts eval / 6 normalization term + EXPECT_NEAR(xt::sum(costs, immediate)(), 40.0, 1e-2); +} diff --git a/test/motion_model_tests.cpp b/test/motion_model_tests.cpp index d76b3a58..b09dd156 100644 --- a/test/motion_model_tests.cpp +++ b/test/motion_model_tests.cpp @@ -165,7 +165,7 @@ TEST(MotionModelTests, AckermannTest) EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); EXPECT_NE(initial_control_sequence.wz, control_sequence.wz); - // Now, check the specifics of the minimum curvature constraint TODO(SM) + // Now, check the specifics of the minimum curvature constraint EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6); for (unsigned int i = 1; i != control_sequence.vx.shape(0); i++) { EXPECT_TRUE(fabs(control_sequence.vx(i)) / fabs(control_sequence.wz(i)) <= 0.2); diff --git a/test/optimizer_smoke_test.cpp b/test/optimizer_smoke_test.cpp index d22ee83e..88f7be23 100644 --- a/test/optimizer_smoke_test.cpp +++ b/test/optimizer_smoke_test.cpp @@ -103,13 +103,13 @@ INSTANTIATE_TEST_SUITE_P( std::make_tuple( "DiffDrive", std::vector( - {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, {"PathAlignCritic"}, + {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}), true), std::make_tuple( "Ackermann", std::vector( - {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, {"PathAlignCritic"}, + {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}), true)) ); diff --git a/test/optimizer_unit_tests.cpp b/test/optimizer_unit_tests.cpp index 19b8b2ff..318a3d8a 100644 --- a/test/optimizer_unit_tests.cpp +++ b/test/optimizer_unit_tests.cpp @@ -131,6 +131,7 @@ class OptimizerTester : public Optimizer EXPECT_NEAR(xt::sum(costs_, immediate)(), 0, 1e-6); // should be reset EXPECT_FALSE(critics_data_.fail_flag); // should be reset EXPECT_FALSE(critics_data_.motion_model->isHolonomic()); // object is valid + diff drive + EXPECT_FALSE(critics_data_.furthest_reached_path_point.has_value()); // val is not set EXPECT_EQ(state_.pose.pose.position.x, 999); EXPECT_EQ(state_.speed.linear.y, 4.0); EXPECT_EQ(path_.x.shape(0), 17u); diff --git a/test/utils_test.cpp b/test/utils_test.cpp index 86da88ce..669a7175 100644 --- a/test/utils_test.cpp +++ b/test/utils_test.cpp @@ -238,8 +238,6 @@ TEST(UtilsTests, FurthestReachedPoint) {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt}; /// Caution, keep references EXPECT_EQ(findPathFurthestReachedPoint(data3), 5u); - data3.furthest_reached_path_point = 5u; - EXPECT_EQ(getPathRatioReached(data3), 0.5); } TEST(UtilsTests, SmootherTest) From e43e6a730dae924cec4be03603ccc5785275afb8 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 16 Dec 2022 14:49:37 -0800 Subject: [PATCH 39/53] obstacle changes complete --- include/mppic/critics/obstacles_critic.hpp | 14 ++++---- src/critics/obstacles_critic.cpp | 38 +++++++++++++++------- 2 files changed, 35 insertions(+), 17 deletions(-) diff --git a/include/mppic/critics/obstacles_critic.hpp b/include/mppic/critics/obstacles_critic.hpp index 353da079..deb207d6 100644 --- a/include/mppic/critics/obstacles_critic.hpp +++ b/include/mppic/critics/obstacles_critic.hpp @@ -32,13 +32,16 @@ namespace mppi::critics */ struct CollisionCost { - float cost; - bool using_footprint; + float cost{0}; + bool using_footprint{false}; }; /** * @class mppi::critics::ConstraintCritic - * @brief Critic objective function for avoiding obstacles + * @brief Critic objective function for avoiding obstacles, allowing it to deviate off + * the planned path. This is important to tune in tandem with PathAlign to make a balance + * between path-tracking and dynamic obstacle avoidance capabilities as desirable for a + * particular application */ class ObstaclesCritic : public CriticFunction { @@ -100,15 +103,14 @@ class ObstaclesCritic : public CriticFunction bool consider_footprint_{true}; double collision_cost_{0}; - float inflation_scale_factor_; + float inflation_scale_factor_{0}, inflation_radius_{0}; float possibly_inscribed_cost_; - float trajectory_penalty_distance_; float collision_margin_distance_; float near_goal_distance_; unsigned int power_{0}; - float weight_{0}; + float repulsion_weight_, critical_weight_{0}; }; } // namespace mppi::critics diff --git a/src/critics/obstacles_critic.cpp b/src/critics/obstacles_critic.cpp index 1fd829b4..3d65ff26 100644 --- a/src/critics/obstacles_critic.cpp +++ b/src/critics/obstacles_critic.cpp @@ -23,9 +23,9 @@ void ObstaclesCritic::initialize() auto getParam = parameters_handler_->getParamGetter(name_); getParam(consider_footprint_, "consider_footprint", false); getParam(power_, "cost_power", 2); - getParam(weight_, "cost_weight", 1.2); + getParam(repulsion_weight_, "repulsion_weight", 4.0); + getParam(critical_weight_, "critical_weight", 1.2); getParam(collision_cost_, "collision_cost", 2000.0); - getParam(trajectory_penalty_distance_, "trajectory_penalty_distance", 1.0); getParam(collision_margin_distance_, "collision_margin_distance", 0.12); getParam(near_goal_distance_, "near_goal_distance", 0.5); @@ -58,6 +58,7 @@ double ObstaclesCritic::findCircumscribedCost( const double resolution = costmap->getCostmap()->getResolution(); result = inflation_layer->computeCost(circum_radius / resolution); inflation_scale_factor_ = static_cast(inflation_layer->getCostScalingFactor()); + inflation_radius_ = static_cast(inflation_layer->getInflationRadius()); } if (!inflation_layer_found) { @@ -67,7 +68,7 @@ double ObstaclesCritic::findCircumscribedCost( "If this is an SE2-collision checking plugin, it cannot use costmap potential " "field to speed up collision checking by only checking the full footprint " "when robot is within possibly-inscribed radius of an obstacle. This may " - "significantly slow down planning times!"); + "significantly slow down planning times and not avoid anything but absolute collisions!"); } return result; @@ -102,38 +103,53 @@ void ObstaclesCritic::score(CriticData & data) } auto && raw_cost = xt::xtensor::from_shape({data.costs.shape(0)}); + raw_cost.fill(0.0); + auto && repulsive_cost = xt::xtensor::from_shape({data.costs.shape(0)}); + repulsive_cost.fill(0.0); + const size_t traj_len = data.trajectories.x.shape(1); bool all_trajectories_collide = true; for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) { bool trajectory_collide = false; float traj_cost = 0.0; const auto & traj = data.trajectories; - float repulsion_cost = 0.0; + CollisionCost pose_cost; for (size_t j = 0; j < traj_len; j++) { - const CollisionCost pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j)); + pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j)); + if (pose_cost.cost < 1) {continue;} // In free space if (inCollision(pose_cost.cost)) { trajectory_collide = true; break; } + // Cannot process repulsion if inflation layer does not exist + if (inflation_radius_ == 0 || inflation_scale_factor_ == 0) { + continue; + } + const float dist_to_obj = distanceToObstacle(pose_cost); + + // Let near-collision trajectory points be punished severely if (dist_to_obj < collision_margin_distance_) { - // Near-collision, all points must be punished traj_cost += (collision_margin_distance_ - dist_to_obj); - } else if (dist_to_obj < trajectory_penalty_distance_ && !near_goal) { - // Prefer general trajectories further from obstacles - repulsion_cost = std::max(repulsion_cost, (trajectory_penalty_distance_ - dist_to_obj)); + } + + // Generally prefer trajectories further from obstacles + if (!near_goal) { + repulsive_cost[i] += inflation_radius_ - dist_to_obj; } } - traj_cost += repulsion_cost; if (!trajectory_collide) {all_trajectories_collide = false;} raw_cost[i] = static_cast(trajectory_collide ? collision_cost_ : traj_cost); } - data.costs = xt::pow(raw_cost * weight_, power_); + data.costs += xt::pow( + (critical_weight_ * raw_cost) + + (repulsion_weight_ * repulsive_cost / traj_len), + power_); data.fail_flag = all_trajectories_collide; } From 6d75601c08df35fc8326b3233c721318b1167b98 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 16 Dec 2022 14:58:39 -0800 Subject: [PATCH 40/53] adding docs --- README.md | 39 +++++++++++++++++--------------- src/critics/obstacles_critic.cpp | 7 +++--- 2 files changed, 25 insertions(+), 21 deletions(-) diff --git a/README.md b/README.md index 83265111..4fd82075 100644 --- a/README.md +++ b/README.md @@ -88,27 +88,29 @@ This process is then repeated a number of times and returns a converged solution | -------------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | cost_weight | double | Default 5.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | + | threshold_to_consider | double | Default 1.0. Distance between robot and goal above which goal cost starts being considered | #### Obstacles Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. | - | cost_weight | double | Default 2.0. Weight to apply to critic term. | + | critical_weight | double | Default 1.2. Weight to apply to critic for near collisions closer than `collision_margin_distance` to prevent near collisions **only** as a method of virtually inflating the footprint. This should not be used to generally influence obstacle avoidance away from criticial collisions. | + | repulsion_weight | double | Default 2.0. Weight to apply to critic for generally preferring routes in lower cost space. This is separated from the critical term to allow for fine tuning of obstacle behaviors with path alignment for dynamic scenes without impacting actions which may directly lead to near-collisions. This is applied within the `inflation_radius` distance from obstacles. | | cost_power | int | Default 2. Power order to apply to term. | | collision_cost | double | Default 2000.0. Cost to apply to a true collision in a trajectory. | - | collision_margin_distance | double | Default 0.12. Margin distance from collision to apply severe penalty. Between 0.05-0.2 is reasonable. | - | trajectory_penalty_distance | double | Default 1.0. Minimum trajectory distance from obstacle to apply a preferential penalty to incentivize navigating farther away from obstacles. | - | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term (e.g. `trajectory_penalty_distance` term) to allow robot to smoothly converge to goal pose in close proximity to obstacles. + | collision_margin_distance | double | Default 0.10. Margin distance from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable. | + | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. #### Path Align Critic - | Parameter | Type | Definition | - | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | cost_weight | double | Default 2.0. Weight to apply to critic term. | - | cost_power | int | Default 1. Power order to apply to term. | - | path_point_step | int | Default 2. Consider every N path points for alignment to speed up critic. | - | trajectory_point_step | int | Default 3. Consider every N generated trajectories points to speed up critic evaluation. | - | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which path align cost stops being considered | + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | cost_weight | double | Default 2.0. Weight to apply to critic term. | + | cost_power | int | Default 1. Power order to apply to term. | + | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which path align cost stops being considered | + | offset_from_furthest | double | Default 20. Checks that the candidate trajectories are sufficiently far along their way tracking the path to apply the alignment critic. This ensures that path alignment is only considered when actually tracking the path, preventing awkward initialization motions preventing the robot from leaving the path to achieve the appropriate heading. | + | trajectory_point_step | double | Default 5. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable. | + #### Path Angle Critic | Parameter | Type | Definition | @@ -125,7 +127,7 @@ This process is then repeated a number of times and returns a converged solution | cost_weight | double | Default 3.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | | offset_from_furthest | int | Default 10. Number of path points after furthest one any trajectory achieves to drive path tracking relative to. | - | max_path_ratio | float | Default 0.4. Maximum percentage (0-1.0) of path overlapping with trajectory points to apply critic. Lets goal critics take over on approach to goal. | + | threshold_to_consider | float | Default 0.4. Distance between robot and goal above which path follow cost stops being considered | #### Prefer Forward Critic | Parameter | Type | Definition | @@ -179,6 +181,7 @@ controller_server: enabled: true cost_power: 1 cost_weight: 5.0 + threshold_to_consider: 1.0 GoalAngleCritic: enabled: true cost_power: 1 @@ -187,24 +190,24 @@ controller_server: ObstaclesCritic: enabled: true cost_power: 2 - cost_weight: 1.2 + critical_weight: 1.2 + repulsion_weight: 1.2 consider_footprint: false collision_cost: 2000.0 - trajectory_penalty_distance: 1.0 - collision_margin_distance: 0.12 + collision_margin_distance: 0.10 PathAlignCritic: enabled: true cost_power: 1 cost_weight: 1.0 - path_point_step: 1 - trajectory_point_step: 3 threshold_to_consider: 0.40 + trajectory_point_step: 5 + offset_from_furthest: 20 PathFollowCritic: enabled: true cost_power: 1 cost_weight: 3.0 offset_from_furthest: 10 - max_path_ratio: 0.40 + threshold_to_consider: 0.40 PathAngleCritic: enabled: true cost_power: 1 diff --git a/src/critics/obstacles_critic.cpp b/src/critics/obstacles_critic.cpp index 3d65ff26..fea66945 100644 --- a/src/critics/obstacles_critic.cpp +++ b/src/critics/obstacles_critic.cpp @@ -26,16 +26,17 @@ void ObstaclesCritic::initialize() getParam(repulsion_weight_, "repulsion_weight", 4.0); getParam(critical_weight_, "critical_weight", 1.2); getParam(collision_cost_, "collision_cost", 2000.0); - getParam(collision_margin_distance_, "collision_margin_distance", 0.12); + getParam(collision_margin_distance_, "collision_margin_distance", 0.10); getParam(near_goal_distance_, "near_goal_distance", 0.5); collision_checker_.setCostmap(costmap_); possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); RCLCPP_INFO( logger_, - "ObstaclesCritic instantiated with %d power and %f weight. " + "ObstaclesCritic instantiated with %d power and %f / %f weights. " "Critic will collision check based on %s cost.", - power_, weight_, consider_footprint_ ? "footprint" : "circular"); + power_, critical_weight_, repulsion_weight_, consider_footprint_ ? + "footprint" : "circular"); } double ObstaclesCritic::findCircumscribedCost( From bb0d4eabf7e1c2c28f72bb155554e7f49199d26f Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 16 Dec 2022 15:19:43 -0800 Subject: [PATCH 41/53] adding discussion on cost values --- README.md | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/README.md b/README.md index 4fd82075..51dfc773 100644 --- a/README.md +++ b/README.md @@ -232,12 +232,26 @@ controller_server: ## Notes to Users +### General Words of Wisdom + The `model_dt` parameter generally should be set to the duration of your control frequency. So if your control frequency is 20hz, this should be `0.05`. However, you may also set it lower **but not larger**. Visualization of the trajectories using `visualize` uses compute resources to back out trajectories for visualization and therefore slows compute time. It is not suggested that this parameter is set to `true` during a deployed use, but is a useful debug instrument while tuning the system, but use sparingly. Visualizating 2000 batches @ 56 points at 30 hz is _alot_. +### Prediction Horizon, Costmap Sizing, and Offsets + As this is a predictive planner, there is some relationship between maximum speed, prediction times, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artifically limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the **minimum** required costmap radius is 1.5m - or 3m total width. The same applies to the Path Follow and Align offsets from furthest. In the same example if the furthest point we can consider is already at the edge of the costmap, then further offsets are thresholded because they're unusable. So its important while selecting these parameters to make sure that the theoretical offsets can exist on the costmap settings selected with the maximum prediction horizon and velocities desired. The Path Follow critic cannot drive velocities greater than the projectable distance of that velocity on the available path on the rolling costmap. The Path Align critic offset represents the number of path points a trajectory passes through while tracking the path. If this is set either absurdly low (e.g. 5) it can trigger when a robot is simply trying to start path tracking causing some suboptimal behaviors and local minima while starting a task. If it is set absurdly high (e.g. 50) relative to the path resolution and costmap size, then the critic may never trigger or only do so when at full-speed. A balance here is wise. A selection of this value to be ~30% of the maximum velocity distance projected is good (e.g. if a planner produces points every 2.5cm, 60 can fit on the 1.5m local costmap radius. If the max speed is 0.5m/s with a 3s prediction time, then 20 points represents 33% of the maximum speed projected over the prediction horizon onto the path). When in doubt, `prediction_horizon_s * max_speed / path_resolution / 3.0` is a good baseline. + +### Obstacle, Inflation Layer, and Path Following + +There also exists a relationship between the costmap configurations and the Obstacle critic configurations. If the Obstacle critic is not well tuned with the costmap parameters (inflation radius, scale) it can cause the robot to wobble significantly as it attempts to take finitely lower-cost trajectories with a slightly lower cost in exchange for jerky motion. It may also perform awkward maneuvors when in free-space to try to maximize time in a small pocket of 0-cost over a more natural motion which involves moving into some low-costed region. Finally, it may generally refuse to go into costed space at all when starting in a free 0-cost space if the gain is set disproportionately higher than the Path Follow scoring to encourage the robot to move along the path. This is due to the critic cost of staying in free space becoming more attractive than entering even lightly costed space in exchange for progression along the task. + +Thus, care should be taken to select weights of the obstacle critic in conjunction with the costmap inflation radius and scale so that a robot does not have such issues. How I (Steve, your friendly neighborhood navigator) tuned this was to first create the appropriate obstacle critic behavior desirable in conjunction with the inflation layer parameters. Its worth noting that the Obstacle critic converts the cost into a distance from obstacles, so the nature of the distribution of costs in the inflation isn't overly significant. However, the inflation radius and the scale will define the cost at the end of the distribution where free-space meets the lowest cost value within the radius. So testing for quality behavior when going over that threshold should be considered. + +As you increase or decrease your weights on the Obstacle, you may notice the aforementioned behaviors (e.g. won't overcome free to non-free threshold). To overcome them, increase the FollowPath critic cost to increase the desire for the trajectory planner to continue moving towards the goal. Make sure to not overshoot this though, keep them balanced. A desirable outcome is smooth motion roughly in the center of spaces without significant close interactions with obstacles. It shouldn't be perfectly following a path yet nor should the output velocity be wobbling jaggedly. + +Once you have your obstacle avoidance behavior tuned and matched with an appropriate path following penalty, tune the Path Align critic to align with the path. If you design exact-path-alignment behavior, its possible to skip the obstacle critic step as highly tuning the system to follow the path will give it less ability to deviate to avoid obstacles (though it'll slow and stop). Tuning the critic weight for the Obstacle critic high will do the job to avoid near-collisions but the repulsion weight is largely unnecessary to you. For others wanting more dynamic behavior, it _can_ be beneficial to slowly lower the weight on the obstacle critic to give the path alignment critic some more room to work. If your path was generated with a cost-aware planner (like all provided by Nav2) and providing paths sufficiently far from obstacles for your satesfaction, the impact of a slightly reduced Obstacle critic with a Path Alignment critic will do you well. Not over-weighting the path align critic will allow the robot to deviate from the path to get around dynamic obstacles in the scene or other obstacles not previous considered duing path planning. It is subjective as to the best behavior for your application, but it has been shown that MPPI can be an exact path tracker and/or avoid dynamic obstacles very fluidly and everywhere in between. The defaults provided are in the generally right regime for a balanced initial trade-off. From 8dcbe771eceddcc4aebf709b73cb5e1129ea46cc Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Mon, 19 Dec 2022 17:59:55 -0800 Subject: [PATCH 42/53] change to else if --- src/critics/obstacles_critic.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/critics/obstacles_critic.cpp b/src/critics/obstacles_critic.cpp index fea66945..d9325928 100644 --- a/src/critics/obstacles_critic.cpp +++ b/src/critics/obstacles_critic.cpp @@ -138,8 +138,8 @@ void ObstaclesCritic::score(CriticData & data) } // Generally prefer trajectories further from obstacles - if (!near_goal) { - repulsive_cost[i] += inflation_radius_ - dist_to_obj; + else if (!near_goal) { + repulsive_cost[i] += (inflation_radius_ - dist_to_obj); } } From 9c738caa00aea3a9f287ab0d6f790b9b7cc480ec Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Mon, 19 Dec 2022 19:15:29 -0800 Subject: [PATCH 43/53] add path follow validity check --- include/mppic/critic_function.hpp | 53 ++++++++++++++++++++++ include/mppic/critics/obstacles_critic.hpp | 10 ---- src/critics/obstacles_critic.cpp | 41 +++++++++-------- src/critics/path_follow_critic.cpp | 10 +++- 4 files changed, 85 insertions(+), 29 deletions(-) diff --git a/include/mppic/critic_function.hpp b/include/mppic/critic_function.hpp index e5f5d964..7119cc49 100644 --- a/include/mppic/critic_function.hpp +++ b/include/mppic/critic_function.hpp @@ -27,6 +27,16 @@ namespace mppi::critics { +/** + * @class mppi::critics::CollisionCost + * @brief Utility for storing cost information + */ +struct CollisionCost +{ + float cost{0}; + bool using_footprint{false}; +}; + /** * @class mppi::critics::CriticFunction * @brief Abstract critic objective function to score trajectories @@ -93,6 +103,49 @@ class CriticFunction } protected: + /** + * @brief Checks if cost represents a collision for a path point + * @param cost Costmap cost + * @return bool if in collision + */ + bool inCollision(float cost) + { + bool is_tracking_unknown = + costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + + switch (static_cast(cost)) { + using namespace nav2_costmap_2d; // NOLINT + case (LETHAL_OBSTACLE): + return true; + case (INSCRIBED_INFLATED_OBSTACLE): + return true; + case (NO_INFORMATION): + return is_tracking_unknown ? false : true; + } + + return false; + } + + /** + * @brief Checks if the path point for driving is valid + * @param idx Index of path point to evaluate + * @param data Data struct + */ + bool isPathPtValid(size_t idx, CriticData & data) + { + const auto path_x = data.path.x(idx); + const auto path_y = data.path.y(idx); + unsigned int map_x, map_y; + auto * costmap = costmap_ros_->getCostmap(); + costmap->worldToMap(path_x, path_y, map_x, map_y); + + if (inCollision(static_cast(costmap->getCost(map_x, map_y)))) { + return false; + } + + return true; + } + bool enabled_; std::string name_, parent_name_; rclcpp_lifecycle::LifecycleNode::WeakPtr parent_; diff --git a/include/mppic/critics/obstacles_critic.hpp b/include/mppic/critics/obstacles_critic.hpp index deb207d6..161e4dd3 100644 --- a/include/mppic/critics/obstacles_critic.hpp +++ b/include/mppic/critics/obstacles_critic.hpp @@ -26,16 +26,6 @@ namespace mppi::critics { -/** - * @class mppi::critics::CollisionCost - * @brief Utility for storing cost information - */ -struct CollisionCost -{ - float cost{0}; - bool using_footprint{false}; -}; - /** * @class mppi::critics::ConstraintCritic * @brief Critic objective function for avoiding obstacles, allowing it to deviate off diff --git a/src/critics/obstacles_critic.cpp b/src/critics/obstacles_critic.cpp index d9325928..12f3f9db 100644 --- a/src/critics/obstacles_critic.cpp +++ b/src/critics/obstacles_critic.cpp @@ -154,24 +154,11 @@ void ObstaclesCritic::score(CriticData & data) data.fail_flag = all_trajectories_collide; } -CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) -{ - CollisionCost collision_cost; - float & cost = collision_cost.cost; - collision_cost.using_footprint = false; - unsigned int x_i, y_i; - collision_checker_.worldToMap(x, y, x_i, y_i); - cost = collision_checker_.pointCost(x_i, y_i); - - if (consider_footprint_ && cost >= possibly_inscribed_cost_) { - cost = static_cast(collision_checker_.footprintCostAtPose( - x, y, theta, costmap_ros_->getRobotFootprint())); - collision_cost.using_footprint = true; - } - - return collision_cost; -} - +/** + * @brief Checks if cost represents a collision + * @param cost Costmap cost + * @return bool if in collision + */ bool ObstaclesCritic::inCollision(float cost) const { bool is_tracking_unknown = @@ -190,6 +177,24 @@ bool ObstaclesCritic::inCollision(float cost) const return false; } +CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) +{ + CollisionCost collision_cost; + float & cost = collision_cost.cost; + collision_cost.using_footprint = false; + unsigned int x_i, y_i; + collision_checker_.worldToMap(x, y, x_i, y_i); + cost = collision_checker_.pointCost(x_i, y_i); + + if (consider_footprint_ && cost >= possibly_inscribed_cost_) { + cost = static_cast(collision_checker_.footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint())); + collision_cost.using_footprint = true; + } + + return collision_cost; +} + unsigned char ObstaclesCritic::maxCost() { return consider_footprint_ ? nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE : diff --git a/src/critics/path_follow_critic.cpp b/src/critics/path_follow_critic.cpp index 9466bdf7..8e69b522 100644 --- a/src/critics/path_follow_critic.cpp +++ b/src/critics/path_follow_critic.cpp @@ -41,9 +41,17 @@ void PathFollowCritic::score(CriticData & data) } utils::setPathFurthestPointIfNotSet(data); + const size_t path_size = data.path.x.shape(0) - 1; auto offseted_idx = std::min( - *data.furthest_reached_path_point + offset_from_furthest_, data.path.x.shape(0) - 1); + *data.furthest_reached_path_point + offset_from_furthest_, path_size); + + // The path point to drive towards needs to be valid for safety + bool valid = false; + while (!valid && offseted_idx < path_size) { + valid = isPathPtValid(offseted_idx, data); + offseted_idx++; + } const auto path_x = data.path.x(offseted_idx); const auto path_y = data.path.y(offseted_idx); From b0075395d69a833f0234551187c7ad1b72b1a1dd Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Mon, 19 Dec 2022 19:32:19 -0800 Subject: [PATCH 44/53] removing crashing code temp --- src/critics/path_follow_critic.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/critics/path_follow_critic.cpp b/src/critics/path_follow_critic.cpp index 8e69b522..346f9378 100644 --- a/src/critics/path_follow_critic.cpp +++ b/src/critics/path_follow_critic.cpp @@ -46,13 +46,6 @@ void PathFollowCritic::score(CriticData & data) auto offseted_idx = std::min( *data.furthest_reached_path_point + offset_from_furthest_, path_size); - // The path point to drive towards needs to be valid for safety - bool valid = false; - while (!valid && offseted_idx < path_size) { - valid = isPathPtValid(offseted_idx, data); - offseted_idx++; - } - const auto path_x = data.path.x(offseted_idx); const auto path_y = data.path.y(offseted_idx); From dd3d990d2b3567c73f8c6212fa17405d41a7e96a Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Tue, 20 Dec 2022 14:50:58 -0800 Subject: [PATCH 45/53] completed for dynamic obstacles --- README.md | 4 ++- include/mppic/critic_data.hpp | 2 ++ include/mppic/critic_function.hpp | 43 ------------------------ include/mppic/optimizer.hpp | 2 +- include/mppic/tools/utils.hpp | 53 ++++++++++++++++++++++++++++++ src/critics/obstacles_critic.cpp | 5 +-- src/critics/path_align_critic.cpp | 16 +++++++-- src/critics/path_follow_critic.cpp | 11 +++++++ test/critic_manager_test.cpp | 3 +- test/critics_tests.cpp | 24 +++++++++----- test/utils_test.cpp | 6 ++-- 11 files changed, 105 insertions(+), 64 deletions(-) diff --git a/README.md b/README.md index 51dfc773..1b7f1258 100644 --- a/README.md +++ b/README.md @@ -238,13 +238,15 @@ The `model_dt` parameter generally should be set to the duration of your control Visualization of the trajectories using `visualize` uses compute resources to back out trajectories for visualization and therefore slows compute time. It is not suggested that this parameter is set to `true` during a deployed use, but is a useful debug instrument while tuning the system, but use sparingly. Visualizating 2000 batches @ 56 points at 30 hz is _alot_. +The most common parameters you might want to start off changing are the velocity profiles (`vx_max`, `vx_min`, `wz_max`, and `vy_max` if holonomic) and the `motion_model` to correspond to your vehicle. Its wise to consider the `prune_distance` of the path plan in proportion to your maximum velocity and prediction horizon. The only deeper parameter that will likely need to be adjusted for your particular settings is the Obstacle critics' `repulsion_weight` since the tuning of this is proprtional to your inflation layer's radius. Higher radii should correspond to reduced `repulsion_weight` due to the penalty formation (e.g. `inflation_radius - min_dist_to_obstacle`). If this penalty is too high, the robot will slow significantly when entering cost-space from non-cost space or jitter in narrow corridors. It is noteworthy, but likely not necessary to be changed, that the Obstacle critic may use the full footprint information if `consider_footprint = true`, though comes at an increased compute cost. + ### Prediction Horizon, Costmap Sizing, and Offsets As this is a predictive planner, there is some relationship between maximum speed, prediction times, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artifically limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the **minimum** required costmap radius is 1.5m - or 3m total width. The same applies to the Path Follow and Align offsets from furthest. In the same example if the furthest point we can consider is already at the edge of the costmap, then further offsets are thresholded because they're unusable. So its important while selecting these parameters to make sure that the theoretical offsets can exist on the costmap settings selected with the maximum prediction horizon and velocities desired. -The Path Follow critic cannot drive velocities greater than the projectable distance of that velocity on the available path on the rolling costmap. The Path Align critic offset represents the number of path points a trajectory passes through while tracking the path. If this is set either absurdly low (e.g. 5) it can trigger when a robot is simply trying to start path tracking causing some suboptimal behaviors and local minima while starting a task. If it is set absurdly high (e.g. 50) relative to the path resolution and costmap size, then the critic may never trigger or only do so when at full-speed. A balance here is wise. A selection of this value to be ~30% of the maximum velocity distance projected is good (e.g. if a planner produces points every 2.5cm, 60 can fit on the 1.5m local costmap radius. If the max speed is 0.5m/s with a 3s prediction time, then 20 points represents 33% of the maximum speed projected over the prediction horizon onto the path). When in doubt, `prediction_horizon_s * max_speed / path_resolution / 3.0` is a good baseline. +The Path Follow critic cannot drive velocities greater than the projectable distance of that velocity on the available path on the rolling costmap. The Path Align critic `offset_from_furthest` represents the number of path points a trajectory passes through while tracking the path. If this is set either absurdly low (e.g. 5) it can trigger when a robot is simply trying to start path tracking causing some suboptimal behaviors and local minima while starting a task. If it is set absurdly high (e.g. 50) relative to the path resolution and costmap size, then the critic may never trigger or only do so when at full-speed. A balance here is wise. A selection of this value to be ~30% of the maximum velocity distance projected is good (e.g. if a planner produces points every 2.5cm, 60 can fit on the 1.5m local costmap radius. If the max speed is 0.5m/s with a 3s prediction time, then 20 points represents 33% of the maximum speed projected over the prediction horizon onto the path). When in doubt, `prediction_horizon_s * max_speed / path_resolution / 3.0` is a good baseline. ### Obstacle, Inflation Layer, and Path Following diff --git a/include/mppic/critic_data.hpp b/include/mppic/critic_data.hpp index 075ebe01..bcc742ff 100644 --- a/include/mppic/critic_data.hpp +++ b/include/mppic/critic_data.hpp @@ -16,6 +16,7 @@ #define MPPIC__CRITIC_DATA_HPP_ #include +#include #include #include "geometry_msgs/msg/pose_stamped.hpp" @@ -46,6 +47,7 @@ struct CriticData bool fail_flag; nav2_core::GoalChecker * goal_checker; std::shared_ptr motion_model; + std::optional> path_pts_valid; std::optional furthest_reached_path_point; }; diff --git a/include/mppic/critic_function.hpp b/include/mppic/critic_function.hpp index 7119cc49..74b7d1f9 100644 --- a/include/mppic/critic_function.hpp +++ b/include/mppic/critic_function.hpp @@ -103,49 +103,6 @@ class CriticFunction } protected: - /** - * @brief Checks if cost represents a collision for a path point - * @param cost Costmap cost - * @return bool if in collision - */ - bool inCollision(float cost) - { - bool is_tracking_unknown = - costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); - - switch (static_cast(cost)) { - using namespace nav2_costmap_2d; // NOLINT - case (LETHAL_OBSTACLE): - return true; - case (INSCRIBED_INFLATED_OBSTACLE): - return true; - case (NO_INFORMATION): - return is_tracking_unknown ? false : true; - } - - return false; - } - - /** - * @brief Checks if the path point for driving is valid - * @param idx Index of path point to evaluate - * @param data Data struct - */ - bool isPathPtValid(size_t idx, CriticData & data) - { - const auto path_x = data.path.x(idx); - const auto path_y = data.path.y(idx); - unsigned int map_x, map_y; - auto * costmap = costmap_ros_->getCostmap(); - costmap->worldToMap(path_x, path_y, map_x, map_y); - - if (inCollision(static_cast(costmap->getCost(map_x, map_y)))) { - return false; - } - - return true; - } - bool enabled_; std::string name_, parent_name_; rclcpp_lifecycle::LifecycleNode::WeakPtr parent_; diff --git a/include/mppic/optimizer.hpp b/include/mppic/optimizer.hpp index 8bc06602..8af11e33 100644 --- a/include/mppic/optimizer.hpp +++ b/include/mppic/optimizer.hpp @@ -254,7 +254,7 @@ class Optimizer CriticData critics_data_ = {state_, generated_trajectories_, path_, costs_, settings_.model_dt, false, nullptr, nullptr, - std::nullopt}; /// Caution, keep references + std::nullopt, std::nullopt}; /// Caution, keep references rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; }; diff --git a/include/mppic/tools/utils.hpp b/include/mppic/tools/utils.hpp index 2cd8bd2f..e8449efb 100644 --- a/include/mppic/tools/utils.hpp +++ b/include/mppic/tools/utils.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -332,6 +333,58 @@ inline void setPathFurthestPointIfNotSet(CriticData & data) } } +/** + * @brief evaluate path costs + * @param data Data to use + */ +inline void findPathCosts( + CriticData & data, + std::shared_ptr costmap_ros) +{ + auto * costmap = costmap_ros->getCostmap(); + unsigned int map_x, map_y; + const size_t path_segments_count = data.path.x.shape(0) - 1; + data.path_pts_valid->resize(path_segments_count - 1, false); + for (unsigned int idx = 0; idx < path_segments_count - 1; idx++) { + const auto path_x = data.path.x(idx); + const auto path_y = data.path.y(idx); + if (!costmap->worldToMap(path_x, path_y, map_x, map_y)) { + (*data.path_pts_valid)[idx] = false; + continue; + } + + switch (costmap->getCost(map_x, map_y)) { + using namespace nav2_costmap_2d; // NOLINT + case (LETHAL_OBSTACLE): + (*data.path_pts_valid)[idx] = false; + continue; + case (INSCRIBED_INFLATED_OBSTACLE): + (*data.path_pts_valid)[idx] = false; + continue; + case (NO_INFORMATION): + const bool is_tracking_unknown = + costmap_ros->getLayeredCostmap()->isTrackingUnknown(); + (*data.path_pts_valid)[idx] = is_tracking_unknown ? true : false; + continue; + } + + (*data.path_pts_valid)[idx] = true; + } +} + +/** + * @brief evaluate path costs if it is not set + * @param data Data to use + */ +inline void setPathCostsIfNotSet( + CriticData & data, + std::shared_ptr costmap_ros) +{ + if (!data.path_pts_valid) { + findPathCosts(data, costmap_ros); + } +} + /** * @brief evaluate angle from pose (have angle) to point (no angle) * @param pose pose diff --git a/src/critics/obstacles_critic.cpp b/src/critics/obstacles_critic.cpp index 12f3f9db..8bda41e1 100644 --- a/src/critics/obstacles_critic.cpp +++ b/src/critics/obstacles_critic.cpp @@ -135,10 +135,7 @@ void ObstaclesCritic::score(CriticData & data) // Let near-collision trajectory points be punished severely if (dist_to_obj < collision_margin_distance_) { traj_cost += (collision_margin_distance_ - dist_to_obj); - } - - // Generally prefer trajectories further from obstacles - else if (!near_goal) { + } else if (!near_goal) { // Generally prefer trajectories further from obstacles repulsive_cost[i] += (inflation_radius_ - dist_to_obj); } } diff --git a/src/critics/path_align_critic.cpp b/src/critics/path_align_critic.cpp index 38dfef4e..d4a4e453 100644 --- a/src/critics/path_align_critic.cpp +++ b/src/critics/path_align_critic.cpp @@ -54,6 +54,8 @@ void PathAlignCritic::score(CriticData & data) return; } + utils::setPathCostsIfNotSet(data, costmap_ros_); + const auto & T_x = data.trajectories.x; const auto & T_y = data.trajectories.y; @@ -64,16 +66,17 @@ void PathAlignCritic::score(CriticData & data) const size_t time_steps = T_x.shape(1); const size_t traj_pts_eval = floor(time_steps / trajectory_point_step_); const size_t path_segments_count = data.path.x.shape(0) - 1; + auto && cost = xt::xtensor::from_shape({data.costs.shape(0)}); + if (path_segments_count < 1) { return; } - auto && cost = xt::xtensor::from_shape({data.costs.shape(0)}); - for (size_t t = 0; t < batch_size; ++t) { float summed_dist = 0; for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { double min_dist_sq = std::numeric_limits::max(); + size_t min_s = 0; // Find closest path segment to the trajectory point for (size_t s = 0; s < path_segments_count - 1; s++) { @@ -83,10 +86,17 @@ void PathAlignCritic::score(CriticData & data) float dist_sq = dx * dx + dy * dy; if (dist_sq < min_dist_sq) { min_dist_sq = dist_sq; + min_s = s; } } - summed_dist += std::sqrt(min_dist_sq); + + // The nearest path point to align to needs to be not in collision, else + // let the obstacle critic take over in this region due to dynamic obstacles + if (min_s != 0 && (*data.path_pts_valid)[min_s]) { + summed_dist += std::sqrt(min_dist_sq); + } } + cost[t] = summed_dist / traj_pts_eval; } diff --git a/src/critics/path_follow_critic.cpp b/src/critics/path_follow_critic.cpp index 346f9378..0ed382df 100644 --- a/src/critics/path_follow_critic.cpp +++ b/src/critics/path_follow_critic.cpp @@ -41,11 +41,22 @@ void PathFollowCritic::score(CriticData & data) } utils::setPathFurthestPointIfNotSet(data); + utils::setPathCostsIfNotSet(data, costmap_ros_); const size_t path_size = data.path.x.shape(0) - 1; auto offseted_idx = std::min( *data.furthest_reached_path_point + offset_from_furthest_, path_size); + // Drive to the first valid path point, in case of dynamic obstacles on path + // we want to drive past it, not through it + bool valid = false; + while (!valid && offseted_idx < path_size - 1) { + valid = (*data.path_pts_valid)[offseted_idx]; + if (!valid) { + offseted_idx++; + } + } + const auto path_x = data.path.x(offseted_idx); const auto path_y = data.path.y(offseted_idx); diff --git a/test/critic_manager_test.cpp b/test/critic_manager_test.cpp index 48c1b3fc..7e4a1765 100644 --- a/test/critic_manager_test.cpp +++ b/test/critic_manager_test.cpp @@ -106,7 +106,8 @@ TEST(CriticManagerTests, BasicCriticOperations) xt::xtensor costs; float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt}; + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + std::nullopt, std::nullopt}; data.fail_flag = true; EXPECT_FALSE(critic_manager.getDummyCriticScored()); diff --git a/test/critics_tests.cpp b/test/critics_tests.cpp index 6b6aa904..bb730641 100644 --- a/test/critics_tests.cpp +++ b/test/critics_tests.cpp @@ -56,7 +56,8 @@ TEST(CriticTests, ConstraintsCritic) 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}; + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, + std::nullopt}; data.motion_model = std::make_shared(); // Initialization testing @@ -128,7 +129,8 @@ TEST(CriticTests, GoalAngleCritic) 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}; + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, + std::nullopt}; data.motion_model = std::make_shared(); // Initialization testing @@ -179,7 +181,8 @@ TEST(CriticTests, GoalCritic) 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}; + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, + std::nullopt}; data.motion_model = std::make_shared(); // Initialization testing @@ -228,7 +231,8 @@ TEST(CriticTests, PathAngleCritic) 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}; + {state, generated_trajectories, path, 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 @@ -284,7 +288,8 @@ TEST(CriticTests, PreferForwardCritic) 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}; + {state, generated_trajectories, path, 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 @@ -336,7 +341,8 @@ TEST(CriticTests, TwirlingCritic) 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}; + {state, generated_trajectories, path, 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; @@ -395,7 +401,8 @@ TEST(CriticTests, PathFollowCritic) 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}; + {state, generated_trajectories, path, 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; @@ -443,7 +450,8 @@ TEST(CriticTests, PathAlignCritic) 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}; + {state, generated_trajectories, path, 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; diff --git a/test/utils_test.cpp b/test/utils_test.cpp index 669a7175..042be2e0 100644 --- a/test/utils_test.cpp +++ b/test/utils_test.cpp @@ -207,7 +207,7 @@ TEST(UtilsTests, FurthestReachedPoint) CriticData data = {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, - std::nullopt}; /// Caution, keep references + std::nullopt, std::nullopt}; /// Caution, keep references // Attempt to set furthest point if notionally set, should not change data.furthest_reached_path_point = 99999; @@ -217,7 +217,7 @@ TEST(UtilsTests, FurthestReachedPoint) // 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, - std::nullopt}; /// Caution, keep references + std::nullopt, std::nullopt}; /// Caution, keep references setPathFurthestPointIfNotSet(data2); EXPECT_EQ(data2.furthest_reached_path_point, 0); @@ -236,7 +236,7 @@ TEST(UtilsTests, FurthestReachedPoint) CriticData data3 = {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, - std::nullopt}; /// Caution, keep references + std::nullopt, std::nullopt}; /// Caution, keep references EXPECT_EQ(findPathFurthestReachedPoint(data3), 5u); } From 2f02b028fb038fe0cc74fb45ba91f30054359a7c Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Tue, 20 Dec 2022 16:32:14 -0800 Subject: [PATCH 46/53] make prune utilize things within costmap with better gaurentees --- src/path_handler.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/path_handler.cpp b/src/path_handler.cpp index 9f3eaa44..08a25f50 100644 --- a/src/path_handler.cpp +++ b/src/path_handler.cpp @@ -57,13 +57,14 @@ PathRange PathHandler::getGlobalPlanConsideringBounds( // Find the furthest relevent point on the path to consider within costmap // bounds - auto max_costmap_dist = getMaxCostmapDist(); - + const auto * costmap = costmap_->getCostmap(); + unsigned int mx, my; auto last_point = std::find_if( closest_point, end, [&](const geometry_msgs::msg::PoseStamped & global_plan_pose) { auto distance = euclidean_distance(global_pose, global_plan_pose); - return distance > max_costmap_dist || distance >= prune_distance_; + return distance >= prune_distance_ || !costmap->worldToMap( + global_plan_pose.pose.position.x, global_plan_pose.pose.position.y, mx, my); }); return {closest_point, last_point}; From 050947a4cccda88515fdd103bd8c9a3bf453f862 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Tue, 20 Dec 2022 16:55:48 -0800 Subject: [PATCH 47/53] dynamic obstacle fully supported --- README.md | 1 + include/mppic/critics/path_align_critic.hpp | 1 + include/mppic/tools/utils.hpp | 25 +++++++++++++++++++++ src/critics/path_align_critic.cpp | 13 +++++++++++ 4 files changed, 40 insertions(+) diff --git a/README.md b/README.md index 1b7f1258..4dbc5f2b 100644 --- a/README.md +++ b/README.md @@ -110,6 +110,7 @@ This process is then repeated a number of times and returns a converged solution | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which path align cost stops being considered | | offset_from_furthest | double | Default 20. Checks that the candidate trajectories are sufficiently far along their way tracking the path to apply the alignment critic. This ensures that path alignment is only considered when actually tracking the path, preventing awkward initialization motions preventing the robot from leaving the path to achieve the appropriate heading. | | trajectory_point_step | double | Default 5. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable. | + | max_path_occupancy_ratio | double | Default 0.07 (7%). Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presense of dynamic objects in the scene. | #### Path Angle Critic diff --git a/include/mppic/critics/path_align_critic.hpp b/include/mppic/critics/path_align_critic.hpp index 7d5708b4..30fa4039 100644 --- a/include/mppic/critics/path_align_critic.hpp +++ b/include/mppic/critics/path_align_critic.hpp @@ -48,6 +48,7 @@ class PathAlignCritic : public CriticFunction size_t offset_from_furthest_{0}; int trajectory_point_step_{0}; float threshold_to_consider_{0}; + float max_path_occupancy_ratio_{0}; unsigned int power_{0}; float weight_{0}; }; diff --git a/include/mppic/tools/utils.hpp b/include/mppic/tools/utils.hpp index e8449efb..b1c2c14d 100644 --- a/include/mppic/tools/utils.hpp +++ b/include/mppic/tools/utils.hpp @@ -322,6 +322,31 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) return max_id_by_trajectories; } +/** + * @brief Evaluate closest point idx of data.path which is + * nearset to the start of the trajectory in data.trajectories + * @param data Data to use + * @return Idx of closest path point at start of the trajectories + */ +inline size_t findPathTrajectoryInitialPoint(const CriticData & data) +{ + // First point should be the same for all trajectories from initial conditions + const auto dx = data.path.x - data.trajectories.x(0, 0); + const auto dy = data.path.y - data.trajectories.y(0, 0); + const auto dists = dx * dx + dy * dy; + + double min_distance_by_path = std::numeric_limits::max(); + size_t min_id = 0; + for (size_t j = 0; j < dists.shape(0); j++) { + if (dists(j) < min_distance_by_path) { + min_distance_by_path = dists(j); + min_id = j; + } + } + + return min_id; +} + /** * @brief evaluate path furthest point if it is not set * @param data Data to use diff --git a/src/critics/path_align_critic.cpp b/src/critics/path_align_critic.cpp index d4a4e453..9ca81281 100644 --- a/src/critics/path_align_critic.cpp +++ b/src/critics/path_align_critic.cpp @@ -29,6 +29,7 @@ void PathAlignCritic::initialize() getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 1.0); + getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07); getParam(offset_from_furthest_, "offset_from_furthest", 20); getParam(trajectory_point_step_, "trajectory_point_step", 5); getParam( @@ -43,18 +44,30 @@ 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)) { return; } + // Don't apply when first getting bearing w.r.t. the path utils::setPathFurthestPointIfNotSet(data); if (*data.furthest_reached_path_point < offset_from_furthest_) { return; } + // Don't apply when dynamic obstacles are blocking significant proportions of the local path utils::setPathCostsIfNotSet(data, costmap_ros_); + const size_t closest_initial_path_point = utils::findPathTrajectoryInitialPoint(data); + unsigned int invalid_ctr = 0; + const float range = *data.furthest_reached_path_point - closest_initial_path_point; + for (size_t i = closest_initial_path_point; i < *data.furthest_reached_path_point; i++) { + if (!(*data.path_pts_valid)[i]) {invalid_ctr++;} + if (static_cast(invalid_ctr) / range > max_path_occupancy_ratio_ && invalid_ctr > 2) { + return; + } + } const auto & T_x = data.trajectories.x; const auto & T_y = data.trajectories.y; From a7b267459b95082f7067a745cef0d6a29bbceb82 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 21 Dec 2022 11:03:42 -0800 Subject: [PATCH 48/53] updating defaults --- README.md | 58 ++++++++++++++++-------------- src/critics/obstacles_critic.cpp | 8 ++--- src/critics/path_align_critic.cpp | 4 +-- src/critics/path_angle_critic.cpp | 2 +- src/critics/path_follow_critic.cpp | 4 +-- 5 files changed, 40 insertions(+), 36 deletions(-) diff --git a/README.md b/README.md index 4dbc5f2b..f51e997a 100644 --- a/README.md +++ b/README.md @@ -79,7 +79,7 @@ This process is then repeated a number of times and returns a converged solution #### Goal Angle Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | cost_weight | double | Default 1.0. Weight to apply to critic term. | + | cost_weight | double | Default 3.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | | threshold_to_consider | double | Default 0.40. Minimal distance between robot and goal above which angle goal cost considered. | @@ -95,21 +95,21 @@ This process is then repeated a number of times and returns a converged solution | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. | - | critical_weight | double | Default 1.2. Weight to apply to critic for near collisions closer than `collision_margin_distance` to prevent near collisions **only** as a method of virtually inflating the footprint. This should not be used to generally influence obstacle avoidance away from criticial collisions. | - | repulsion_weight | double | Default 2.0. Weight to apply to critic for generally preferring routes in lower cost space. This is separated from the critical term to allow for fine tuning of obstacle behaviors with path alignment for dynamic scenes without impacting actions which may directly lead to near-collisions. This is applied within the `inflation_radius` distance from obstacles. | - | cost_power | int | Default 2. Power order to apply to term. | - | collision_cost | double | Default 2000.0. Cost to apply to a true collision in a trajectory. | + | critical_weight | double | Default 20.0. Weight to apply to critic for near collisions closer than `collision_margin_distance` to prevent near collisions **only** as a method of virtually inflating the footprint. This should not be used to generally influence obstacle avoidance away from criticial collisions. | + | repulsion_weight | double | Default 1.5. Weight to apply to critic for generally preferring routes in lower cost space. This is separated from the critical term to allow for fine tuning of obstacle behaviors with path alignment for dynamic scenes without impacting actions which may directly lead to near-collisions. This is applied within the `inflation_radius` distance from obstacles. | + | cost_power | int | Default 1. Power order to apply to term. | + | collision_cost | double | Default 10000.0. Cost to apply to a true collision in a trajectory. | | collision_margin_distance | double | Default 0.10. Margin distance from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable. | | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. #### Path Align Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | cost_weight | double | Default 2.0. Weight to apply to critic term. | + | cost_weight | double | Default 10.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which path align cost stops being considered | | offset_from_furthest | double | Default 20. Checks that the candidate trajectories are sufficiently far along their way tracking the path to apply the alignment critic. This ensures that path alignment is only considered when actually tracking the path, preventing awkward initialization motions preventing the robot from leaving the path to achieve the appropriate heading. | - | trajectory_point_step | double | Default 5. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable. | + | trajectory_point_step | double | Default 4. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable. | | max_path_occupancy_ratio | double | Default 0.07 (7%). Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presense of dynamic objects in the scene. | @@ -120,20 +120,20 @@ This process is then repeated a number of times and returns a converged solution | cost_power | int | Default 1. Power order to apply to term. | | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which path angle cost stops being considered | | offset_from_furthest | int | Default 4. Number of path points after furthest one any trajectory achieves to compute path angle relative to. | - | max_angle_to_furthest | double | Default PI/2. Distance between robot and goal above which path angle cost starts being considered | + | max_angle_to_furthest | double | Default 1.2. Angular distance between robot and goal above which path angle cost starts being considered | #### Path Follow Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | cost_weight | double | Default 3.0. Weight to apply to critic term. | + | cost_weight | double | Default 5.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | - | offset_from_furthest | int | Default 10. Number of path points after furthest one any trajectory achieves to drive path tracking relative to. | + | offset_from_furthest | int | Default 6. Number of path points after furthest one any trajectory achieves to drive path tracking relative to. | | threshold_to_consider | float | Default 0.4. Distance between robot and goal above which path follow cost stops being considered | #### Prefer Forward Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | cost_weight | double | Default 3.0. Weight to apply to critic term. | + | cost_weight | double | Default 5.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which prefer forward cost stops being considered | @@ -160,7 +160,7 @@ controller_server: vx_max: 0.5 vx_min: -0.35 vy_max: 0.5 - wz_max: 1.3 + wz_max: 1.9 iteration_count: 1 prune_distance: 1.7 transform_tolerance: 0.1 @@ -188,39 +188,43 @@ controller_server: cost_power: 1 cost_weight: 3.0 threshold_to_consider: 0.4 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.4 ObstaclesCritic: enabled: true - cost_power: 2 - critical_weight: 1.2 - repulsion_weight: 1.2 + cost_power: 1 + repulsion_weight: 1.5 + critical_weight: 20.0 consider_footprint: false - collision_cost: 2000.0 - collision_margin_distance: 0.10 + collision_cost: 10000.0 + collision_margin_distance: 0.1 + near_goal_distance: 0.5 PathAlignCritic: enabled: true cost_power: 1 - cost_weight: 1.0 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 3 threshold_to_consider: 0.40 - trajectory_point_step: 5 offset_from_furthest: 20 PathFollowCritic: enabled: true cost_power: 1 - cost_weight: 3.0 - offset_from_furthest: 10 - threshold_to_consider: 0.40 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 0.6 PathAngleCritic: enabled: true cost_power: 1 cost_weight: 2.0 offset_from_furthest: 4 threshold_to_consider: 0.40 - PreferForwardCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - threshold_to_consider: 0.4 + max_angle_to_furthest: 1.0 # TwirlingCritic: + # enabled: true # twirling_cost_power: 1 # twirling_cost_weight: 10.0 ``` diff --git a/src/critics/obstacles_critic.cpp b/src/critics/obstacles_critic.cpp index 8bda41e1..f53c4193 100644 --- a/src/critics/obstacles_critic.cpp +++ b/src/critics/obstacles_critic.cpp @@ -22,10 +22,10 @@ void ObstaclesCritic::initialize() { auto getParam = parameters_handler_->getParamGetter(name_); getParam(consider_footprint_, "consider_footprint", false); - getParam(power_, "cost_power", 2); - getParam(repulsion_weight_, "repulsion_weight", 4.0); - getParam(critical_weight_, "critical_weight", 1.2); - getParam(collision_cost_, "collision_cost", 2000.0); + getParam(power_, "cost_power", 1); + getParam(repulsion_weight_, "repulsion_weight", 1.5); + getParam(critical_weight_, "critical_weight", 20.0); + getParam(collision_cost_, "collision_cost", 10000.0); getParam(collision_margin_distance_, "collision_margin_distance", 0.10); getParam(near_goal_distance_, "near_goal_distance", 0.5); diff --git a/src/critics/path_align_critic.cpp b/src/critics/path_align_critic.cpp index 9ca81281..3cba36ce 100644 --- a/src/critics/path_align_critic.cpp +++ b/src/critics/path_align_critic.cpp @@ -27,11 +27,11 @@ void PathAlignCritic::initialize() { auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 1.0); + getParam(weight_, "cost_weight", 10.0); getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07); getParam(offset_from_furthest_, "offset_from_furthest", 20); - getParam(trajectory_point_step_, "trajectory_point_step", 5); + getParam(trajectory_point_step_, "trajectory_point_step", 4); getParam( threshold_to_consider_, "threshold_to_consider", 0.40f); diff --git a/src/critics/path_angle_critic.cpp b/src/critics/path_angle_critic.cpp index 9dffded1..558ac225 100644 --- a/src/critics/path_angle_critic.cpp +++ b/src/critics/path_angle_critic.cpp @@ -30,7 +30,7 @@ void PathAngleCritic::initialize() "threshold_to_consider", 0.40f); getParam( max_angle_to_furthest_, - "max_angle_to_furthest", M_PI_2); + "max_angle_to_furthest", 1.2); RCLCPP_INFO( diff --git a/src/critics/path_follow_critic.cpp b/src/critics/path_follow_critic.cpp index 0ed382df..7e390946 100644 --- a/src/critics/path_follow_critic.cpp +++ b/src/critics/path_follow_critic.cpp @@ -27,9 +27,9 @@ void PathFollowCritic::initialize() getParam( threshold_to_consider_, "threshold_to_consider", 0.40f); - getParam(offset_from_furthest_, "offset_from_furthest", 10); + getParam(offset_from_furthest_, "offset_from_furthest", 6); getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 3.0); + getParam(weight_, "cost_weight", 5.0); } void PathFollowCritic::score(CriticData & data) From 056418a89495a8bd570508e65daf8b4991464fb6 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 21 Dec 2022 11:44:59 -0800 Subject: [PATCH 49/53] linting and error handling --- include/mppic/tools/utils.hpp | 4 ++++ src/path_handler.cpp | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/include/mppic/tools/utils.hpp b/include/mppic/tools/utils.hpp index b1c2c14d..68988694 100644 --- a/include/mppic/tools/utils.hpp +++ b/include/mppic/tools/utils.hpp @@ -366,6 +366,10 @@ inline void findPathCosts( CriticData & data, std::shared_ptr costmap_ros) { + if (!costmap_ros) { + return; + } + auto * costmap = costmap_ros->getCostmap(); unsigned int map_x, map_y; const size_t path_segments_count = data.path.x.shape(0) - 1; diff --git a/src/path_handler.cpp b/src/path_handler.cpp index 08a25f50..5c92cb22 100644 --- a/src/path_handler.cpp +++ b/src/path_handler.cpp @@ -64,7 +64,7 @@ PathRange PathHandler::getGlobalPlanConsideringBounds( closest_point, end, [&](const geometry_msgs::msg::PoseStamped & global_plan_pose) { auto distance = euclidean_distance(global_pose, global_plan_pose); return distance >= prune_distance_ || !costmap->worldToMap( - global_plan_pose.pose.position.x, global_plan_pose.pose.position.y, mx, my); + global_plan_pose.pose.position.x, global_plan_pose.pose.position.y, mx, my); }); return {closest_point, last_point}; From 996a93ade778dfd6540a511c8ac4da30af3a1209 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 21 Dec 2022 14:01:24 -0800 Subject: [PATCH 50/53] add additional unit tests --- include/mppic/tools/utils.hpp | 9 ++--- src/optimizer.cpp | 3 +- test/critics_tests.cpp | 45 ++++++++++++++++++++---- test/optimizer_unit_tests.cpp | 1 + test/path_handler_test.cpp | 6 ++-- test/utils_test.cpp | 65 ++++++++++++++++++++++++++++++++++- 6 files changed, 111 insertions(+), 18 deletions(-) diff --git a/include/mppic/tools/utils.hpp b/include/mppic/tools/utils.hpp index 68988694..ca7d67ac 100644 --- a/include/mppic/tools/utils.hpp +++ b/include/mppic/tools/utils.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -366,15 +367,11 @@ inline void findPathCosts( CriticData & data, std::shared_ptr costmap_ros) { - if (!costmap_ros) { - return; - } - auto * costmap = costmap_ros->getCostmap(); unsigned int map_x, map_y; const size_t path_segments_count = data.path.x.shape(0) - 1; - data.path_pts_valid->resize(path_segments_count - 1, false); - for (unsigned int idx = 0; idx < path_segments_count - 1; idx++) { + data.path_pts_valid = std::vector(path_segments_count - 1, false); + for (unsigned int idx = 0; idx < path_segments_count; idx++) { const auto path_x = data.path.x(idx); const auto path_y = data.path.y(idx); if (!costmap->worldToMap(path_x, path_y, map_x, map_y)) { diff --git a/src/optimizer.cpp b/src/optimizer.cpp index d9e19765..81660957 100644 --- a/src/optimizer.cpp +++ b/src/optimizer.cpp @@ -23,7 +23,7 @@ #include #include -#include "nav2_core/exceptions.hpp" +#include "nav2_core/controller_exceptions.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" namespace mppi @@ -190,6 +190,7 @@ void Optimizer::prepare( critics_data_.goal_checker = goal_checker; critics_data_.motion_model = motion_model_; critics_data_.furthest_reached_path_point.reset(); + critics_data_.path_pts_valid.reset(); } void Optimizer::shiftControlSequence() diff --git a/test/critics_tests.cpp b/test/critics_tests.cpp index bb730641..61bcde27 100644 --- a/test/critics_tests.cpp +++ b/test/critics_tests.cpp @@ -418,19 +418,18 @@ TEST(CriticTests, PathFollowCritic) // provide state poses and path close within positional tolerances state.pose.pose.position.x = 1.0; - path.reset(10); - path.x(9) = 0.85; + path.reset(6); + path.x(5) = 0.85; 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(9) = 0.15; + path.x(5) = 0.15; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 450.0, 1e-2); // 0.15 * 3 weight * 1000 + EXPECT_NEAR(xt::sum(costs, immediate)(), 750.0, 1e-2); // 0.15 * 5 weight * 1000 } - TEST(CriticTests, PathAlignCritic) { // Standard preamble @@ -486,7 +485,10 @@ TEST(CriticTests, PathAlignCritic) EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); // provide state pose and path far enough to enable, with data to pass condition + // and with a valid path to pass invalid path condition state.pose.pose.position.x = 0.0; + data.path_pts_valid.reset(); // Recompute on new path + path.reset(22); path.x(0) = 0; path.x(1) = 0.1; path.x(2) = 0.2; @@ -497,8 +499,37 @@ TEST(CriticTests, PathAlignCritic) path.x(7) = 0.7; path.x(8) = 0.8; path.x(9) = 0.9; + path.x(10) = 0.9; + path.x(11) = 0.9; + path.x(12) = 0.9; + path.x(13) = 0.9; + path.x(14) = 0.9; + path.x(15) = 0.9; + path.x(16) = 0.9; + path.x(17) = 0.9; + path.x(18) = 0.9; + path.x(19) = 0.9; + path.x(20) = 0.9; + path.x(21) = 0.9; generated_trajectories.x = 0.66 * xt::ones({1000, 30}); critic.score(data); - // 0.04 * 1000 * 1 weight * 6 num pts eval / 6 normalization term - EXPECT_NEAR(xt::sum(costs, immediate)(), 40.0, 1e-2); + // 0.04 * 1000 * 10 weight * 4 num pts eval / 4 normalization term + EXPECT_NEAR(xt::sum(costs, immediate)(), 400.0, 1e-2); + + // provide state pose and path far enough to enable, with data to pass condition + // but path is blocked in collision + auto * costmap = costmap_ros->getCostmap(); + // island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution + for (unsigned int i = 11; i <= 30; ++i) { // 1.1m-3m + for (unsigned int j = 11; j <= 30; ++j) { // 1.1m-3m + costmap->setCost(i, j, 254); + } + } + + data.path_pts_valid.reset(); // Recompute on new path + xt::xtensor costs = xt::zeros({1000}); + path.x = 1.5 * xt::ones({22}); + path.y = 1.5 * xt::ones({22}); + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); } diff --git a/test/optimizer_unit_tests.cpp b/test/optimizer_unit_tests.cpp index 318a3d8a..e5d68d08 100644 --- a/test/optimizer_unit_tests.cpp +++ b/test/optimizer_unit_tests.cpp @@ -132,6 +132,7 @@ class OptimizerTester : public Optimizer EXPECT_FALSE(critics_data_.fail_flag); // should be reset EXPECT_FALSE(critics_data_.motion_model->isHolonomic()); // object is valid + diff drive EXPECT_FALSE(critics_data_.furthest_reached_path_point.has_value()); // val is not set + EXPECT_FALSE(critics_data_.path_pts_valid.has_value()); // val is not set EXPECT_EQ(state_.pose.pose.position.x, 999); EXPECT_EQ(state_.speed.linear.y, 4.0); EXPECT_EQ(path_.x.shape(0), 17u); diff --git a/test/path_handler_test.cpp b/test/path_handler_test.cpp index b502737a..ca0d217a 100644 --- a/test/path_handler_test.cpp +++ b/test/path_handler_test.cpp @@ -121,7 +121,7 @@ TEST(PathHandlerTests, TestBounds) auto [closest, furthest] = handler.getGlobalPlanConsideringBoundsWrapper(robot_pose); auto & path_in = handler.getPath(); EXPECT_EQ(closest - path_in.poses.begin(), 25); - EXPECT_EQ(furthest - path_in.poses.begin(), 27); + EXPECT_EQ(furthest - path_in.poses.begin(), 25); handler.pruneGlobalPlanWrapper(closest); auto & path_pruned = handler.getPath(); EXPECT_EQ(path_pruned.poses.size(), 75u); @@ -150,10 +150,10 @@ TEST(PathHandlerTests, TestTransforms) geometry_msgs::msg::PoseStamped robot_pose, output_pose; robot_pose.header.frame_id = "map"; - robot_pose.pose.position.x = 25.0; + robot_pose.pose.position.x = 2.5; EXPECT_TRUE(handler.transformPoseWrapper("map", robot_pose, output_pose)); - EXPECT_EQ(output_pose.pose.position.x, 25.0); + EXPECT_EQ(output_pose.pose.position.x, 2.5); EXPECT_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose), std::runtime_error); handler.setPath(path); diff --git a/test/utils_test.cpp b/test/utils_test.cpp index 042be2e0..496f1be5 100644 --- a/test/utils_test.cpp +++ b/test/utils_test.cpp @@ -197,7 +197,7 @@ TEST(UtilsTests, AnglesTests) EXPECT_NEAR(posePointAngle(pose, point_x, point_y), 0.0, 1e-6); } -TEST(UtilsTests, FurthestReachedPoint) +TEST(UtilsTests, FurthestAndClosestReachedPoint) { models::State state; models::Trajectories generated_trajectories; @@ -238,6 +238,69 @@ TEST(UtilsTests, FurthestReachedPoint) {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references EXPECT_EQ(findPathFurthestReachedPoint(data3), 5u); + EXPECT_EQ(findPathTrajectoryInitialPoint(data3), 5u); +} + +TEST(UtilsTests, findPathCosts) +{ + models::State state; + models::Trajectories generated_trajectories; + models::Path path; + xt::xtensor costs; + float model_dt = 0.1; + + CriticData data = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + std::nullopt, std::nullopt}; /// Caution, keep references + + // Test not set if already set, should not change + data.path_pts_valid = std::vector(10, false); + for (unsigned int i = 0; i != 10; i++) { + (*data.path_pts_valid)[i] = false; + } + EXPECT_TRUE(data.path_pts_valid); + setPathCostsIfNotSet(data, nullptr); + EXPECT_EQ(data.path_pts_valid->size(), 10u); + + CriticData data3 = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + std::nullopt, std::nullopt}; /// Caution, keep references + + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + auto * costmap = costmap_ros->getCostmap(); + // island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution + for (unsigned int i = 10; i <= 30; ++i) { // 1m-3m + for (unsigned int j = 10; j <= 30; ++j) { // 1m-3m + costmap->setCost(i, j, 254); + } + } + for (unsigned int i = 40; i <= 45; ++i) { // 4m-4.5m + for (unsigned int j = 45; j <= 45; ++j) { // 4m-4.5m + costmap->setCost(i, j, 253); + } + } + + path.reset(50); + path.x(1) = 999999999; // OFF COSTMAP + path.y(1) = 999999999; + path.x(10) = 1.5; // IN LETHAL + path.y(10) = 1.5; + path.x(20) = 4.2; // IN INFLATED + path.y(20) = 4.2; + + // This should be evaluated and have real outputs now + setPathCostsIfNotSet(data3, costmap_ros); + EXPECT_TRUE(data3.path_pts_valid.has_value()); + for (unsigned int i = 0; i != path.x.shape(0) - 1; i++) { + if (i == 1 || i == 10) { + EXPECT_FALSE((*data3.path_pts_valid)[i]); + } else { + EXPECT_TRUE((*data3.path_pts_valid)[i]); + } + } } TEST(UtilsTests, SmootherTest) From 13ca43d79034ed67ffcacce26209c109cb160e43 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 21 Dec 2022 14:02:28 -0800 Subject: [PATCH 51/53] typo fix --- test/critics_tests.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/critics_tests.cpp b/test/critics_tests.cpp index 61bcde27..ce033600 100644 --- a/test/critics_tests.cpp +++ b/test/critics_tests.cpp @@ -527,7 +527,7 @@ TEST(CriticTests, PathAlignCritic) } data.path_pts_valid.reset(); // Recompute on new path - xt::xtensor costs = xt::zeros({1000}); + costs = xt::zeros({1000}); path.x = 1.5 * xt::ones({22}); path.y = 1.5 * xt::ones({22}); critic.score(data); From 0807e2205ce6d2182caf58f433c6209adf74e59e Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 21 Dec 2022 14:36:36 -0800 Subject: [PATCH 52/53] update defaults --- README.md | 14 +++++++------- src/optimizer.cpp | 14 +++++++------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index f51e997a..759d8792 100644 --- a/README.md +++ b/README.md @@ -38,18 +38,18 @@ This process is then repeated a number of times and returns a converged solution | motion_model | string | Default: DiffDrive. Type of model [DiffDrive, Omni, Ackermann]. | | critics | string | Default: None. Critics (plugins) names | | iteration_count | int | Default 1. Iteration count in MPPI algorithm. Recommend to keep as 1 and prefer more batches. | - | batch_size | int | Default 400. Count of randomly sampled candidate trajectories | - | time_steps | int | Default 15. Number of time steps (points) in each sampled trajectory | - | model_dt | double | Default: 0.1. Time interval (s) between two sampled points in trajectories. | + | batch_size | int | Default 1000. Count of randomly sampled candidate trajectories | + | time_steps | int | Default 56. Number of time steps (points) in each sampled trajectory | + | model_dt | double | Default: 0.05. Time interval (s) between two sampled points in trajectories. | | vx_std | double | Default 0.2. Sampling standart deviation for VX | | vy_std | double | Default 0.2. Sampling standart deviation for VY | - | wx_std | double | Default 1.0. Sampling standart deviation for WX | + | wx_std | double | Default 0.4. Sampling standart deviation for WX | | vx_max | double | Default 0.5. Max VX (m/s) | | vy_max | double | Default 0.5. Max VY in either direction, if holonomic. (m/s) | | vx_min | double | Default -0.35. Min VX (m/s) | - | wz_max | double | Default 1.3. Max WZ (rad/s) | - | temperature | double | Default: 0.35. Selectiveness of trajectories by their costs (The closer this value to 0, the "more" we take in considiration controls with less cost), 0 mean use control with best cost, huge value will lead to just taking mean of all trajectories without cost consideration | - | gamma | double | Default: 0.1. A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default of `0.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information. | + | wz_max | double | Default 1.9. Max WZ (rad/s) | + | temperature | double | Default: 0.3. Selectiveness of trajectories by their costs (The closer this value to 0, the "more" we take in considiration controls with less cost), 0 mean use control with best cost, huge value will lead to just taking mean of all trajectories without cost consideration | + | gamma | double | Default: 0.015. A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default of `0.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information. | | visualize | bool | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging. | | retry_attempt_limit | int | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure. | #### Trajectory Visualizer diff --git a/src/optimizer.cpp b/src/optimizer.cpp index 81660957..4ac0b517 100644 --- a/src/optimizer.cpp +++ b/src/optimizer.cpp @@ -66,19 +66,19 @@ void Optimizer::getParams() auto & s = settings_; auto getParam = parameters_handler_->getParamGetter(name_); auto getParentParam = parameters_handler_->getParamGetter(""); - getParam(s.model_dt, "model_dt", 0.1f); - getParam(s.time_steps, "time_steps", 15); - getParam(s.batch_size, "batch_size", 400); + getParam(s.model_dt, "model_dt", 0.05f); + getParam(s.time_steps, "time_steps", 56); + getParam(s.batch_size, "batch_size", 1000); getParam(s.iteration_count, "iteration_count", 1); - getParam(s.temperature, "temperature", 0.15f); - getParam(s.gamma, "gamma", 0.10f); + getParam(s.temperature, "temperature", 0.3f); + getParam(s.gamma, "gamma", 0.015f); getParam(s.base_constraints.vx_max, "vx_max", 0.5); getParam(s.base_constraints.vx_min, "vx_min", -0.35); getParam(s.base_constraints.vy, "vy_max", 0.5); - getParam(s.base_constraints.wz, "wz_max", 1.3); + getParam(s.base_constraints.wz, "wz_max", 1.9); getParam(s.sampling_std.vx, "vx_std", 0.2); getParam(s.sampling_std.vy, "vy_std", 0.2); - getParam(s.sampling_std.wz, "wz_std", 1.0); + getParam(s.sampling_std.wz, "wz_std", 0.4); getParam(s.retry_attempt_limit, "retry_attempt_limit", 1); getParam(motion_model_name, "motion_model", std::string("DiffDrive")); From ce2e8d69bdd40d57bb97ffc329ee64305dc0ebbd Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 21 Dec 2022 15:38:18 -0800 Subject: [PATCH 53/53] adding a couple of line overlooks --- src/optimizer.cpp | 5 ++--- test/optimizer_unit_tests.cpp | 1 + 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/optimizer.cpp b/src/optimizer.cpp index 4ac0b517..173ba341 100644 --- a/src/optimizer.cpp +++ b/src/optimizer.cpp @@ -409,9 +409,8 @@ void Optimizer::setMotionModel(const std::string & model) } else { throw std::runtime_error( std::string( - "Model %s is not valid! Valid options are DiffDrive, Omni, " - "or Ackermann", - model.c_str())); + "Model " + model + " is not valid! Valid options are DiffDrive, Omni, " + "or Ackermann")); } } diff --git a/test/optimizer_unit_tests.cpp b/test/optimizer_unit_tests.cpp index e5d68d08..f65f5fa6 100644 --- a/test/optimizer_unit_tests.cpp +++ b/test/optimizer_unit_tests.cpp @@ -245,6 +245,7 @@ TEST(OptimizerTests, BasicInitializedFunctions) EXPECT_EQ(traj.shape(0), 50u); EXPECT_EQ(traj.shape(1), 3u); + optimizer_tester.reset(); optimizer_tester.shutdown(); }