diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp index 2646c98fd9e..c285a14b664 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp @@ -144,7 +144,6 @@ class CostCritic : public CriticFunction std::string inflation_layer_name_; unsigned int power_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp index 58c108b21cd..08ec354cf3b 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp @@ -46,7 +46,6 @@ class GoalAngleCritic : public CriticFunction float threshold_to_consider_{0}; unsigned int power_{0}; float weight_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp index 6b248f29945..8fb8fb688ae 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp @@ -46,7 +46,6 @@ class GoalCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; float threshold_to_consider_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp index 23fb3db2ca0..31b6a3d0df8 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -97,7 +97,6 @@ class ObstaclesCritic : public CriticFunction unsigned int power_{0}; float repulsion_weight_, critical_weight_{0}; - bool enforce_path_inversion_{false}; std::string inflation_layer_name_; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index 133447a8774..f7ad2fda6a9 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -52,7 +52,6 @@ class PathAlignCritic : public CriticFunction bool use_path_orientations_{false}; unsigned int power_{0}; float weight_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp index 85b9d48eb83..b137270aba9 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp @@ -81,7 +81,6 @@ class PathAngleCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp index aacec055ccf..1ccd08c32fe 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp @@ -53,7 +53,6 @@ class PathFollowCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp index 811ba9ae9a6..e17ad235e5c 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp @@ -45,7 +45,6 @@ class PreferForwardCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; float threshold_to_consider_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp index bc40b194404..3e316a567e0 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp @@ -44,7 +44,6 @@ class TwirlingCritic : public CriticFunction protected: unsigned int power_{0}; float weight_{0}; - bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp index a8c81c3b07d..50f9d400a91 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp @@ -40,6 +40,7 @@ struct State geometry_msgs::msg::PoseStamped pose; geometry_msgs::msg::Twist speed; + float local_path_length; /** * @brief Reset state data diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index e70cfcab786..1914abde647 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -33,6 +33,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/path.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_mppi_controller/models/optimizer_settings.hpp" #include "nav2_mppi_controller/motion_models.hpp" diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index f518f16e169..4a3cc6b7c06 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -243,81 +243,6 @@ inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path) return pathPose; } -/** - * @brief Get the target pose to be evaluated by the critic - * @param data Data to use - * @param enforce_path_inversion True to return the cusp point (last pose of the path) - * instead of the original goal - * @return geometry_msgs::msg::Pose Target pose for the critic - */ -inline geometry_msgs::msg::Pose getCriticGoal( - const CriticData & data, - bool enforce_path_inversion) -{ - if (enforce_path_inversion) { - return getLastPathPose(data.path); - } else { - return data.goal; - } -} - -/** - * @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 goal Goal pose - * @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, - const geometry_msgs::msg::Pose & goal) -{ - if (goal_checker) { - geometry_msgs::msg::Pose pose_tolerance; - geometry_msgs::msg::Twist velocity_tolerance; - goal_checker->getTolerances(pose_tolerance, velocity_tolerance); - - const auto pose_tolerance_sq = pose_tolerance.position.x * pose_tolerance.position.x; - - auto dx = robot.position.x - goal.position.x; - auto dy = robot.position.y - goal.position.y; - - auto dist_sq = dx * dx + dy * dy; - - if (dist_sq < pose_tolerance_sq) { - return true; - } - } - - 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 goal Goal pose - * @return bool If robot is within tolerance to the goal - */ -inline bool withinPositionGoalTolerance( - float pose_tolerance, - const geometry_msgs::msg::Pose & robot, - const geometry_msgs::msg::Pose & goal) -{ - const double & dist_sq = - std::pow(goal.position.x - robot.position.x, 2) + - std::pow(goal.position.y - robot.position.y, 2); - - const float pose_tolerance_sq = pose_tolerance * pose_tolerance; - - if (dist_sq < pose_tolerance_sq) { - return true; - } - - return false; -} - /** * @brief normalize * Normalizes the angle to be -M_PIF circle to +M_PIF circle diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index dedf910187e..6b15d992ca8 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -23,8 +23,6 @@ namespace mppi::critics void CostCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); getParam(consider_footprint_, "consider_footprint", false); getParam(power_, "cost_power", 1); @@ -135,8 +133,6 @@ void CostCritic::score(CriticData & data) return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - // Setup cost information for various parts of the critic is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); auto * costmap = collision_checker_.getCostmap(); @@ -153,7 +149,7 @@ void CostCritic::score(CriticData & data) // 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, goal)) { + if (data.state.local_path_length < near_goal_distance_) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 8ac899140f8..8792df2aaa2 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -20,8 +20,6 @@ namespace mppi::critics void GoalAngleCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 3.0f); @@ -36,17 +34,11 @@ void GoalAngleCritic::initialize() void GoalAngleCritic::score(CriticData & data) { - if (!enabled_) { + if (!enabled_ || data.state.local_path_length > threshold_to_consider_) { return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - - if (!utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) - { - return; - } + geometry_msgs::msg::Pose goal = utils::getLastPathPose(data.path); double goal_yaw = tf2::getYaw(goal.orientation); diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 927ef950de8..9889ef3f071 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -22,10 +22,7 @@ namespace mppi::critics void GoalCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); - getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0f); getParam(threshold_to_consider_, "threshold_to_consider", 1.4f); @@ -37,17 +34,11 @@ void GoalCritic::initialize() void GoalCritic::score(CriticData & data) { - if (!enabled_) { + if (!enabled_ || data.state.local_path_length > threshold_to_consider_) { return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - - if (!utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) - { - return; - } + geometry_msgs::msg::Pose goal = utils::getLastPathPose(data.path); auto goal_x = goal.position.x; auto goal_y = goal.position.y; diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index db2972dfc9e..0577db0a76f 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -24,8 +24,6 @@ namespace mppi::critics void ObstaclesCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); getParam(consider_footprint_, "consider_footprint", false); getParam(power_, "cost_power", 1); @@ -130,11 +128,9 @@ void ObstaclesCritic::score(CriticData & data) possible_collision_cost_ = findCircumscribedCost(costmap_ros_); } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - // 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, goal)) { + if (data.state.local_path_length < near_goal_distance_) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 3ea13c641ab..befa9e42eb9 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -20,12 +20,9 @@ namespace mppi::critics void PathAlignCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 10.0f); - getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07f); getParam(offset_from_furthest_, "offset_from_furthest", 20); getParam(trajectory_point_step_, "trajectory_point_step", 4); @@ -42,16 +39,7 @@ void PathAlignCritic::initialize() void PathAlignCritic::score(CriticData & data) { - if (!enabled_) { - return; - } - - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - - // Don't apply close to goal, let the goal critics take over - if (utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) - { + if (!enabled_ || data.state.local_path_length < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index 96ceae1b93b..6f42d33b042 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -23,7 +23,6 @@ namespace mppi::critics void PathAngleCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); float vx_min; getParentParam(vx_min, "vx_min", -0.35); if (fabs(vx_min) < 1e-6f) { // zero @@ -62,15 +61,7 @@ void PathAngleCritic::initialize() void PathAngleCritic::score(CriticData & data) { - if (!enabled_) { - return; - } - - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - - if (utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) - { + if (!enabled_ || data.state.local_path_length < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index 4f8c1c70eec..240c49725d6 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -22,10 +22,7 @@ namespace mppi::critics void PathFollowCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); - getParam( threshold_to_consider_, "threshold_to_consider", 1.4f); @@ -40,12 +37,7 @@ void PathFollowCritic::score(CriticData & data) return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - - if (data.path.x.size() < 2 || - utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) - { + if (data.path.x.size() < 2 || data.state.local_path_length < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 47c84321559..20c58324365 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -22,8 +22,6 @@ namespace mppi::critics void PreferForwardCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0f); @@ -41,11 +39,7 @@ void PreferForwardCritic::score(CriticData & data) return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); - - if (utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, goal)) - { + if (data.state.local_path_length < threshold_to_consider_) { return; } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index c9cfc79233c..2c6842c3a26 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -22,10 +22,7 @@ namespace mppi::critics void TwirlingCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); - auto getParam = parameters_handler_->getParamGetter(name_); - getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 10.0f); @@ -39,12 +36,14 @@ void TwirlingCritic::score(CriticData & data) return; } - geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + if (data.goal_checker != nullptr) { + geometry_msgs::msg::Pose pose_tolerance; + geometry_msgs::msg::Twist velocity_tolerance; + data.goal_checker->getTolerances(pose_tolerance, velocity_tolerance); - if (utils::withinPositionGoalTolerance( - data.goal_checker, data.state.pose.pose, goal)) - { - return; + if (data.state.local_path_length < pose_tolerance.position.x) { + return; + } } if (power_ > 1u) { diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 97a16249814..013b29de502 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -257,6 +257,7 @@ void Optimizer::prepare( { state_.pose = robot_pose; state_.speed = robot_speed; + state_.local_path_length = nav2_util::geometry_utils::calculate_path_length(plan); path_ = utils::toTensor(plan); costs_.setZero(); goal_ = goal; diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 813cba14318..88e823019a7 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -251,16 +251,19 @@ TEST(CriticTests, GoalAngleCritic) goal.orientation.y = 0.0; goal.orientation.z = 1.0; goal.orientation.w = 0.0; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0, 1e-6); // Let's move it even closer, just to be sure it still doesn't trigger state.pose.pose.position.x = 9.2; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0, 1e-6); // provide state pose and path below `threshold_to_consider` to consider state.pose.pose.position.x = 9.7; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_GT(costs.sum(), 0); EXPECT_NEAR(costs(0), 9.42, 0.02); // (3.14 - 0.0) * 3.0 weight @@ -305,6 +308,7 @@ TEST(CriticTests, GoalCritic) path.x(9) = 10.0; path.y(9) = 0.0; goal.position.x = 10.0; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs(2), 0.0, 1e-6); // (0 * 5.0 weight EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // Should all be 0 * 1000 @@ -314,6 +318,7 @@ TEST(CriticTests, GoalCritic) path.x(9) = 0.5; path.y(9) = 0.0; goal.position.x = 0.5; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs(2), 2.5, 1e-6); // (sqrt(10.0 * 10.0) * 5.0 weight EXPECT_NEAR(costs.sum(), 2500.0, 1e-3); // should be 2.5 * 1000 @@ -360,12 +365,14 @@ TEST(CriticTests, PathAngleCritic) state.pose.pose.position.y = 0.0; path.x(9) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 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; goal.position.x = 0.95; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); 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; @@ -478,12 +485,14 @@ TEST(CriticTests, PreferForwardCritic) state.pose.pose.position.x = 1.0; path.x(9) = 10.0; goal.position.x = 10.0; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f); // provide state pose and path close to trigger behavior but with all forward motion path.x(9) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); state.vx.setOnes(); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f); @@ -536,12 +545,14 @@ TEST(CriticTests, TwirlingCritic) state.pose.pose.position.x = 1.0; path.x(9) = 10.0; goal.position.x = 10.0; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path close to trigger behavior but with no angular variation path.x(9) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); state.wz.setZero(); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -601,6 +612,7 @@ TEST(CriticTests, PathFollowCritic) state.pose.pose.position.x = 2.0; path.x(5) = 1.8; goal.position.x = 1.8; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -608,6 +620,7 @@ TEST(CriticTests, PathFollowCritic) // pose differential is (0, 0) and (0.15, 0) path.x(5) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 750.0, 1e-2); // 0.15 * 5 weight * 1000 } @@ -653,6 +666,7 @@ TEST(CriticTests, PathAlignCritic) state.pose.pose.position.x = 1.0; path.x(9) = 0.85; goal.position.x = 0.85; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -660,6 +674,7 @@ TEST(CriticTests, PathAlignCritic) // but data furthest point reached is 0 and offset default is 20, so returns path.x(9) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -668,6 +683,7 @@ TEST(CriticTests, PathAlignCritic) *data.furthest_reached_path_point = 21; path.x(9) = 0.15; goal.position.x = 0.15; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -699,6 +715,7 @@ TEST(CriticTests, PathAlignCritic) path.x(20) = 0.9; path.x(21) = 0.9; goal.position.x = 0.9; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); generated_trajectories.x.setConstant(0.66f); critic.score(data); // 0.66 * 1000 * 10 weight * 6 num pts eval / 6 normalization term @@ -719,6 +736,7 @@ TEST(CriticTests, PathAlignCritic) path.x.setConstant(1.5f); path.y.setConstant(1.5f); goal.position.x = 1.5; + state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); } diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index 733229b77df..c88422f3a84 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -22,6 +22,7 @@ #include #include #include "tf2_ros/buffer.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_mppi_controller/optimizer.hpp" #include "nav2_mppi_controller/tools/parameters_handler.hpp" @@ -79,7 +80,8 @@ TEST_P(OptimizerSuite, OptimizerTest) { auto goal = path.poses.back().pose; nav2_core::GoalChecker * dummy_goal_checker{nullptr}; - auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, goal, dummy_goal_checker); + auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, goal, + dummy_goal_checker); EXPECT_GT(trajectory.rows(), 0); EXPECT_GT(trajectory.cols(), 0); } diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 6f70e19cfa5..70733e37db4 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -116,56 +116,6 @@ TEST(UtilsTests, ConversionTests) 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; - - nav_msgs::msg::Path path; - path.poses.resize(2); - geometry_msgs::msg::Pose & goal = path.poses.back().pose; - - // Create CriticData with state and goal initialized - models::State state; - state.pose.pose = pose; - models::Trajectories generated_trajectories; - models::Path path_critic; - Eigen::ArrayXf costs; - float model_dt; - CriticData data = { - state, generated_trajectories, path_critic, goal, - costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; - - // Test not in tolerance - goal.position.x = 0.0; - goal.position.y = 0.0; - EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, goal)); - EXPECT_FALSE(withinPositionGoalTolerance(0.25, pose, goal)); - - // Test in tolerance - goal.position.x = 9.8; - goal.position.y = 0.95; - EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal)); - EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal)); - - goal.position.x = 10.0; - goal.position.y = 0.76; - EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal)); - EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal)); - - goal.position.x = 9.76; - goal.position.y = 1.0; - EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal)); - EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal)); - - delete goal_checker; - goal_checker = nullptr; - EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, goal)); -} - TEST(UtilsTests, AnglesTests) { // Test angle normalization by creating insane angles @@ -650,61 +600,6 @@ TEST(UtilsTests, getLastPathPoseTest) EXPECT_NEAR(last_path_pose.orientation.w, 0.0, 1e-3); } -TEST(UtilsTests, getCriticGoalTest) -{ - geometry_msgs::msg::Pose pose; - pose.position.x = 10.0; - pose.position.y = 1.0; - - nav_msgs::msg::Path path; - path.poses.resize(10); - path.poses[9].pose.position.x = 5.0; - path.poses[9].pose.position.y = 50.0; - path.poses[9].pose.orientation.x = 0.0; - path.poses[9].pose.orientation.y = 0.0; - path.poses[9].pose.orientation.z = 1.0; - path.poses[9].pose.orientation.w = 0.0; - - geometry_msgs::msg::Pose goal; - goal.position.x = 6.0; - goal.position.y = 60.0; - goal.orientation.x = 0.0; - goal.orientation.y = 0.0; - goal.orientation.z = 0.0; - goal.orientation.w = 1.0; - - // Create CriticData with state and goal initialized - models::State state; - state.pose.pose = pose; - models::Trajectories generated_trajectories; - models::Path path_t = toTensor(path); - Eigen::ArrayXf costs; - float model_dt; - CriticData data = { - state, generated_trajectories, path_t, goal, - costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; - - bool enforce_path_inversion = true; - geometry_msgs::msg::Pose target_goal = utils::getCriticGoal(data, enforce_path_inversion); - - EXPECT_EQ(target_goal.position.x, 5); - EXPECT_EQ(target_goal.position.y, 50); - EXPECT_NEAR(target_goal.orientation.x, 0.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.y, 0.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.z, 1.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.w, 0.0, 1e-3); - - enforce_path_inversion = false; - target_goal = utils::getCriticGoal(data, enforce_path_inversion); - - EXPECT_EQ(target_goal.position.x, 6); - EXPECT_EQ(target_goal.position.y, 60); - EXPECT_NEAR(target_goal.orientation.x, 0.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.y, 0.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.z, 0.0, 1e-3); - EXPECT_NEAR(target_goal.orientation.w, 1.0, 1e-3); -} - int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv);