planner_node.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #include <ros/ros.h>
37 #include <nav_core2/exceptions.h>
39 #include <geometry_msgs/PoseStamped.h>
40 #include <geometry_msgs/PoseWithCovarianceStamped.h>
41 #include <visualization_msgs/Marker.h>
42 #include <memory>
43 #include <string>
44 
45 namespace dlux_global_planner
46 {
62 {
63 public:
64  PlannerNode() : costmap_loader_("nav_core2", "nav_core2::Costmap"), has_start_(false), has_goal_(false)
65  {
66  ros::NodeHandle nh("~");
67  marker_pub_ = nh.advertise<visualization_msgs::Marker>("/visualization_marker", 10);
68 
69  tf_ = std::make_shared<tf::TransformListener>(ros::Duration(10));
70  std::string costmap_class;
71  nh.param("global_costmap_class", costmap_class, std::string("nav_core_adapter::CostmapAdapter"));
72  costmap_ = costmap_loader_.createUniqueInstance(costmap_class);
73  costmap_->initialize(nh, std::string("global_costmap"), tf_);
74 
75  gp_.initialize(nh, "planner", tf_, costmap_);
76  goal_sub_ = nh.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1,
77  boost::bind(&PlannerNode::goalCB, this, _1));
78  pose_sub_ = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1,
79  boost::bind(&PlannerNode::poseCB, this, _1));
80  nh.param("red", red_, 1.0);
81  nh.param("green", green_, 1.0);
82  nh.param("blue", blue_, 1.0);
83  nh.param("marker_ns", marker_ns_, std::string(""));
84  }
85 
87  {
88  costmap_.reset();
89  }
90 private:
91  void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal)
92  {
93  has_goal_ = true;
95  publishPointMarker(goal_, false);
96  plan();
97  }
98 
99  void poseCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& goal)
100  {
101  has_start_ = true;
102  start_.header = goal->header;
103  start_.pose.x = goal->pose.pose.position.x;
104  start_.pose.y = goal->pose.pose.position.y;
105  publishPointMarker(start_, true);
106  plan();
107  }
108 
109  void plan()
110  {
111  if (!has_goal_ || !has_start_) return;
112  nav_2d_msgs::Path2D plan;
113  try
114  {
115  plan = gp_.makePlan(start_, goal_);
116  }
117  catch (nav_core2::PlannerException& e)
118  {
119  ROS_ERROR("%s", e.what());
120  }
121 
122  // publish plan as markers
123  nav_msgs::Path path = nav_2d_utils::pathToPath(plan);
124  double resolution = costmap_->getResolution();
125 
126  visualization_msgs::Marker m;
127  m.header.stamp = ros::Time::now();
128  m.header.frame_id = plan.header.frame_id;
129  m.ns = marker_ns_ + "path";
130  m.type = visualization_msgs::Marker::LINE_STRIP;
131  m.scale.x = resolution / 4;
132  m.color.r = red_;
133  m.color.g = green_;
134  m.color.b = blue_;
135  m.color.a = 0.7;
136  for (unsigned int i = 0; i < plan.poses.size(); i++)
137  {
138  m.points.push_back(path.poses[i].pose.position);
139  }
140  marker_pub_.publish(m);
141 
142  m.type = visualization_msgs::Marker::SPHERE_LIST;
143  m.id += 1;
144  m.color.r = red_ / 2;
145  m.color.g = green_ / 2;
146  m.color.b = blue_ / 2;
147  m.scale.x = resolution / 2;
148  m.scale.y = resolution / 2;
149  m.scale.z = resolution / 2;
150  marker_pub_.publish(m);
151  }
152 
153  void publishPointMarker(nav_2d_msgs::Pose2DStamped pose, bool start)
154  {
155  visualization_msgs::Marker m;
156  m.header = pose.header;
157  m.header.stamp = ros::Time::now();
158  m.ns = (start ? "start" : "goal");
159  m.type = visualization_msgs::Marker::CYLINDER;
160  m.pose.position.x = pose.pose.x;
161  m.pose.position.y = pose.pose.y;
162 
163  double resolution = costmap_->getResolution();
164  m.scale.x = 4 * resolution;
165  m.scale.y = 4 * resolution;
166  m.scale.z = 0.1;
167  m.color.r = start ? 0.0 : 1.0;
168  m.color.g = start ? 1.0 : 0.0;
169  m.color.a = 0.5;
170  marker_pub_.publish(m);
171  }
172 
175 
180 
181  nav_2d_msgs::Pose2DStamped start_, goal_;
183 
184  double red_, green_, blue_;
185  std::string marker_ns_;
186 };
187 } // namespace dlux_global_planner
188 
189 int main(int argc, char** argv)
190 {
191  ros::init(argc, argv, "global_planner");
193  ros::spin();
194  return 0;
195 }
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Demonstration/debug tool that creates paths between arbitrary points.
nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path &path)
void poseCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &goal)
nav_2d_msgs::Pose2DStamped goal_
ROSCPP_DECL void spin(Spinner &spinner)
nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const geometry_msgs::PoseStamped &pose)
pluginlib::ClassLoader< nav_core2::Costmap > costmap_loader_
int main(int argc, char **argv)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
nav_2d_msgs::Pose2DStamped start_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Plugin-based global wavefront planner that conforms to the nav_core2 GlobalPlanner interface...
void publishPointMarker(nav_2d_msgs::Pose2DStamped pose, bool start)
nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped &start, const nav_2d_msgs::Pose2DStamped &goal) override
nav_core2::Costmap::Ptr costmap_
static Time now()
void initialize(const ros::NodeHandle &parent, const std::string &name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override
std::shared_ptr< Costmap > Ptr
std::shared_ptr< tf::TransformListener > TFListenerPtr
dlux_global_planner::DluxGlobalPlanner gp_
#define ROS_ERROR(...)
void goalCB(const geometry_msgs::PoseStamped::ConstPtr &goal)


dlux_global_planner
Author(s):
autogenerated on Sun Jan 10 2021 04:08:52