diff --git a/CMakeLists.txt b/CMakeLists.txt index a426031..7b3f702 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 00200f2..4dbc5f2 100644 --- a/README.md +++ b/README.md @@ -88,27 +88,30 @@ 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. | + | 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 | Parameter | Type | Definition | @@ -125,7 +128,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 +182,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 +191,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 @@ -229,7 +233,28 @@ 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. +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_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 + +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. diff --git a/include/mppic/controller.hpp b/include/mppic/controller.hpp index 9e382db..d2a126c 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/critic_data.hpp b/include/mppic/critic_data.hpp index 075ebe0..bcc742f 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 e5f5d96..74b7d1f 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 diff --git a/include/mppic/critics/goal_critic.hpp b/include/mppic/critics/goal_critic.hpp index 9868656..006dbb1 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/obstacles_critic.hpp b/include/mppic/critics/obstacles_critic.hpp index 353da07..161e4dd 100644 --- a/include/mppic/critics/obstacles_critic.hpp +++ b/include/mppic/critics/obstacles_critic.hpp @@ -26,19 +26,12 @@ 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 + * @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 +93,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/include/mppic/critics/path_align_critic.hpp b/include/mppic/critics/path_align_critic.hpp index 03a9810..30fa403 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,10 @@ 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}; - + float max_path_occupancy_ratio_{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 e359f8d..456b151 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 566bc85..a5937ce 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 8dbd057..8af11e3 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 @@ -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 4482a9a..b1c2c14 100644 --- a/include/mppic/tools/utils.hpp +++ b/include/mppic/tools/utils.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -311,7 +312,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; } @@ -321,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 @@ -332,6 +358,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 @@ -349,22 +427,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 fa0aa60..579bbb0 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 bdafc0e..0a0cd38 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 ef3367e..4e89ff8 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/obstacles_critic.cpp b/src/critics/obstacles_critic.cpp index 1fd829b..8bda41e 100644 --- a/src/critics/obstacles_critic.cpp +++ b/src/critics/obstacles_critic.cpp @@ -23,19 +23,20 @@ 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(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( @@ -58,6 +59,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 +69,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,59 +104,58 @@ 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)); + } else if (!near_goal) { // Generally prefer trajectories further from obstacles + 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; } -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 = @@ -173,6 +174,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_align_critic.cpp b/src/critics/path_align_critic.cpp index d5d8622..9ca8128 100644 --- a/src/critics/path_align_critic.cpp +++ b/src/critics/path_align_critic.cpp @@ -20,14 +20,18 @@ 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(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( threshold_to_consider_, "threshold_to_consider", 0.40f); @@ -40,89 +44,76 @@ 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; } - using namespace xt::placeholders; // NOLINT - using xt::evaluation_strategy::immediate; - - // see http://paulbourke.net/geometry/pointlineplane/ + // 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; + } - // P3 points from which we calculate distance to segments - const auto & P3_x = data.trajectories.x; - const auto & P3_y = data.trajectories.y; + // 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 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; + auto && cost = xt::xtensor::from_shape({data.costs.shape(0)}); - 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; - }; + if (path_segments_count < 1) { + return; + } - 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) { + 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; min_s = s; - min_dist_sq = dist; } } - max_s = std::max(max_s, min_s); - mean_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) = 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 c9f48a6..0ed382d 100644 --- a/src/critics/path_follow_critic.cpp +++ b/src/critics/path_follow_critic.cpp @@ -25,31 +25,40 @@ 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; - } + 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_, data.path.x.shape(0) - 1); + *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 = 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 8e8ba1c..d9e1976 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/src/path_handler.cpp b/src/path_handler.cpp index 9f3eaa4..08a25f5 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}; diff --git a/test/critic_manager_test.cpp b/test/critic_manager_test.cpp index 48c1b3f..7e4a176 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 f310a73..bb73064 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 @@ -191,14 +194,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 @@ -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; @@ -375,3 +381,124 @@ 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, + 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, + 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 d76b3a5..b09dd15 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 d22ee83..88f7be2 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 19b8b2f..318a3d8 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 86da88c..042be2e 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,10 +236,8 @@ 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); - data3.furthest_reached_path_point = 5u; - EXPECT_EQ(getPathRatioReached(data3), 0.5); } TEST(UtilsTests, SmootherTest)