diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp index 05adb417911..8f06566eae2 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp @@ -38,14 +38,15 @@ namespace mppi /** * @struct mppi::CriticData - * @brief Data to pass to critics for scoring, including state, trajectories, path, costs, and - * important parameters to share + * @brief Data to pass to critics for scoring, including state, trajectories, + * pruned path, global goal, costs, and important parameters to share */ struct CriticData { const models::State & state; const models::Trajectories & trajectories; const models::Path & path; + const geometry_msgs::msg::Pose & goal; xt::xtensor & costs; float & model_dt; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index a985da7cb75..80870dbebac 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -96,7 +96,7 @@ class Optimizer 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); + const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker); /** * @brief Get the trajectories generated in a cycle for visualization @@ -138,7 +138,8 @@ class Optimizer 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); + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker); /** * @brief Obtain the main controller's parameters @@ -256,10 +257,12 @@ class Optimizer std::array control_history_; models::Trajectories generated_trajectories_; models::Path path_; + geometry_msgs::msg::Pose goal_; xt::xtensor costs_; - CriticData critics_data_ = - {state_, generated_trajectories_, path_, costs_, settings_.model_dt, false, nullptr, nullptr, + CriticData critics_data_ = { + state_, generated_trajectories_, path_, goal_, + costs_, settings_.model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp index eeb28fa01b3..56b54af705d 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp @@ -90,6 +90,12 @@ class PathHandler */ nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose); + /** + * @brief Get the global goal pose transformed to the local frame + * @return Transformed goal pose + */ + geometry_msgs::msg::PoseStamped getTransformedGoal(); + protected: /** * @brief Transform a pose to another frame 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 a7ca647aa88..0af34c4afe1 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -204,18 +204,14 @@ inline models::Path toTensor(const nav_msgs::msg::Path & path) * @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 retrieve goal pose from + * @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 models::Path & path) + const geometry_msgs::msg::Pose & goal) { - const auto goal_idx = path.x.shape(0) - 1; - const auto goal_x = path.x(goal_idx); - const auto goal_y = path.y(goal_idx); - if (goal_checker) { geometry_msgs::msg::Pose pose_tolerance; geometry_msgs::msg::Twist velocity_tolerance; @@ -223,8 +219,8 @@ inline bool withinPositionGoalTolerance( const auto pose_tolerance_sq = pose_tolerance.position.x * pose_tolerance.position.x; - auto dx = robot.position.x - goal_x; - auto dy = robot.position.y - goal_y; + auto dx = robot.position.x - goal.position.x; + auto dy = robot.position.y - goal.position.y; auto dist_sq = dx * dx + dy * dy; @@ -240,25 +236,20 @@ inline bool withinPositionGoalTolerance( * @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 retrieve goal pose from + * @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 models::Path & path) + const geometry_msgs::msg::Pose & goal) { - const auto goal_idx = path.x.shape(0) - 1; - const float goal_x = path.x(goal_idx); - const float goal_y = path.y(goal_idx); + 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; - const float dx = static_cast(robot.position.x) - goal_x; - const float dy = static_cast(robot.position.y) - goal_y; - - float dist_sq = dx * dx + dy * dy; - if (dist_sq < pose_tolerance_sq) { return true; } diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 54eb1f57a08..5edeb1d6aaa 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -84,13 +84,15 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( #endif std::lock_guard param_lock(*parameters_handler_->getLock()); + geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal().pose; + nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose); nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); std::unique_lock costmap_lock(*(costmap->getMutex())); geometry_msgs::msg::TwistStamped cmd = - optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal_checker); + optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker); #ifdef BENCHMARK_TESTING auto end = std::chrono::system_clock::now(); diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index e75c68f6c29..6578d6f7335 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -119,7 +119,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, data.path)) { + if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.goal)) { 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 99ad3950de1..c8636e8b78c 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -36,7 +36,7 @@ void GoalAngleCritic::initialize() void GoalAngleCritic::score(CriticData & data) { if (!enabled_ || !utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, data.path)) + threshold_to_consider_, data.state.pose.pose, data.goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 4dbaf063f35..6d98566b990 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -36,15 +36,13 @@ void GoalCritic::initialize() void GoalCritic::score(CriticData & data) { if (!enabled_ || !utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, data.path)) + threshold_to_consider_, data.state.pose.pose, data.goal)) { return; } - 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 auto & goal_x = data.goal.position.x; + const auto & goal_y = data.goal.position.y; const auto traj_x = xt::view(data.trajectories.x, xt::all(), xt::all()); const auto traj_y = xt::view(data.trajectories.y, xt::all(), xt::all()); diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index e3e49918d72..6634a8be324 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -116,7 +116,7 @@ void ObstaclesCritic::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, data.path)) { + if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.goal)) { 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 bd0245f824b..4ce9f46ad76 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -43,8 +43,8 @@ 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)) + if (!enabled_ || utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, data.goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index e1e049a5257..4b4d6c16fba 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -64,7 +64,7 @@ void PathAngleCritic::initialize() void PathAngleCritic::score(CriticData & data) { if (!enabled_ || - utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index eacbb56efd5..5ca3df8ed1e 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -35,7 +35,7 @@ void PathFollowCritic::initialize() void PathFollowCritic::score(CriticData & data) { if (!enabled_ || data.path.x.shape(0) < 2 || - utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 885d1fadc95..f1a3b03205b 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -33,8 +33,8 @@ void PreferForwardCritic::initialize() void PreferForwardCritic::score(CriticData & data) { using xt::evaluation_strategy::immediate; - 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.goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index f4ae970dc63..9bffcf49a57 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -32,7 +32,7 @@ void TwirlingCritic::score(CriticData & data) { using xt::evaluation_strategy::immediate; if (!enabled_ || - utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.path)) + utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.goal)) { return; } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index d5c111217af..6a98cc9e941 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -152,9 +152,11 @@ bool Optimizer::isHolonomic() const geometry_msgs::msg::TwistStamped Optimizer::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) + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal, + nav2_core::GoalChecker * goal_checker) { - prepare(robot_pose, robot_speed, plan, goal_checker); + prepare(robot_pose, robot_speed, plan, goal, goal_checker); do { optimize(); @@ -201,12 +203,15 @@ bool Optimizer::fallback(bool fail) void Optimizer::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) + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal, + nav2_core::GoalChecker * goal_checker) { state_.pose = robot_pose; state_.speed = robot_speed; path_ = utils::toTensor(plan); costs_.fill(0.0f); + goal_ = goal; critics_data_.fail_flag = false; critics_data_.goal_checker = goal_checker; diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 254062eb297..1b961f9ed14 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -186,6 +186,20 @@ void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) plan.poses.erase(plan.poses.begin(), end); } +geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal() +{ + auto goal = global_plan_.poses.back(); + goal.header = global_plan_.header; + if (goal.header.frame_id.empty()) { + throw nav2_core::ControllerTFError("Goal pose has an empty frame_id"); + } + geometry_msgs::msg::PoseStamped transformed_goal; + if (!transformPose(costmap_->getGlobalFrameID(), goal, transformed_goal)) { + throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame"); + } + return transformed_goal; +} + bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose) { // Keep full path if we are within tolerance of the inversion pose diff --git a/nav2_mppi_controller/test/controller_state_transition_test.cpp b/nav2_mppi_controller/test/controller_state_transition_test.cpp index dc8f49a2a3a..46b2e4ff96b 100644 --- a/nav2_mppi_controller/test/controller_state_transition_test.cpp +++ b/nav2_mppi_controller/test/controller_state_transition_test.cpp @@ -60,6 +60,8 @@ TEST(ControllerStateTransitionTest, ControllerNotFail) auto pose = getDummyPointStamped(node, start_pose); auto velocity = getDummyTwist(); auto path = getIncrementalDummyPath(node, path_settings); + path.header.frame_id = costmap_ros->getGlobalFrameID(); + pose.header.frame_id = costmap_ros->getGlobalFrameID(); controller->setPlan(path); diff --git a/nav2_mppi_controller/test/critic_manager_test.cpp b/nav2_mppi_controller/test/critic_manager_test.cpp index e2cf5d80034..52830e97710 100644 --- a/nav2_mppi_controller/test/critic_manager_test.cpp +++ b/nav2_mppi_controller/test/critic_manager_test.cpp @@ -117,10 +117,11 @@ TEST(CriticManagerTests, BasicCriticOperations) models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; + geometry_msgs::msg::Pose goal; xt::xtensor costs; float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.fail_flag = true; diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index e4a28f3df1c..34e64f53899 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -70,11 +70,12 @@ TEST(CriticTests, ConstraintsCritic) models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; + geometry_msgs::msg::Pose goal; 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}; + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); // Initialization testing @@ -143,11 +144,12 @@ TEST(CriticTests, GoalAngleCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + geometry_msgs::msg::Pose goal; 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}; + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); // Initialization testing @@ -165,6 +167,7 @@ TEST(CriticTests, GoalAngleCritic) path.x(9) = 10.0; path.y(9) = 0.0; path.yaws(9) = 3.14; + goal.position.x = 10.0; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); @@ -195,11 +198,12 @@ TEST(CriticTests, GoalCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + geometry_msgs::msg::Pose goal; 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}; + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); // Initialization testing @@ -216,6 +220,7 @@ TEST(CriticTests, GoalCritic) path.reset(10); path.x(9) = 10.0; path.y(9) = 0.0; + goal.position.x = 10.0; critic.score(data); 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 @@ -224,6 +229,7 @@ TEST(CriticTests, GoalCritic) // provide state pose and path close path.x(9) = 0.5; path.y(9) = 0.0; + goal.position.x = 0.5; 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 @@ -245,11 +251,12 @@ TEST(CriticTests, PathAngleCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + geometry_msgs::msg::Pose goal; 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}; + {state, generated_trajectories, path, goal, 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 @@ -267,11 +274,13 @@ TEST(CriticTests, PathAngleCritic) state.pose.pose.position.y = 0.0; path.reset(10); path.x(9) = 0.15; + goal.position.x = 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; + goal.position.x = 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; @@ -360,11 +369,12 @@ TEST(CriticTests, PreferForwardCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + geometry_msgs::msg::Pose goal; 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}; + {state, generated_trajectories, path, goal, 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 @@ -381,11 +391,13 @@ TEST(CriticTests, PreferForwardCritic) state.pose.pose.position.x = 1.0; path.reset(10); path.x(9) = 10.0; + goal.position.x = 10.0; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 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.vx = xt::ones({1000, 30}); critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0f, 1e-6f); @@ -413,11 +425,12 @@ TEST(CriticTests, TwirlingCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + geometry_msgs::msg::Pose goal; 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}; + {state, generated_trajectories, path, goal, 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; @@ -435,11 +448,13 @@ TEST(CriticTests, TwirlingCritic) state.pose.pose.position.x = 1.0; path.reset(10); path.x(9) = 10.0; + goal.position.x = 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; + goal.position.x = 0.15; state.wz = xt::zeros({1000, 30}); critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); @@ -473,11 +488,12 @@ TEST(CriticTests, PathFollowCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + geometry_msgs::msg::Pose goal; 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}; + {state, generated_trajectories, path, goal, 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; @@ -491,16 +507,18 @@ TEST(CriticTests, PathFollowCritic) // Scoring testing - // provide state poses and path close within positional tolerances + // provide state poses and goal close within positional tolerances state.pose.pose.position.x = 2.0; path.reset(6); - path.x(5) = 1.7; + path.x(5) = 1.8; + goal.position.x = 1.8; 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(5) = 0.15; + goal.position.x = 0.15; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 750.0, 1e-2); // 0.15 * 5 weight * 1000 } @@ -521,11 +539,12 @@ TEST(CriticTests, PathAlignCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + geometry_msgs::msg::Pose goal; 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}; + {state, generated_trajectories, path, goal, 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; @@ -543,12 +562,14 @@ TEST(CriticTests, PathAlignCritic) state.pose.pose.position.x = 1.0; path.reset(10); path.x(9) = 0.85; + goal.position.x = 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; + goal.position.x = 0.15; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); @@ -556,6 +577,7 @@ TEST(CriticTests, PathAlignCritic) // but with empty trajectories and paths, should still be zero *data.furthest_reached_path_point = 21; path.x(9) = 0.15; + goal.position.x = 0.15; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); @@ -586,6 +608,7 @@ TEST(CriticTests, PathAlignCritic) path.x(19) = 0.9; path.x(20) = 0.9; path.x(21) = 0.9; + goal.position.x = 0.9; generated_trajectories.x = 0.66 * xt::ones({1000, 30}); critic.score(data); // 0.66 * 1000 * 10 weight * 6 num pts eval / 6 normalization term @@ -605,6 +628,7 @@ TEST(CriticTests, PathAlignCritic) costs = xt::zeros({1000}); path.x = 1.5 * xt::ones({22}); path.y = 1.5 * xt::ones({22}); + goal.position.x = 1.5; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); } @@ -626,11 +650,12 @@ TEST(CriticTests, VelocityDeadbandCritic) models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; + geometry_msgs::msg::Pose goal; 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}; + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); // Initialization testing diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index 9a77f6d8b06..b2210ee2aaf 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -85,9 +85,10 @@ TEST_P(OptimizerSuite, OptimizerTest) { auto pose = getDummyPointStamped(node, start_pose); auto velocity = getDummyTwist(); auto path = getIncrementalDummyPath(node, path_settings); + auto goal = path.poses.back().pose; nav2_core::GoalChecker * dummy_goal_checker{nullptr}; - EXPECT_NO_THROW(optimizer->evalControl(pose, velocity, path, dummy_goal_checker)); + EXPECT_NO_THROW(optimizer->evalControl(pose, velocity, path, goal, dummy_goal_checker)); } INSTANTIATE_TEST_SUITE_P( diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index db28b904dd0..63994b085a4 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -123,9 +123,11 @@ class OptimizerTester : public Optimizer 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) + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal, + nav2_core::GoalChecker * goal_checker) { - prepare(robot_pose, robot_speed, plan, goal_checker); + prepare(robot_pose, robot_speed, plan, goal, goal_checker); EXPECT_EQ(critics_data_.goal_checker, nullptr); EXPECT_NEAR(xt::sum(costs_, immediate)(), 0, 1e-6); // should be reset @@ -381,9 +383,10 @@ TEST(OptimizerTests, PrepareTests) geometry_msgs::msg::Twist speed; speed.linear.y = 4.0; nav_msgs::msg::Path path; + geometry_msgs::msg::Pose goal; path.poses.resize(17); - optimizer_tester.testPrepare(pose, speed, path, nullptr); + optimizer_tester.testPrepare(pose, speed, path, goal, nullptr); } TEST(OptimizerTests, shiftControlSequenceTests) diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 2adf1c59463..bcb3d97be45 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -136,37 +136,46 @@ TEST(UtilsTests, WithTolTests) 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)); + 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; + xt::xtensor 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 - 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)); + 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, path_t)); + EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, goal)); } TEST(UtilsTests, AnglesTests) @@ -225,11 +234,12 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) models::State state; models::Trajectories generated_trajectories; models::Path path; + geometry_msgs::msg::Pose goal; xt::xtensor costs; float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references // Attempt to set furthest point if notionally set, should not change @@ -239,7 +249,7 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) // 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, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references setPathFurthestPointIfNotSet(data2); EXPECT_EQ(data2.furthest_reached_path_point, 0); @@ -258,7 +268,7 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) path = toTensor(plan); CriticData data3 = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references EXPECT_EQ(findPathFurthestReachedPoint(data3), 5u); } @@ -268,11 +278,12 @@ TEST(UtilsTests, findPathCosts) models::State state; models::Trajectories generated_trajectories; models::Path path; + geometry_msgs::msg::Pose goal; xt::xtensor costs; float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references // Test not set if already set, should not change @@ -285,7 +296,7 @@ TEST(UtilsTests, findPathCosts) EXPECT_EQ(data.path_pts_valid->size(), 10u); CriticData data3 = - {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references auto costmap_ros = std::make_shared(