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 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(); }