52 : costmap_ros_(NULL), planner_(), initialized_(false), allow_unknown_(true) {}
93 ROS_WARN(
"This planner has already been initialized, you can't call it twice, doing nothing");
105 ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
110 geometry_msgs::Point p;
113 p.y = world_point.y - tolerance;
115 while(p.y <= world_point.y + tolerance)
117 p.x = world_point.x - tolerance;
118 while(p.x <= world_point.x + tolerance)
137 ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
145 unsigned int index = my *
planner_->nx + mx;
153 ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
164 if(!costmap->
worldToMap(world_point.x, world_point.y, mx, my))
178 return planner_->calcNavFnDijkstra();
185 ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
195 makePlan(req.start, req.goal, resp.plan.poses);
211 const geometry_msgs::PoseStamped&
goal, std::vector<geometry_msgs::PoseStamped>& plan)
217 const geometry_msgs::PoseStamped&
goal,
double tolerance, std::vector<geometry_msgs::PoseStamped>& plan)
219 boost::mutex::scoped_lock lock(
mutex_);
222 ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
236 ROS_ERROR(
"The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.",
243 ROS_ERROR(
"The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.",
248 double wx = start.pose.position.x;
249 double wy = start.pose.position.y;
254 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?");
267 snprintf( filename, 1000,
"navfnros-makeplan-costmapB-%04d.pgm", n++ );
268 costmap->saveRawMap( std::string( filename ));
280 snprintf( filename, 1000,
"navfnros-makeplan-costmapC-%04d", n++ );
289 wx = goal.pose.position.x;
290 wy = goal.pose.position.y;
296 ROS_WARN_THROTTLE(1.0,
"The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal.");
314 geometry_msgs::PoseStamped p, best_pose;
317 bool found_legal =
false;
318 double best_sdist = DBL_MAX;
320 p.pose.position.y = goal.pose.position.y - tolerance;
322 while(p.pose.position.y <= goal.pose.position.y + tolerance)
324 p.pose.position.x = goal.pose.position.x - tolerance;
325 while(p.pose.position.x <= goal.pose.position.x + tolerance)
329 if(potential <
POT_HIGH && sdist < best_sdist)
335 p.pose.position.x += resolution;
337 p.pose.position.y += resolution;
346 geometry_msgs::PoseStamped goal_copy = best_pose;
348 plan.push_back(goal_copy);
352 ROS_ERROR(
"Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
359 pcl::PointCloud<PotarrPoint> pot_area;
360 pot_area.header.frame_id = global_frame;
361 pot_area.points.clear();
362 std_msgs::Header header;
379 pot_area.push_back(pt);
389 ROS_DEBUG(
"Navfn: Gloal position: x %f, y %f",plan.back().pose.position.x,plan.back().pose.position.y );
390 costmap->
worldToMap(plan.back().pose.position.x, plan.back().pose.position.y, mx2, my2);
392 if(costmap->
getCost(mx2,my2) > 180)
395 while(costmap->
getCost(mx2,my2) > 180){
397 costmap->
worldToMap(plan.back().pose.position.x, plan.back().pose.position.y, mx2, my2);
399 plan.back().pose.orientation = goal.pose.orientation;
404 return !plan.empty();
407 void NavfnROS::publishPlan(
const std::vector<geometry_msgs::PoseStamped>& path,
double r,
double g,
double b,
double a)
411 ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
416 nav_msgs::Path gui_path;
417 gui_path.poses.resize(path.size());
421 gui_path.header.frame_id = path[0].header.frame_id;
422 gui_path.header.stamp = path[0].header.stamp;
426 for(
unsigned int i=0; i < path.size(); i++)
428 gui_path.poses[i] = path[i];
438 ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
451 ROS_ERROR(
"The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.",
456 double wx = goal.pose.position.x;
457 double wy = goal.pose.position.y;
463 ROS_WARN_THROTTLE(1.0,
"The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal.");
481 for(
int i = len - 1; i >= 0; --i)
484 double world_x, world_y;
487 geometry_msgs::PoseStamped pose;
488 pose.header.stamp = plan_time;
489 pose.header.frame_id = global_frame;
490 pose.pose.position.x = world_x;
491 pose.pose.position.y = world_y;
492 pose.pose.position.z = 0.0;
493 pose.pose.orientation.x = 0.0;
494 pose.pose.orientation.y = 0.0;
495 pose.pose.orientation.z = 0.0;
496 pose.pose.orientation.w = 1.0;
497 plan.push_back(pose);
502 return !plan.empty();
void setCost(unsigned int mx, unsigned int my, unsigned char cost)
bool visualize_potential_
void publishPlan(const std::vector< geometry_msgs::PoseStamped > &path, double r, double g, double b, double a)
Publish a path for visualization purposes.
#define ROS_WARN_THROTTLE(rate,...)
boost::shared_ptr< NavFn > planner_
void publish(const boost::shared_ptr< M > &message) const
std::string getPrefixParam(ros::NodeHandle &nh)
bool computePotential(const geometry_msgs::Point &world_point)
Computes the full navigation function for the map given a point in the world to start from...
costmap_2d::Costmap2DROS * costmap_ros_
Store a copy of the current costmap in costmap. Called by makePlan.
std::string getGlobalFrameID()
Provides a ROS wrapper for the navfn planner which runs a fast, interpolated navigation function on a...
double getOriginX() const
void clearRobotCell(const tf::Stamped< tf::Pose > &global_pose, unsigned int mx, unsigned int my)
bool validPointPotential(const geometry_msgs::Point &world_point)
Check for a valid potential value at a given point in the world (Note: You should call computePotenti...
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &plan)
Given a goal pose in the world, compute a plan.
TFSIMD_FORCE_INLINE const tfScalar & y() const
static const unsigned char FREE_SPACE
#define PLUGINLIB_DECLARE_CLASS(pkg, class_name, class_type, base_class_type)
unsigned char * getCharMap() const
std::string resolve(const std::string &prefix, const std::string &frame_name)
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
unsigned char getCost(unsigned int mx, unsigned int my) const
ros::ServiceServer make_plan_srv_
double sq_distance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2)
Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based. Origin is upper left, x is right, y is down.
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
double getPointPotential(const geometry_msgs::Point &world_point)
Get the potential, or naviagation cost, at a given point in the world (Note: You should call computeP...
TFSIMD_FORCE_INLINE const tfScalar & x() const
bool getPlanFromPotential(const geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &plan)
Compute a plan to a goal after the potential for a start point has already been computed (Note: You s...
double getOriginY() const
bool makePlanService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
unsigned int getSizeInCellsY() const
double default_tolerance_
NavfnROS()
Default constructor for the NavFnROS object.
unsigned int getSizeInCellsX() const
void mapToWorld(double mx, double my, double &wx, double &wy)
pcl_ros::Publisher< PotarrPoint > potarr_pub_
double getResolution() const
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
Initialization function for the NavFnROS object.
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)