303 lines
12 KiB
C++
Executable File
303 lines
12 KiB
C++
Executable File
/*
|
|
* 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)
|