HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

93
mir_robot/mir_dwb_critics/src/path_angle.cpp Executable file
View File

@@ -0,0 +1,93 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, DFKI GmbH
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <mir_dwb_critics/path_angle.h>
#include <pluginlib/class_list_macros.h>
#include <nav_2d_utils/path_ops.h>
#include <nav_grid/coordinate_conversion.h>
#include <vector>
namespace mir_dwb_critics {
bool PathAngleCritic::prepare(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel,
const geometry_msgs::Pose2D &goal, const nav_2d_msgs::Path2D &global_plan) {
const nav_core2::Costmap &costmap = *costmap_;
const nav_grid::NavGridInfo &info = costmap.getInfo();
nav_2d_msgs::Path2D adjusted_global_plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution);
if (global_plan.poses.empty()) {
ROS_ERROR_NAMED("PathAngleCritic", "The global plan was empty.");
return false;
}
// find the angle of the plan at the pose on the plan closest to the robot
double distance_min = std::numeric_limits<double>::infinity();
bool started_path = false;
for (unsigned int i = 0; i < adjusted_global_plan.poses.size(); i++) {
double g_x = adjusted_global_plan.poses[i].x;
double g_y = adjusted_global_plan.poses[i].y;
unsigned int map_x, map_y;
if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != costmap.NO_INFORMATION) {
double distance = nav_2d_utils::poseDistance(adjusted_global_plan.poses[i], pose);
if (distance_min > distance) {
// still getting closer
desired_angle_ = adjusted_global_plan.poses[i].theta;
distance_min = distance;
started_path = true;
} else {
// plan is going away from the robot again
break;
}
} else if (started_path) {
// Off the costmap after being on the costmap.
break;
}
// else, we have not yet found a point on the costmap, so we just continue
}
if (!started_path) {
ROS_ERROR_NAMED("PathAngleCritic", "None of the points of the global plan were in the local costmap.");
return false;
}
return true;
}
double PathAngleCritic::scoreTrajectory(const dwb_msgs::Trajectory2D &traj) {
double diff = fabs(remainder(traj.poses.back().theta - desired_angle_, 2 * M_PI));
return diff * diff;
}
} // namespace mir_dwb_critics
PLUGINLIB_EXPORT_CLASS(mir_dwb_critics::PathAngleCritic, dwb_local_planner::TrajectoryCritic)

101
mir_robot/mir_dwb_critics/src/path_dist_pruned.cpp Executable file
View File

@@ -0,0 +1,101 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, DFKI GmbH
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <nav_2d_utils/path_ops.h>
#include <pluginlib/class_list_macros.h>
#include "mir_dwb_critics/path_dist_pruned.h"
namespace mir_dwb_critics {
bool PathDistPrunedCritic::prepare(
const geometry_msgs::Pose2D &pose,
const nav_2d_msgs::Twist2D &vel,
const geometry_msgs::Pose2D &goal,
const nav_2d_msgs::Path2D &global_plan) {
const nav_core2::Costmap &costmap = *costmap_;
const nav_grid::NavGridInfo &info = costmap.getInfo();
auto plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses;
// --- stolen from PathProgressCritic::getGoalPose ---
// find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map
unsigned int start_index = 0;
double distance_to_start = std::numeric_limits<double>::infinity();
bool started_path = false;
for (unsigned int i = 0; i < plan.size(); i++) {
double g_x = plan[i].x;
double g_y = plan[i].y;
unsigned int map_x, map_y;
if (worldToGridBounded(info, g_x, g_y, map_x, map_y)
&& costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION) {
// Still on the costmap. Continue.
double distance = nav_2d_utils::poseDistance(plan[i], pose);
if (distance_to_start > distance) {
start_index = i;
distance_to_start = distance;
started_path = true;
} else {
// Plan is going away from the robot again. It's possible that it comes back and we would find a pose that's
// even closer to the robot, but then we would skip over parts of the plan.
break;
}
} else if (started_path) {
// Off the costmap after being on the costmap.
break;
}
// else, we have not yet found a point on the costmap, so we just continue
}
if (!started_path) {
ROS_ERROR_NAMED("PathDistPrunedCritic", "None of the points of the global plan were in the local costmap.");
return false;
}
// ---------------------------------------------------
// remove the first part of the path, everything before start_index, to
// disregard that part in the PathDistCritic implementation.
nav_2d_msgs::Path2D global_plan_pruned;
global_plan_pruned.header = global_plan.header;
global_plan_pruned.poses = std::vector<geometry_msgs::Pose2D>(
plan.begin() + start_index,
plan.end());
return dwb_critics::PathDistCritic::prepare(pose, vel, goal, global_plan_pruned);
}
} // namespace mir_dwb_critics
PLUGINLIB_EXPORT_CLASS(mir_dwb_critics::PathDistPrunedCritic, dwb_local_planner::TrajectoryCritic)

302
mir_robot/mir_dwb_critics/src/path_progress.cpp Executable file
View File

@@ -0,0 +1,302 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, DFKI GmbH
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <mir_dwb_critics/path_progress.h>
#include <nav_grid/coordinate_conversion.h>
#include <pluginlib/class_list_macros.h>
#include <nav_2d_utils/path_ops.h>
#include <vector>
namespace mir_dwb_critics
{
bool PathProgressCritic::prepare(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel,
const geometry_msgs::Pose2D &goal,
const nav_2d_msgs::Path2D &global_plan)
{
dwb_critics::MapGridCritic::reset();
unsigned int local_goal_x, local_goal_y;
if (!getGoalPose(pose, global_plan, local_goal_x, local_goal_y, desired_angle_))
{
return false;
}
// Enqueue just the last pose
cell_values_.setValue(local_goal_x, local_goal_y, 0.0);
queue_->enqueueCell(local_goal_x, local_goal_y);
// MapGridCritic::propogateManhattanDistances
propogateManhattanDistances();
return true;
}
void PathProgressCritic::onInit()
{
dwb_critics::MapGridCritic::onInit();
critic_nh_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.20);
critic_nh_.param("angle_threshold", angle_threshold_, M_PI_4);
critic_nh_.param("heading_scale", heading_scale_, 1.0);
reached_intermediate_goals_pub_ = critic_nh_.advertise<geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
current_goal_pub_ = critic_nh_.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
closet_robot_goal_pub_ = critic_nh_.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 1);
// divide heading scale by position scale because the sum will be multiplied by scale again
heading_scale_ /= getScale();
}
void PathProgressCritic::reset()
{
reached_intermediate_goals_.clear();
}
double PathProgressCritic::scoreTrajectory(const dwb_msgs::Trajectory2D &traj)
{
double position_score = MapGridCritic::scoreTrajectory(traj);
double heading_diff = fabs(remainder(traj.poses.back().theta - desired_angle_, 2 * M_PI));
double heading_score = heading_diff * heading_diff;
return position_score + heading_scale_ * heading_score;
}
bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Path2D &global_plan,
unsigned int &x, unsigned int &y, double &desired_angle)
{
const nav_core2::Costmap &costmap = *costmap_;
const nav_grid::NavGridInfo &info = costmap.getInfo();
if (global_plan.poses.empty())
{
ROS_ERROR_NAMED("PathProgressCritic", "The global plan was empty.");
return false;
}
// Chuyển đổi dữ liệu kế hoạch đường đi 'nav_2d_msgs::Path2D' sang 'std::vector<geometry_msgs::Pose2D>'
std::vector<geometry_msgs::Pose2D> plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses;
// find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map
// Tìm điểm pose bắt đầu trên kế hoạch đường đi gần nhất với robot đồng thời có tồn tại trong local map
unsigned int start_index = 0; // số vị trí pose trong kế hoạch đường đi
double distance_to_start = std::numeric_limits<double>::infinity(); // Xét giá trị là vô cùng oo
bool started_path = false;
for (unsigned int i = 0; i < plan.size(); i++)
{
double g_x = plan[i].x;
double g_y = plan[i].y;
unsigned int map_x, map_y;
if (worldToGridBounded(info, g_x, g_y, map_x, map_y) // chuyển tọa độ từ đơn vị mét sang pixel
&& costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION)
{ // chi phí phải khác -1
// Still on the costmap. Continue.
double distance = nav_2d_utils::poseDistance(plan[i], robot_pose); // Tính khoảng cách từ vị trí của robot đến pose đang xét
if (distance_to_start > distance)
{
start_index = i;
distance_to_start = distance;
started_path = true;
}
// else
// {
// // Plan is going away from the robot again. It's possible that it comes back and we would find a pose that's
// // even closer to the robot, but then we would skip over parts of the plan.
// ss <<" index_max_ " << index_max_;
// break;
// }
}
else if (started_path)
{
// Off the costmap after being on the costmap.
break;
}
// else, we have not yet found a point on the costmap, so we just continue
}
geometry_msgs::PoseStamped pose;
pose.header.stamp = ros::Time::now();
pose.header.frame_id = global_plan.header.frame_id;
pose.pose.position.x = plan[start_index].x;
pose.pose.position.y = plan[start_index].y;
tf2::Quaternion temp;
temp.setRPY(0, 0, plan[start_index].theta);
pose.pose.orientation.x = temp.getX();
pose.pose.orientation.y = temp.getY();
pose.pose.orientation.z = temp.getZ();
pose.pose.orientation.w = temp.getW();
closet_robot_goal_pub_.publish(pose);
// Nếu khồng tìm được điểm gần với robot nhất thì trả về false
if (!started_path)
{
ROS_ERROR_NAMED("PathProgressCritic", "None of the points of the global plan were in the local costmap.");
return false;
}
// current_index_ = start_index;
// find the "last valid pose", i.e. the last pose on the plan after the start pose that is still on the local map
// Tìm điểm pose cuôi cùng sau điểm pose bắt đầu và có tồn tại trong local map
unsigned int last_valid_index = start_index;
for (unsigned int i = start_index + 1; i < plan.size(); i++)
{
double g_x = plan[i].x;
double g_y = plan[i].y;
unsigned int map_x, map_y;
if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION)
{
// Still on the costmap. Continue.
last_valid_index = i;
}
else
{
// Off the costmap after being on the costmap.
break;
}
}
// find the "goal pose" by walking along the plan as long as each step leads far enough away from the starting point,
// i.e. is within angle_threshold_ of the starting direction.
// Tìm điểm mục tiêu chạy từ điểm start pose đến last valid pose dọc thep plan
unsigned int goal_index = start_index;
while (goal_index < last_valid_index && critic_nh_.ok())
{
// Tìm kiếm vị trí điểm mục tiêu phù hợp
goal_index = getGoalIndex(robot_pose, plan, start_index, last_valid_index);
// check if goal already reached
bool goal_already_reached = false;
geometry_msgs::PoseArray poseArrayMsg;
poseArrayMsg.header.stamp = ros::Time::now();
poseArrayMsg.header.frame_id = global_plan.header.frame_id;
for (const auto &pose2D : reached_intermediate_goals_)
{
geometry_msgs::Pose pose;
pose.position.x = pose2D.x;
pose.position.y = pose2D.y;
tf2::Quaternion temp;
temp.setRPY(0, 0, pose2D.theta);
pose.orientation.x = temp.getX();
pose.orientation.y = temp.getY();
pose.orientation.z = temp.getZ();
pose.orientation.w = temp.getW();
poseArrayMsg.poses.push_back(pose);
}
reached_intermediate_goals_pub_.publish(poseArrayMsg);
for (auto &reached_intermediate_goal : reached_intermediate_goals_)
{
double distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]);
if (distance < xy_local_goal_tolerance_)
{
goal_already_reached = true;
// choose a new start_index by walking along the plan until we're outside xy_local_goal_tolerance_ and try again
// (start_index is now > goal_index)
for (start_index = goal_index; start_index <= last_valid_index; ++start_index)
{
distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[start_index]);
if (distance >= xy_local_goal_tolerance_)
{
break;
}
}
break;
}
}
pose.header.stamp = ros::Time::now();
pose.header.frame_id = global_plan.header.frame_id;
pose.pose.position.x = plan[goal_index].x;
pose.pose.position.y = plan[goal_index].y;
tf2::Quaternion temp;
temp.setRPY(0, 0, plan[goal_index].theta);
pose.pose.orientation.x = temp.getX();
pose.pose.orientation.y = temp.getY();
pose.pose.orientation.z = temp.getZ();
pose.pose.orientation.w = temp.getW();
current_goal_pub_.publish(pose);
if (!goal_already_reached)
{
// new goal not in reached_intermediate_goals_
double distance = nav_2d_utils::poseDistance(plan[goal_index], robot_pose);
if (distance < xy_local_goal_tolerance_)
{
// the robot has currently reached the goal
reached_intermediate_goals_.push_back(plan[goal_index]);
ROS_DEBUG_NAMED("PathProgressCritic", "Number of reached_intermediate goals: %zu", reached_intermediate_goals_.size());
}
else
{
// we've found a new goal!
break;
}
}
}
if (start_index > goal_index)
start_index = goal_index;
ROS_ASSERT(goal_index <= last_valid_index);
// save goal in x, y
worldToGridBounded(info, plan[goal_index].x, plan[goal_index].y, x, y);
desired_angle = plan[start_index].theta;
return true;
}
unsigned int PathProgressCritic::getGoalIndex(const geometry_msgs::Pose2D &robot_pose,
const std::vector<geometry_msgs::Pose2D> &plan,
unsigned int start_index,
unsigned int last_valid_index) const
{
if (start_index >= last_valid_index)
return last_valid_index;
unsigned int goal_index = start_index;
for (unsigned int i = start_index + 1; i <= last_valid_index; ++i)
{
const double current_direction_x = plan[i].x - plan[i - 1].x;
const double current_direction_y = plan[i].y - plan[i - 1].y;
if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
{
double current_angle = atan2(current_direction_y, current_direction_x);
if (fabs(remainder(robot_pose.theta - current_angle, 2 * M_PI)) > angle_threshold_)
break;
goal_index = i;
}
}
if (nav_2d_utils::poseDistance(plan[goal_index], plan[last_valid_index]) <= xy_local_goal_tolerance_)
goal_index = last_valid_index;
return goal_index;
}
} // namespace mir_dwb_critics
PLUGINLIB_EXPORT_CLASS(mir_dwb_critics::PathProgressCritic, dwb_local_planner::TrajectoryCritic)

221
mir_robot/mir_dwb_critics/src/path_progress_default.cpp Executable file
View File

@@ -0,0 +1,221 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, DFKI GmbH
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <mir_dwb_critics/path_progress_default.h>
#include <nav_grid/coordinate_conversion.h>
#include <pluginlib/class_list_macros.h>
#include <nav_2d_utils/path_ops.h>
#include <vector>
namespace mir_dwb_critics {
bool PathProgressCritic::prepare(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel,
const geometry_msgs::Pose2D &goal,
const nav_2d_msgs::Path2D &global_plan) {
dwb_critics::MapGridCritic::reset();
unsigned int local_goal_x, local_goal_y;
if (!getGoalPose(pose, global_plan, local_goal_x, local_goal_y, desired_angle_)) {
return false;
}
// Enqueue just the last pose
cell_values_.setValue(local_goal_x, local_goal_y, 0.0);
queue_->enqueueCell(local_goal_x, local_goal_y);
propogateManhattanDistances();
return true;
}
void PathProgressCritic::onInit() {
dwb_critics::MapGridCritic::onInit();
critic_nh_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.20);
critic_nh_.param("angle_threshold", angle_threshold_, M_PI_4);
critic_nh_.param("heading_scale", heading_scale_, 1.0);
// divide heading scale by position scale because the sum will be multiplied by scale again
heading_scale_ /= getScale();
}
void PathProgressCritic::reset() {
reached_intermediate_goals_.clear();
}
double PathProgressCritic::scoreTrajectory(const dwb_msgs::Trajectory2D &traj) {
double position_score = MapGridCritic::scoreTrajectory(traj);
double heading_diff = fabs(remainder(traj.poses.back().theta - desired_angle_, 2 * M_PI));
double heading_score = heading_diff * heading_diff;
return position_score + heading_scale_ * heading_score;
}
bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Path2D &global_plan,
unsigned int &x, unsigned int &y, double &desired_angle) {
const nav_core2::Costmap &costmap = *costmap_;
const nav_grid::NavGridInfo &info = costmap.getInfo();
if (global_plan.poses.empty()) {
ROS_ERROR_NAMED("PathProgressCritic", "The global plan was empty.");
return false;
}
std::vector<geometry_msgs::Pose2D> plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses;
// find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map
unsigned int start_index = 0;
double distance_to_start = std::numeric_limits<double>::infinity();
bool started_path = false;
for (unsigned int i = 0; i < plan.size(); i++) {
double g_x = plan[i].x;
double g_y = plan[i].y;
unsigned int map_x, map_y;
if (worldToGridBounded(info, g_x, g_y, map_x, map_y)
&& costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION) {
// Still on the costmap. Continue.
double distance = nav_2d_utils::poseDistance(plan[i], robot_pose);
if (distance_to_start > distance) {
start_index = i;
distance_to_start = distance;
started_path = true;
} else {
// Plan is going away from the robot again. It's possible that it comes back and we would find a pose that's
// even closer to the robot, but then we would skip over parts of the plan.
break;
}
} else if (started_path) {
// Off the costmap after being on the costmap.
break;
}
// else, we have not yet found a point on the costmap, so we just continue
}
if (!started_path) {
ROS_ERROR_NAMED("PathProgressCritic", "None of the points of the global plan were in the local costmap.");
return false;
}
// find the "last valid pose", i.e. the last pose on the plan after the start pose that is still on the local map
unsigned int last_valid_index = start_index;
for (unsigned int i = start_index + 1; i < plan.size(); i++) {
double g_x = plan[i].x;
double g_y = plan[i].y;
unsigned int map_x, map_y;
if (worldToGridBounded(info, g_x, g_y, map_x, map_y)
&& costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION) {
// Still on the costmap. Continue.
last_valid_index = i;
} else {
// Off the costmap after being on the costmap.
break;
}
}
// find the "goal pose" by walking along the plan as long as each step leads far enough away from the starting point,
// i.e. is within angle_threshold_ of the starting direction.
unsigned int goal_index = start_index;
while (goal_index < last_valid_index) {
goal_index = getGoalIndex(plan, start_index, last_valid_index);
// check if goal already reached
bool goal_already_reached = false;
for (auto &reached_intermediate_goal : reached_intermediate_goals_) {
double distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]);
if (distance < xy_local_goal_tolerance_) {
goal_already_reached = true;
// choose a new start_index by walking along the plan until we're outside xy_local_goal_tolerance_ and try again
// (start_index is now > goal_index)
for (start_index = goal_index; start_index <= last_valid_index; ++start_index) {
distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[start_index]);
if (distance >= xy_local_goal_tolerance_) {
break;
}
}
break;
}
}
if (!goal_already_reached) {
// new goal not in reached_intermediate_goals_
double distance = nav_2d_utils::poseDistance(plan[goal_index], robot_pose);
if (distance < xy_local_goal_tolerance_) {
// the robot has currently reached the goal
reached_intermediate_goals_.push_back(plan[goal_index]);
ROS_DEBUG_NAMED("PathProgressCritic", "Number of reached_intermediate goals: %zu", reached_intermediate_goals_.size());
} else {
// we've found a new goal!
break;
}
}
}
if (start_index > goal_index)
start_index = goal_index;
ROS_ASSERT(goal_index <= last_valid_index);
// save goal in x, y
worldToGridBounded(info, plan[goal_index].x, plan[goal_index].y, x, y);
desired_angle = plan[start_index].theta;
return true;
}
unsigned int PathProgressCritic::getGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan,
unsigned int start_index,
unsigned int last_valid_index) const {
if (start_index >= last_valid_index)
return last_valid_index;
unsigned int goal_index = start_index;
const double start_direction_x = plan[start_index + 1].x - plan[start_index].x;
const double start_direction_y = plan[start_index + 1].y - plan[start_index].y;
if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) { // make sure that atan2 is defined
double start_angle = atan2(start_direction_y, start_direction_x);
for (unsigned int i = start_index + 1; i <= last_valid_index; ++i) {
const double current_direction_x = plan[i].x - plan[i - 1].x;
const double current_direction_y = plan[i].y - plan[i - 1].y;
if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) {
double current_angle = atan2(current_direction_y, current_direction_x);
// goal pose is found if plan doesn't point far enough away from the starting point
if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) > angle_threshold_)
break;
goal_index = i;
}
}
}
return goal_index;
}
} // namespace mir_dwb_critics
PLUGINLIB_EXPORT_CLASS(mir_dwb_critics::PathProgressCritic, dwb_local_planner::TrajectoryCritic)