navfn_node.cpp
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: Bhaskara Marthi
36 *********************************************************************/
37 #include <navfn/navfn_ros.h>
38 #include <navfn/MakeNavPlan.h>
39 #include <boost/shared_ptr.hpp>
41 
42 namespace cm=costmap_2d;
43 namespace rm=geometry_msgs;
44 
45 using std::vector;
46 using rm::PoseStamped;
47 using std::string;
48 using cm::Costmap2D;
49 using cm::Costmap2DROS;
50 
51 namespace navfn {
52 
53 class NavfnWithCostmap : public NavfnROS
54 {
55 public:
56  NavfnWithCostmap(string name, Costmap2DROS* cmap);
57  bool makePlanService(MakeNavPlan::Request& req, MakeNavPlan::Response& resp);
58 
59 private:
60  void poseCallback(const rm::PoseStamped::ConstPtr& goal);
61  Costmap2DROS* cmap_;
64 };
65 
66 
67 bool NavfnWithCostmap::makePlanService(MakeNavPlan::Request& req, MakeNavPlan::Response& resp)
68 {
69  vector<PoseStamped> path;
70 
71  req.start.header.frame_id = "/map";
72  req.goal.header.frame_id = "/map";
73  bool success = makePlan(req.start, req.goal, path);
74  resp.plan_found = success;
75  if (success) {
76  resp.path = path;
77  }
78 
79  return true;
80 }
81 
82 void NavfnWithCostmap::poseCallback(const rm::PoseStamped::ConstPtr& goal) {
83  tf::Stamped<tf::Pose> global_pose;
84  cmap_->getRobotPose(global_pose);
85  vector<PoseStamped> path;
86  rm::PoseStamped start;
87  start.header.stamp = global_pose.stamp_;
88  start.header.frame_id = global_pose.frame_id_;
89  start.pose.position.x = global_pose.getOrigin().x();
90  start.pose.position.y = global_pose.getOrigin().y();
91  start.pose.position.z = global_pose.getOrigin().z();
92  start.pose.orientation.x = global_pose.getRotation().x();
93  start.pose.orientation.y = global_pose.getRotation().y();
94  start.pose.orientation.z = global_pose.getRotation().z();
95  start.pose.orientation.w = global_pose.getRotation().w();
96  makePlan(start, *goal, path);
97 }
98 
99 
100 NavfnWithCostmap::NavfnWithCostmap(string name, Costmap2DROS* cmap) :
101  NavfnROS(name, cmap)
102 {
103  ros::NodeHandle private_nh("~");
104  cmap_ = cmap;
106  pose_sub_ = private_nh.subscribe<rm::PoseStamped>("goal", 1, &NavfnWithCostmap::poseCallback, this);
107 }
108 
109 } // namespace
110 
111 int main (int argc, char** argv)
112 {
113  ros::init(argc, argv, "global_planner");
114 
116 
117  costmap_2d::Costmap2DROS lcr("costmap", tf);
118 
119  navfn::NavfnWithCostmap navfn("navfn_planner", &lcr);
120 
121  ros::spin();
122  return 0;
123 }
124 
125 
126 
127 
128 
129 
130 
131 
132 
133 
134 
135 
136 
137 
138 
139 
Definition: navfn.h:81
Costmap2DROS * cmap_
Definition: navfn_node.cpp:61
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::ServiceServer make_plan_service_
Definition: navfn_node.cpp:62
NavfnWithCostmap(string name, Costmap2DROS *cmap)
Definition: navfn_node.cpp:100
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Provides a ROS wrapper for the navfn planner which runs a fast, interpolated navigation function on a...
Definition: navfn_ros.h:58
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.
Definition: navfn_ros.cpp:210
ROSCPP_DECL void spin(Spinner &spinner)
ros::Time stamp_
std::string frame_id_
void poseCallback(const rm::PoseStamped::ConstPtr &goal)
Definition: navfn_node.cpp:82
ros::Subscriber pose_sub_
Definition: navfn_node.cpp:63
bool makePlanService(MakeNavPlan::Request &req, MakeNavPlan::Response &resp)
Definition: navfn_node.cpp:67


asr_navfn
Author(s): Kurt Konolige, Eitan Marder-Eppstein, contradict@gmail.com, Felix Marek
autogenerated on Mon Jun 10 2019 12:43:25