navfn_ros.h
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2008, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the Willow Garage nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Eitan Marder-Eppstein
36 *********************************************************************/
37 #ifndef NAVFN_NAVFN_ROS_H_
38 #define NAVFN_NAVFN_ROS_H_
39 
40 #include <ros/ros.h>
41 #include <navfn/navfn.h>
42 #include <costmap_2d/costmap_2d.h>
43 #include <geometry_msgs/PoseStamped.h>
44 #include <geometry_msgs/Point.h>
45 #include <nav_msgs/Path.h>
46 #include <tf/transform_datatypes.h>
47 #include <vector>
49 #include <nav_msgs/GetPlan.h>
50 #include <navfn/potarr_point.h>
51 #include <pcl_ros/publisher.h>
52 
53 namespace navfn {
59  public:
63  NavfnROS();
64 
70  NavfnROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
71 
78  NavfnROS(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame);
79 
85  void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
86 
93  void initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame);
94 
102  bool makePlan(const geometry_msgs::PoseStamped& start,
103  const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
104 
113  bool makePlan(const geometry_msgs::PoseStamped& start,
114  const geometry_msgs::PoseStamped& goal, double tolerance, std::vector<geometry_msgs::PoseStamped>& plan);
115 
121  bool computePotential(const geometry_msgs::Point& world_point);
122 
129  bool getPlanFromPotential(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
130 
136  double getPointPotential(const geometry_msgs::Point& world_point);
137 
143  bool validPointPotential(const geometry_msgs::Point& world_point);
144 
151  bool validPointPotential(const geometry_msgs::Point& world_point, double tolerance);
152 
156  void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, double r, double g, double b, double a);
157 
159 
160  bool makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp);
161 
162  protected:
163 
172 
173 
174  private:
175  inline double sq_distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2){
176  double dx = p1.pose.position.x - p2.pose.position.x;
177  double dy = p1.pose.position.y - p2.pose.position.y;
178  return dx*dx +dy*dy;
179  }
180 
181  void mapToWorld(double mx, double my, double& wx, double& wy);
182  void clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my);
184  std::string tf_prefix_;
185  boost::mutex mutex_;
187  std::string global_frame_;
188  };
189 };
190 
191 #endif
Definition: navfn.h:81
bool visualize_potential_
Definition: navfn_ros.h:171
void publishPlan(const std::vector< geometry_msgs::PoseStamped > &path, double r, double g, double b, double a)
Publish a path for visualization purposes.
Definition: navfn_ros.cpp:363
boost::shared_ptr< NavFn > planner_
Definition: navfn_ros.h:168
ros::Publisher plan_pub_
Definition: navfn_ros.h:169
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...
Definition: navfn_ros.cpp:147
costmap_2d::Costmap2D * costmap_
Store a copy of the current costmap in costmap. Called by makePlan.
Definition: navfn_ros.h:167
Provides a ROS wrapper for the navfn planner which runs a fast, interpolated navigation function on a...
Definition: navfn_ros.h:58
bool allow_unknown_
Definition: navfn_ros.h:171
std::string tf_prefix_
Definition: navfn_ros.h:184
void clearRobotCell(const tf::Stamped< tf::Pose > &global_pose, unsigned int mx, unsigned int my)
Definition: navfn_ros.cpp:175
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...
Definition: navfn_ros.cpp:102
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.
Definition: navfn_ros.cpp:199
ros::ServiceServer make_plan_srv_
Definition: navfn_ros.h:186
double sq_distance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2)
Definition: navfn_ros.h:175
boost::mutex mutex_
Definition: navfn_ros.h:185
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...
Definition: navfn_ros.cpp:133
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...
Definition: navfn_ros.cpp:387
bool makePlanService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp)
Definition: navfn_ros.cpp:185
double default_tolerance_
Definition: navfn_ros.h:183
double planner_window_y_
Definition: navfn_ros.h:183
double planner_window_x_
Definition: navfn_ros.h:183
NavfnROS()
Default constructor for the NavFnROS object.
Definition: navfn_ros.cpp:50
void mapToWorld(double mx, double my, double &wx, double &wy)
Definition: navfn_ros.cpp:194
pcl_ros::Publisher< PotarrPoint > potarr_pub_
Definition: navfn_ros.h:170
std::string global_frame_
Definition: navfn_ros.h:187
void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
Initialization function for the NavFnROS object.
Definition: navfn_ros.cpp:98


navfn
Author(s): Kurt Konolige, Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:06:04