bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
boost::mutex::scoped_lock lock(mutex_);
if (!initialized_) {
ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
return false;
}
//clear the plan, just in case
plan.clear();
ros::NodeHandle n;
std::string global_frame = frame_id_;
//until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
if (tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
ROS_ERROR(
"The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
return false;
}
if (tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
ROS_ERROR(
"The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, start.header.frame_id).c_str());
return false;
}
double wx = start.pose.position.x;
double wy = start.pose.position.y;
unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
double start_x, start_y, goal_x, goal_y;
if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
ROS_WARN(
"The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
return false;
}
if(old_navfn_behavior_){
start_x = start_x_i;
start_y = start_y_i;
}else{
worldToMap(wx, wy, start_x, start_y);
}
wx = goal.pose.position.x;
wy = goal.pose.position.y;
if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
ROS_WARN(
"The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal.");
return false;
}
if(old_navfn_behavior_){
goal_x = goal_x_i;
goal_y = goal_y_i;
}else{
worldToMap(wx, wy, goal_x, goal_y);
}
//clear the starting cell within the costmap because we know it can't be an obstacle
tf::Stamped<tf::Pose> start_pose;
tf::poseStampedMsgToTF(start, start_pose);
clearRobotCell(start_pose, start_x_i, start_y_i);
int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
//make sure to resize the underlying array that Navfn uses
p_calc_->setSize(nx, ny);
planner_->setSize(nx, ny);
path_maker_->setSize(nx, ny);
potential_array_ = new float[nx * ny];
outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);
timespec time1, time2;
/* take current time here */
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);
bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
nx * ny * 2, potential_array_);
if(!old_navfn_behavior_)
planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
if(publish_potential_)
publishPotential(potential_array_);
if (found_legal) {
//extract the plan
if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {
//make sure the goal we push on has the same timestamp as the rest of the plan
geometry_msgs::PoseStamped goal_copy = goal;
goal_copy.header.stamp = ros::Time::now();
plan.push_back(goal_copy);
} else {
ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
}
}else{
ROS_ERROR("Failed to get a plan.");
}
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
std::cout<<"time to generate best global path time = " << (diff(time1,time2).tv_sec)*1e3 + (diff(time1,time2).tv_nsec)*1e-6 << " microseconds" <<"\n";
//data<<"["<< (diff(time1,time2).tv_sec)*1e3 + (diff(time1,time2).tv_nsec)*1e-6 ;
// add orientations if needed
orientation_filter_->processPath(start, plan);
float path_length = 0.0;
std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
geometry_msgs::PoseStamped last_pose;
last_pose = *it;
it++;
for (; it!=plan.end(); ++it) {
std::cout<<"路径位姿点：("<<(*it).pose.position.x<<","<<(*it).pose.position.y<<")"<<"\n";
path_length += hypot( (*it).pose.position.x - last_pose.pose.position.x,
(*it).pose.position.y - last_pose.pose.position.y );
last_pose = *it;
}
std::cout <<"------The global path length: "<< path_length<< " meters------"<<"\n";
//data <<"["<< path_length<< ",";
//data<< (diff(time1,time2).tv_sec)*1e3 + (diff(time1,time2).tv_nsec)*1e-6<<"]"<<"\n" ;
std::cout <<"["<< path_length<< ",";
std::cout<< (diff(time1,time2).tv_sec)*1e3 + (diff(time1,time2).tv_nsec)*1e-6<<"]"<<"\n" ;
//publish the plan for visualization purposes
publishPlan(plan);
delete potential_array_;
return !plan.empty();
}
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
boost::mutex::scoped_lock lock(mutex_);
if (!initialized_) {
ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
return false;
}
//clear the plan, just in case
plan.clear();
ros::NodeHandle n;
std::string global_frame = frame_id_;
//until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
if (tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
ROS_ERROR(
"The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
return false;
}
if (tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
ROS_ERROR(
"The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, start.header.frame_id).c_str());
return false;
}
double wx = start.pose.position.x;
double wy = start.pose.position.y;
unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
double start_x, start_y, goal_x, goal_y;
if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
ROS_WARN(
"The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
return false;
}
if(old_navfn_behavior_){
start_x = start_x_i;
start_y = start_y_i;
}else{
worldToMap(wx, wy, start_x, start_y);
}
wx = goal.pose.position.x;
wy = goal.pose.position.y;
if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
ROS_WARN_THROTTLE(1.0,
"The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
return false;
}
if(old_navfn_behavior_){
goal_x = goal_x_i;
goal_y = goal_y_i;
}else{
worldToMap(wx, wy, goal_x, goal_y);
}
//clear the starting cell within the costmap because we know it can't be an obstacle
tf::Stamped<tf::Pose> start_pose;
tf::poseStampedMsgToTF(start, start_pose);
clearRobotCell(start_pose, start_x_i, start_y_i);
int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
//make sure to resize the underlying array that Navfn uses
p_calc_->setSize(nx, ny);
planner_->setSize(nx, ny);
path_maker_->setSize(nx, ny);
potential_array_ = new float[nx * ny];
outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);
bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
nx * ny * 2, potential_array_);
if(!old_navfn_behavior_)
planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
if(publish_potential_)
publishPotential(potential_array_);
if (found_legal) {
//extract the plan
if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {
//make sure the goal we push on has the same timestamp as the rest of the plan
geometry_msgs::PoseStamped goal_copy = goal;
goal_copy.header.stamp = ros::Time::now();
plan.push_back(goal_copy);
} else {
ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
}
}else{
ROS_ERROR("Failed to get a plan.");
}
// add orientations if needed
orientation_filter_->processPath(start, plan);
//publish the plan for visualization purposes
publishPlan(plan);
delete potential_array_;
return !plan.empty();
}
bool VoronoiPlanner::computePlan(costmap_2d::Costmap2D* costmap_, DynamicVoronoi * voronoi_,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, double tolerance,
std::vector<geometry_msgs::PoseStamped>& plan) {
//Sure that plan is clear
plan.clear();
ros::NodeHandle n;
double wx = start.pose.position.x;
double wy = start.pose.position.y;
unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
double start_x, start_y, goal_x, goal_y;
if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
ROS_WARN(
"The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
return false;
}
worldToMap(costmap_, wx, wy, start_x, start_y);
wx = goal.pose.position.x;
wy = goal.pose.position.y;
if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
ROS_WARN_THROTTLE(1.0,
"The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
return false;
}
worldToMap(costmap_, wx, wy, goal_x, goal_y);
clearRobotCell(costmap_, start_x_i, start_y_i);
bool ** map;
computeVoronoi(voronoi_, costmap_, map);
ros::Time t_b = ros::Time::now();
ros::Time t = ros::Time::now();
std::vector<std::pair<float, float> > path1;
std::vector<std::pair<float, float> > path2;
std::vector<std::pair<float, float> > path3;
ROS_INFO("start_x %f, start_y %f", start_x, start_y);
bool res1 = false, res2 = false, res3 = false;
// If goal not are in cell of the vornoi diagram, we have that best findPath of goal to voronoi diagram without have a cell occupancie
if (!voronoi_->isVoronoi(goal_x, goal_y)) {
res3 = computePath(&path3, goal_x, goal_y, start_x, start_y, voronoi_, 0,
1);
std::cout << "computePath goal to VD " << res3 << std::endl;
goal_x = std::get < 0 > (path3[path3.size() - 1]);
goal_y = std::get < 1 > (path3[path3.size() - 1]);
ROS_INFO("Is voronoi goal compute %d", voronoi_->isVoronoi(goal_x, goal_y));
std::reverse(path3.begin(), path3.end());
}
if (!voronoi_->isVoronoi(start_x, start_y)) {
res1 = computePath(&path1, start_x, start_y, goal_x, goal_y, voronoi_, 0,
1);
std::cout << "computePath start to VD " << res1 << std::endl;
start_x = std::get < 0 > (path1[path1.size() - 1]);
start_y = std::get < 1 > (path1[path1.size() - 1]);
ROS_INFO("Is voronoi start compute %d", voronoi_->isVoronoi(start_x, start_y));
}
res2 = computePath(&path2, start_x, start_y, goal_x, goal_y, voronoi_, 1, 0);
ROS_INFO("computePath %d", res2);
if (!(res1 && res2 && res3)) {
ROS_INFO("Failed to compute full path");
}
path1.insert(path1.end(), path2.begin(), path2.end());
path1.insert(path1.end(), path3.begin(), path3.end());
/*for (int i = 0; i < path1.size(); i++) {
int x = std::get < 0 > (path1[i]);
int y = std::get < 1 > (path1[i]);
if (x > 0 && y > 0)
map[x][y] = 1;
}*/
smoothPath(&path1);
for (int i = 0; i < path1.size(); i++) {
geometry_msgs::PoseStamped new_goal = goal;
tf::Quaternion goal_quat = tf::createQuaternionFromYaw(1.54);
new_goal.pose.position.x = std::get < 0 > (path1[i]);
new_goal.pose.position.y = std::get < 1 > (path1[i]);
mapToWorld(costmap_, new_goal.pose.position.x, new_goal.pose.position.y,
new_goal.pose.position.x, new_goal.pose.position.y);
new_goal.pose.orientation.x = goal_quat.x();
new_goal.pose.orientation.y = goal_quat.y();
new_goal.pose.orientation.z = goal_quat.z();
new_goal.pose.orientation.w = goal_quat.w();
plan.push_back(new_goal);
}
ROS_INFO("\nTime to get plan: %f sec\n", (ros::Time::now() - t_b).toSec());
return !plan.empty();
}
