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>
42 
43 namespace cm=costmap_2d;
44 namespace rm=geometry_msgs;
45 
46 using std::vector;
47 using rm::PoseStamped;
48 using std::string;
49 using cm::Costmap2D;
50 using cm::Costmap2DROS;
51 
52 namespace navfn {
53 
54 class NavfnWithCostmap : public NavfnROS
55 {
56 public:
57  NavfnWithCostmap(string name, Costmap2DROS* cmap);
58  bool makePlanService(MakeNavPlan::Request& req, MakeNavPlan::Response& resp);
59 
60 private:
61  void poseCallback(const rm::PoseStamped::ConstPtr& goal);
65 };
66 
67 
68 bool NavfnWithCostmap::makePlanService(MakeNavPlan::Request& req, MakeNavPlan::Response& resp)
69 {
70  vector<PoseStamped> path;
71 
72  req.start.header.frame_id = "map";
73  req.goal.header.frame_id = "map";
74  bool success = makePlan(req.start, req.goal, path);
75  resp.plan_found = success;
76  if (success) {
77  resp.path = path;
78  }
79 
80  return true;
81 }
82 
83 void NavfnWithCostmap::poseCallback(const rm::PoseStamped::ConstPtr& goal) {
84  geometry_msgs::PoseStamped global_pose;
85  cmap_->getRobotPose(global_pose);
86  vector<PoseStamped> path;
87  makePlan(global_pose, *goal, path);
88 }
89 
90 
92  NavfnROS(name, cmap)
93 {
94  ros::NodeHandle private_nh("~");
95  cmap_ = cmap;
97  pose_sub_ = private_nh.subscribe<rm::PoseStamped>("goal", 1, &NavfnWithCostmap::poseCallback, this);
98 }
99 
100 } // namespace
101 
102 int main (int argc, char** argv)
103 {
104  ros::init(argc, argv, "global_planner");
105 
106  tf2_ros::Buffer buffer(ros::Duration(10));
108 
109  costmap_2d::Costmap2DROS lcr("costmap", buffer);
110 
111  navfn::NavfnWithCostmap navfn("navfn_planner", &lcr);
112 
113  ros::spin();
114  return 0;
115 }
116 
117 
118 
119 
120 
121 
122 
123 
124 
125 
126 
127 
128 
129 
130 
131 
navfn::NavfnWithCostmap
Definition: navfn_node.cpp:54
navfn::NavfnWithCostmap::pose_sub_
ros::Subscriber pose_sub_
Definition: navfn_node.cpp:64
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
navfn
Definition: navfn.h:81
geometry_msgs
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
costmap_2d::Costmap2D
costmap_2d_ros.h
ros::ServiceServer
tf2_ros::TransformListener
navfn::NavfnWithCostmap::poseCallback
void poseCallback(const rm::PoseStamped::ConstPtr &goal)
Definition: navfn_node.cpp:83
main
int main(int argc, char **argv)
Definition: navfn_node.cpp:102
navfn::NavfnROS
Provides a ROS wrapper for the navfn planner which runs a fast, interpolated navigation function on a...
Definition: navfn_ros.h:91
tf2_ros::Buffer
goal
int goal[2]
Definition: navtest.cpp:33
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
transform_listener.h
costmap_2d::Costmap2DROS::getRobotPose
bool getRobotPose(geometry_msgs::PoseStamped &global_pose) const
navfn_ros.h
navfn::NavfnWithCostmap::make_plan_service_
ros::ServiceServer make_plan_service_
Definition: navfn_node.cpp:63
ros::spin
ROSCPP_DECL void spin()
tf
navfn::NavfnWithCostmap::NavfnWithCostmap
NavfnWithCostmap(string name, Costmap2DROS *cmap)
Definition: navfn_node.cpp:91
navfn::NavfnWithCostmap::makePlanService
bool makePlanService(MakeNavPlan::Request &req, MakeNavPlan::Response &resp)
Definition: navfn_node.cpp:68
navfn::NavfnWithCostmap::cmap_
Costmap2DROS * cmap_
Definition: navfn_node.cpp:62
costmap_2d
ros::Duration
costmap_2d::Costmap2DROS
ros::NodeHandle
ros::Subscriber
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