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 <vector>
48 #include <nav_msgs/GetPlan.h>
49 #include <navfn/potarr_point.h>
50 
51 namespace navfn {
56  class NavfnROS : public nav_core::BaseGlobalPlanner {
57  public:
61  NavfnROS();
62 
68  NavfnROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
69 
76  NavfnROS(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame);
77 
83  void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
84 
91  void initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame);
92 
100  bool makePlan(const geometry_msgs::PoseStamped& start,
101  const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
102 
111  bool makePlan(const geometry_msgs::PoseStamped& start,
112  const geometry_msgs::PoseStamped& goal, double tolerance, std::vector<geometry_msgs::PoseStamped>& plan);
113 
119  bool computePotential(const geometry_msgs::Point& world_point);
120 
127  bool getPlanFromPotential(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
128 
134  double getPointPotential(const geometry_msgs::Point& world_point);
135 
141  bool validPointPotential(const geometry_msgs::Point& world_point);
142 
149  bool validPointPotential(const geometry_msgs::Point& world_point, double tolerance);
150 
154  void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, double r, double g, double b, double a);
155 
156  ~NavfnROS(){}
157 
158  bool makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp);
159 
160  protected:
161 
170 
171 
172  private:
173  inline double sq_distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2){
174  double dx = p1.pose.position.x - p2.pose.position.x;
175  double dy = p1.pose.position.y - p2.pose.position.y;
176  return dx*dx +dy*dy;
177  }
178 
179  void mapToWorld(double mx, double my, double& wx, double& wy);
180  void clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my);
182  boost::mutex mutex_;
184  std::string global_frame_;
185  };
186 };
187 
188 #endif
navfn::NavfnROS::planner_window_y_
double planner_window_y_
Definition: navfn_ros.h:216
navfn::NavfnROS::global_frame_
std::string global_frame_
Definition: navfn_ros.h:219
navfn::NavfnROS::make_plan_srv_
ros::ServiceServer make_plan_srv_
Definition: navfn_ros.h:218
navfn::NavfnROS::NavfnROS
NavfnROS()
Default constructor for the NavFnROS object.
Definition: navfn_ros.cpp:83
navfn::NavfnROS::potarr_pub_
ros::Publisher potarr_pub_
Definition: navfn_ros.h:203
navfn::NavfnROS::getPlanFromPotential
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:405
navfn::NavfnROS::makePlanService
bool makePlanService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp)
Definition: navfn_ros.cpp:214
navfn::NavfnROS::sq_distance
double sq_distance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2)
Definition: navfn_ros.h:208
ros::Publisher
boost::shared_ptr
navfn::NavfnROS::getPointPotential
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:162
ros.h
costmap_2d.h
navfn
Definition: navfn.h:81
navfn::NavfnROS::~NavfnROS
~NavfnROS()
Definition: navfn_ros.h:191
costmap_2d::Costmap2D
navfn.h
navfn::NavfnROS::clearRobotCell
void clearRobotCell(const geometry_msgs::PoseStamped &global_pose, unsigned int mx, unsigned int my)
Definition: navfn_ros.cpp:204
navfn::NavfnROS::publishPlan
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:378
ros::ServiceServer
potarr_point.h
navfn::NavfnROS::planner_
boost::shared_ptr< NavFn > planner_
Definition: navfn_ros.h:201
navfn::NavfnROS::default_tolerance_
double default_tolerance_
Definition: navfn_ros.h:216
navfn::NavfnROS::mapToWorld
void mapToWorld(double mx, double my, double &wx, double &wy)
Definition: navfn_ros.cpp:223
start
int start[2]
Definition: navtest.cpp:34
goal
int goal[2]
Definition: navtest.cpp:33
navfn::NavfnROS::plan_pub_
ros::Publisher plan_pub_
Definition: navfn_ros.h:202
navfn::NavfnROS::validPointPotential
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:131
base_global_planner.h
navfn::NavfnROS::visualize_potential_
bool visualize_potential_
Definition: navfn_ros.h:204
navfn::NavfnROS::initialize
void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
Initialization function for the NavFnROS object.
Definition: navfn_ros.cpp:127
navfn::NavfnROS::costmap_
costmap_2d::Costmap2D * costmap_
Store a copy of the current costmap in costmap. Called by makePlan.
Definition: navfn_ros.h:200
navfn::NavfnROS::planner_window_x_
double planner_window_x_
Definition: navfn_ros.h:216
navfn::NavfnROS::mutex_
boost::mutex mutex_
Definition: navfn_ros.h:217
nav_core::BaseGlobalPlanner
navfn::NavfnROS::allow_unknown_
bool allow_unknown_
Definition: navfn_ros.h:204
navfn::NavfnROS::computePotential
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:176
navfn::NavfnROS::initialized_
bool initialized_
Definition: navfn_ros.h:204
costmap_2d::Costmap2DROS
navfn::NavfnROS::makePlan
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:228


navfn
Author(s): Kurt Konolige, Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Mon Mar 6 2023 03:50:37