Skip to content

Commit ad2f960

Browse files
redvinaaredvinaa
authored
Add goal pose to CriticData (#4812)
* Add goal pose to CriticData Signed-off-by: redvinaa <[email protected]> * Pass goal pose directly to withinPositionGoalTolerance Signed-off-by: redvinaa <[email protected]> * Fix condition not Signed-off-by: redvinaa <[email protected]> * Add goal positions to tests Signed-off-by: redvinaa <[email protected]> * Use plan stamp Signed-off-by: redvinaa <[email protected]> * Use float instead of auto Signed-off-by: redvinaa <[email protected]> * Set pose frame id in test Signed-off-by: redvinaa <[email protected]> * Fix frame id in test vol 2 Signed-off-by: redvinaa <[email protected]> --------- Signed-off-by: redvinaa <[email protected]>
1 parent 507b365 commit ad2f960

23 files changed

Lines changed: 174 additions & 102 deletions

‎nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -32,14 +32,15 @@ namespace mppi
3232

3333
/**
3434
* @struct mppi::CriticData
35-
* @brief Data to pass to critics for scoring, including state, trajectories, path, costs, and
36-
* important parameters to share
35+
* @brief Data to pass to critics for scoring, including state, trajectories,
36+
* pruned path, global goal, costs, and important parameters to share
3737
*/
3838
struct CriticData
3939
{
4040
const models::State & state;
4141
const models::Trajectories & trajectories;
4242
const models::Path & path;
43+
const geometry_msgs::msg::Pose & goal;
4344

4445
xt::xtensor<float, 1> & costs;
4546
float & model_dt;

‎nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp‎

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -90,7 +90,7 @@ class Optimizer
9090
geometry_msgs::msg::TwistStamped evalControl(
9191
const geometry_msgs::msg::PoseStamped & robot_pose,
9292
const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan,
93-
nav2_core::GoalChecker * goal_checker);
93+
const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker);
9494

9595
/**
9696
* @brief Get the trajectories generated in a cycle for visualization
@@ -132,7 +132,8 @@ class Optimizer
132132
void prepare(
133133
const geometry_msgs::msg::PoseStamped & robot_pose,
134134
const geometry_msgs::msg::Twist & robot_speed,
135-
const nav_msgs::msg::Path & plan, nav2_core::GoalChecker * goal_checker);
135+
const nav_msgs::msg::Path & plan,
136+
const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker);
136137

137138
/**
138139
* @brief Obtain the main controller's parameters
@@ -250,10 +251,12 @@ class Optimizer
250251
std::array<mppi::models::Control, 4> control_history_;
251252
models::Trajectories generated_trajectories_;
252253
models::Path path_;
254+
geometry_msgs::msg::Pose goal_;
253255
xt::xtensor<float, 1> costs_;
254256

255-
CriticData critics_data_ =
256-
{state_, generated_trajectories_, path_, costs_, settings_.model_dt, false, nullptr, nullptr,
257+
CriticData critics_data_ = {
258+
state_, generated_trajectories_, path_, goal_,
259+
costs_, settings_.model_dt, false, nullptr, nullptr,
257260
std::nullopt, std::nullopt}; /// Caution, keep references
258261

259262
rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};

‎nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -89,6 +89,12 @@ class PathHandler
8989
*/
9090
nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose);
9191

92+
/**
93+
* @brief Get the global goal pose transformed to the local frame
94+
* @return Transformed goal pose
95+
*/
96+
geometry_msgs::msg::PoseStamped getTransformedGoal();
97+
9298
protected:
9399
/**
94100
* @brief Transform a pose to another frame

‎nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp‎

Lines changed: 10 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -195,27 +195,23 @@ inline models::Path toTensor(const nav_msgs::msg::Path & path)
195195
* @brief Check if the robot pose is within the Goal Checker's tolerances to goal
196196
* @param global_checker Pointer to the goal checker
197197
* @param robot Pose of robot
198-
* @param path Path to retreive goal pose from
198+
* @param goal Goal pose
199199
* @return bool If robot is within goal checker tolerances to the goal
200200
*/
201201
inline bool withinPositionGoalTolerance(
202202
nav2_core::GoalChecker * goal_checker,
203203
const geometry_msgs::msg::Pose & robot,
204-
const models::Path & path)
204+
const geometry_msgs::msg::Pose & goal)
205205
{
206-
const auto goal_idx = path.x.shape(0) - 1;
207-
const auto goal_x = path.x(goal_idx);
208-
const auto goal_y = path.y(goal_idx);
209-
210206
if (goal_checker) {
211207
geometry_msgs::msg::Pose pose_tolerance;
212208
geometry_msgs::msg::Twist velocity_tolerance;
213209
goal_checker->getTolerances(pose_tolerance, velocity_tolerance);
214210

215211
const auto pose_tolerance_sq = pose_tolerance.position.x * pose_tolerance.position.x;
216212

217-
auto dx = robot.position.x - goal_x;
218-
auto dy = robot.position.y - goal_y;
213+
auto dx = robot.position.x - goal.position.x;
214+
auto dy = robot.position.y - goal.position.y;
219215

220216
auto dist_sq = dx * dx + dy * dy;
221217

@@ -231,24 +227,19 @@ inline bool withinPositionGoalTolerance(
231227
* @brief Check if the robot pose is within tolerance to the goal
232228
* @param pose_tolerance Pose tolerance to use
233229
* @param robot Pose of robot
234-
* @param path Path to retreive goal pose from
230+
* @param goal Goal pose
235231
* @return bool If robot is within tolerance to the goal
236232
*/
237233
inline bool withinPositionGoalTolerance(
238234
float pose_tolerance,
239235
const geometry_msgs::msg::Pose & robot,
240-
const models::Path & path)
236+
const geometry_msgs::msg::Pose & goal)
241237
{
242-
const auto goal_idx = path.x.shape(0) - 1;
243-
const auto goal_x = path.x(goal_idx);
244-
const auto goal_y = path.y(goal_idx);
245-
246-
const auto pose_tolerance_sq = pose_tolerance * pose_tolerance;
247-
248-
auto dx = robot.position.x - goal_x;
249-
auto dy = robot.position.y - goal_y;
238+
const double & dist_sq =
239+
std::pow(goal.position.x - robot.position.x, 2) +
240+
std::pow(goal.position.y - robot.position.y, 2);
250241

251-
auto dist_sq = dx * dx + dy * dy;
242+
const float pose_tolerance_sq = pose_tolerance * pose_tolerance;
252243

253244
if (dist_sq < pose_tolerance_sq) {
254245
return true;

‎nav2_mppi_controller/src/controller.cpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -92,13 +92,15 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
9292
last_time_called_ = clock_->now();
9393

9494
std::lock_guard<std::mutex> param_lock(*parameters_handler_->getLock());
95+
geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal().pose;
96+
9597
nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose);
9698

9799
nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
98100
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> costmap_lock(*(costmap->getMutex()));
99101

100102
geometry_msgs::msg::TwistStamped cmd =
101-
optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal_checker);
103+
optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker);
102104

103105
#ifdef BENCHMARK_TESTING
104106
auto end = std::chrono::system_clock::now();

‎nav2_mppi_controller/src/critics/cost_critic.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -119,7 +119,7 @@ void CostCritic::score(CriticData & data)
119119

120120
// If near the goal, don't apply the preferential term since the goal is near obstacles
121121
bool near_goal = false;
122-
if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) {
122+
if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.goal)) {
123123
near_goal = true;
124124
}
125125

‎nav2_mppi_controller/src/critics/goal_angle_critic.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@ void GoalAngleCritic::initialize()
3636
void GoalAngleCritic::score(CriticData & data)
3737
{
3838
if (!enabled_ || !utils::withinPositionGoalTolerance(
39-
threshold_to_consider_, data.state.pose.pose, data.path))
39+
threshold_to_consider_, data.state.pose.pose, data.goal))
4040
{
4141
return;
4242
}

‎nav2_mppi_controller/src/critics/goal_critic.cpp‎

Lines changed: 3 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -36,15 +36,13 @@ void GoalCritic::initialize()
3636
void GoalCritic::score(CriticData & data)
3737
{
3838
if (!enabled_ || !utils::withinPositionGoalTolerance(
39-
threshold_to_consider_, data.state.pose.pose, data.path))
39+
threshold_to_consider_, data.state.pose.pose, data.goal))
4040
{
4141
return;
4242
}
4343

44-
const auto goal_idx = data.path.x.shape(0) - 1;
45-
46-
const auto goal_x = data.path.x(goal_idx);
47-
const auto goal_y = data.path.y(goal_idx);
44+
const auto & goal_x = data.goal.position.x;
45+
const auto & goal_y = data.goal.position.y;
4846

4947
const auto traj_x = xt::view(data.trajectories.x, xt::all(), xt::all());
5048
const auto traj_y = xt::view(data.trajectories.y, xt::all(), xt::all());

‎nav2_mppi_controller/src/critics/obstacles_critic.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -125,7 +125,7 @@ void ObstaclesCritic::score(CriticData & data)
125125

126126
// If near the goal, don't apply the preferential term since the goal is near obstacles
127127
bool near_goal = false;
128-
if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) {
128+
if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.goal)) {
129129
near_goal = true;
130130
}
131131

‎nav2_mppi_controller/src/critics/path_align_critic.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -46,8 +46,8 @@ void PathAlignCritic::initialize()
4646
void PathAlignCritic::score(CriticData & data)
4747
{
4848
// Don't apply close to goal, let the goal critics take over
49-
if (!enabled_ ||
50-
utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path))
49+
if (!enabled_ || utils::withinPositionGoalTolerance(
50+
threshold_to_consider_, data.state.pose.pose, data.goal))
5151
{
5252
return;
5353
}

0 commit comments

Comments
 (0)