//mark the point of the costmap as local goal where global_plan first leaves the area (or its last point)
void MapGrid::setLocalGoal (const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoseStamped>& global_plan)
{
sizeCheck (costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
int local_goal_x = -1;
int local_goal_y = -1;
bool started_path = false;
std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
adjustPlanResolution (global_plan, adjusted_global_plan, costmap.getResolution());
// skip global path points until we reach the border of the local map
for (unsigned int i = 0; i < adjusted_global_plan.size(); ++i)
{
double g_x = adjusted_global_plan[i].pose.position.x;
double g_y = adjusted_global_plan[i].pose.position.y;
unsigned int map_x, map_y;
if (costmap.worldToMap (g_x, g_y, map_x, map_y) && costmap.getCost (map_x, map_y) != costmap_2d::NO_INFORMATION)
{
local_goal_x = map_x;
local_goal_y = map_y;
started_path = true;
}
else
{
if (started_path)
{
break;
}// else we might have a non pruned path, so we just continue
}
}
if (!started_path)
{
ROS_ERROR ("None of the points of the global plan were in the local costmap, global plan points too far from robot");
return;
}
queue<MapCell*> path_dist_queue;
if (local_goal_x >= 0 && local_goal_y >= 0)
{
MapCell& current = getCell (local_goal_x, local_goal_y);
costmap.mapToWorld (local_goal_x, local_goal_y, goal_x_, goal_y_);
current.target_dist = 0.0;
current.target_mark = true;
path_dist_queue.push (¤t);
}
computeTargetDistance (path_dist_queue, costmap);
}
//update what map cells are considered path based on the global_plan
void MapGrid::setTargetCells (const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoseStamped>& global_plan)
{
sizeCheck (costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
bool started_path = false;
queue<MapCell*> path_dist_queue;
std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
adjustPlanResolution (global_plan, adjusted_global_plan, costmap.getResolution());
if (adjusted_global_plan.size() != global_plan.size())
{
ROS_DEBUG ("Adjusted global plan resolution, added %zu points", adjusted_global_plan.size() - global_plan.size());
}
unsigned int i;
// put global path points into local map until we reach the border of the local map
for (i = 0; i < adjusted_global_plan.size(); ++i)
{
double g_x = adjusted_global_plan[i].pose.position.x;
double g_y = adjusted_global_plan[i].pose.position.y;
unsigned int map_x, map_y;
if (costmap.worldToMap (g_x, g_y, map_x, map_y) && costmap.getCost (map_x, map_y) != costmap_2d::NO_INFORMATION)
{
MapCell& current = getCell (map_x, map_y);
current.target_dist = 0.0;
current.target_mark = true;
path_dist_queue.push (¤t);
started_path = true;
}
else if (started_path)
{
break;
}
}
if (!started_path)
{
ROS_ERROR ("None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
i, adjusted_global_plan.size(), global_plan.size());
return;
}
computeTargetDistance (path_dist_queue, costmap);
}
bool transformGlobalPlan(
const tf::TransformListener& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const tf::Stamped<tf::Pose>& global_pose,
const costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
std::vector<geometry_msgs::PoseStamped>& transformed_plan){
const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
transformed_plan.clear();
try {
if (!global_plan.size() > 0) {
ROS_ERROR("Received plan with zero length");
return false;
}
// get plan_to_global_transform from plan frame to global_frame
tf::StampedTransform plan_to_global_transform;
tf.waitForTransform(global_frame, ros::Time::now(),
plan_pose.header.frame_id, plan_pose.header.stamp,
plan_pose.header.frame_id, ros::Duration(0.5));
tf.lookupTransform(global_frame, ros::Time(),
plan_pose.header.frame_id, plan_pose.header.stamp,
plan_pose.header.frame_id, plan_to_global_transform);
//let's get the pose of the robot in the frame of the plan
tf::Stamped<tf::Pose> robot_pose;
tf.transformPose(plan_pose.header.frame_id, global_pose, robot_pose);
//we'll discard points on the plan that are outside the local costmap
double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
unsigned int i = 0;
double sq_dist_threshold = dist_threshold * dist_threshold;
double sq_dist = 0;
//we need to loop to a point on the plan that is within a certain distance of the robot
while(i < (unsigned int)global_plan.size()) {
double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
if (sq_dist <= sq_dist_threshold) {
break;
}
++i;
}
tf::Stamped<tf::Pose> tf_pose;
geometry_msgs::PoseStamped newer_pose;
//now we'll transform until points are outside of our distance threshold
while(i < (unsigned int)global_plan.size() && sq_dist <= sq_dist_threshold) {
const geometry_msgs::PoseStamped& pose = global_plan[i];
poseStampedMsgToTF(pose, tf_pose);
tf_pose.setData(plan_to_global_transform * tf_pose);
tf_pose.stamp_ = plan_to_global_transform.stamp_;
tf_pose.frame_id_ = global_frame;
poseStampedTFToMsg(tf_pose, newer_pose);
transformed_plan.push_back(newer_pose);
double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
++i;
}
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
