costmap_3d.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2014-2018, the neonavigation authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <ros/ros.h>
31 #include <geometry_msgs/PolygonStamped.h>
32 #include <nav_msgs/OccupancyGrid.h>
33 #include <sensor_msgs/PointCloud.h>
34 
35 #include <string>
36 #include <utility>
37 #include <vector>
38 
39 #include <costmap_cspace_msgs/CSpace3D.h>
40 #include <costmap_cspace_msgs/CSpace3DUpdate.h>
41 
44 
46 {
47 protected:
51  std::vector<ros::Subscriber> sub_map_overlay_;
57 
59  std::vector<
60  std::pair<nav_msgs::OccupancyGrid::ConstPtr,
62 
63  void cbMap(
64  const nav_msgs::OccupancyGrid::ConstPtr& msg,
66  {
67  if (map->getAngularGrid() <= 0)
68  {
69  ROS_ERROR("ang_resolution is not set.");
70  std::runtime_error("ang_resolution is not set.");
71  }
72  ROS_INFO("2D costmap received");
73 
74  map->setBaseMap(msg);
75  ROS_DEBUG("C-Space costmap generated");
76 
77  if (map_buffer_.size() > 0)
78  {
79  for (auto& map : map_buffer_)
80  cbMapOverlay(map.first, map.second);
81  ROS_INFO("%ld buffered costmaps processed", map_buffer_.size());
82  map_buffer_.clear();
83  }
84  }
86  const nav_msgs::OccupancyGrid::ConstPtr& msg,
88  {
89  ROS_DEBUG("Overlay 2D costmap received");
90 
91  auto map_msg = map->getMap();
92  if (map_msg->info.width < 1 ||
93  map_msg->info.height < 1)
94  {
95  map_buffer_.push_back(
96  std::pair<nav_msgs::OccupancyGrid::ConstPtr,
98  return;
99  }
100 
101  map->processMapOverlay(msg);
102  ROS_DEBUG("C-Space costmap updated");
103  }
106  const costmap_cspace_msgs::CSpace3DUpdate::Ptr update)
107  {
108  publishDebug(*map);
109  pub_costmap_.publish<costmap_cspace_msgs::CSpace3D>(*map);
110  return true;
111  }
112  bool cbUpdate(
114  const costmap_cspace_msgs::CSpace3DUpdate::Ptr update)
115  {
116  if (update)
117  {
118  publishDebug(*map);
119  pub_costmap_update_.publish(*update);
120  }
121  else
122  {
123  ROS_WARN("Updated region of the costmap is empty. The position may be out-of-boundary, or input map is wrong.");
124  }
125  return true;
126  }
127  void publishDebug(const costmap_cspace_msgs::CSpace3D& map)
128  {
129  if (pub_debug_.getNumSubscribers() == 0)
130  return;
131  sensor_msgs::PointCloud pc;
132  pc.header = map.header;
133  pc.header.stamp = ros::Time::now();
134  for (size_t yaw = 0; yaw < map.info.angle; yaw++)
135  {
136  for (unsigned int i = 0; i < map.info.width * map.info.height; i++)
137  {
138  int gx = i % map.info.width;
139  int gy = i / map.info.width;
140  if (map.data[i + yaw * map.info.width * map.info.height] < 100)
141  continue;
142  geometry_msgs::Point32 p;
143  p.x = gx * map.info.linear_resolution + map.info.origin.position.x;
144  p.y = gy * map.info.linear_resolution + map.info.origin.position.y;
145  p.z = yaw * 0.1;
146  pc.points.push_back(p);
147  }
148  }
149  pub_debug_.publish(pc);
150  }
151  void cbPublishFootprint(const ros::TimerEvent& event, const geometry_msgs::PolygonStamped msg)
152  {
153  auto footprint = msg;
154  footprint.header.stamp = ros::Time::now();
155  pub_footprint_.publish(footprint);
156  }
157 
159  const std::string overlay_mode_str)
160  {
161  if (overlay_mode_str == "overwrite")
162  {
164  }
165  else if (overlay_mode_str == "max")
166  {
168  }
169  ROS_FATAL("Unknown overlay_mode \"%s\"", overlay_mode_str.c_str());
170  throw std::runtime_error("Unknown overlay_mode.");
171  };
172 
173 public:
175  : nh_()
176  , pnh_("~")
177  {
179  pub_costmap_ = neonavigation_common::compat::advertise<costmap_cspace_msgs::CSpace3D>(
180  nh_, "costmap",
181  pnh_, "costmap", 1, true);
182  pub_costmap_update_ = neonavigation_common::compat::advertise<costmap_cspace_msgs::CSpace3DUpdate>(
183  nh_, "costmap_update",
184  pnh_, "costmap_update", 1, true);
185  pub_footprint_ = pnh_.advertise<geometry_msgs::PolygonStamped>("footprint", 2, true);
186  pub_debug_ = pnh_.advertise<sensor_msgs::PointCloud>("debug", 1, true);
187 
188  int ang_resolution;
189  pnh_.param("ang_resolution", ang_resolution, 16);
190 
191  XmlRpc::XmlRpcValue footprint_xml;
192  if (!pnh_.hasParam("footprint"))
193  {
194  ROS_FATAL("Footprint doesn't specified");
195  throw std::runtime_error("Footprint doesn't specified.");
196  }
197  pnh_.getParam("footprint", footprint_xml);
198  costmap_cspace::Polygon footprint;
199  try
200  {
201  footprint = costmap_cspace::Polygon(footprint_xml);
202  }
203  catch (const std::exception& e)
204  {
205  ROS_FATAL("Invalid footprint");
206  throw e;
207  }
208 
209  costmap_.reset(new costmap_cspace::Costmap3d(ang_resolution));
210 
211  auto root_layer = costmap_->addRootLayer<costmap_cspace::Costmap3dLayerFootprint>();
212  float linear_expand;
213  float linear_spread;
214  pnh_.param("linear_expand", linear_expand, 0.2f);
215  pnh_.param("linear_spread", linear_spread, 0.5f);
216  root_layer->setExpansion(linear_expand, linear_spread);
217  root_layer->setFootprint(footprint);
218 
219  if (pnh_.hasParam("static_layers"))
220  {
221  XmlRpc::XmlRpcValue layers_xml;
222  pnh_.getParam("static_layers", layers_xml);
223 
224  if (layers_xml.getType() != XmlRpc::XmlRpcValue::TypeArray || layers_xml.size() < 1)
225  {
226  ROS_FATAL("static_layers parameter must contain at least one layer config.");
227  ROS_ERROR(
228  "Migration from old version:\n"
229  "--- # Old\n"
230  "static_layers:\n"
231  " YOUR_LAYER_NAME:\n"
232  " type: LAYER_TYPE\n"
233  " parameters: values\n"
234  "--- # New\n"
235  "static_layers:\n"
236  " - name: YOUR_LAYER_NAME\n"
237  " type: LAYER_TYPE\n"
238  " parameters: values\n"
239  "---\n");
240  throw std::runtime_error("layers parameter must contain at least one layer config.");
241  }
242  for (int i = 0; i < layers_xml.size(); ++i)
243  {
244  auto layer_xml = std::pair<std::string, XmlRpc::XmlRpcValue>(
245  layers_xml[i]["name"], layers_xml[i]);
246  ROS_INFO("New static layer: %s", layer_xml.first.c_str());
247 
249  if (layer_xml.second["overlay_mode"].getType() == XmlRpc::XmlRpcValue::TypeString)
250  overlay_mode = getMapOverlayModeFromString(
251  layer_xml.second["overlay_mode"]);
252  else
253  ROS_WARN("overlay_mode of the static layer is not specified. Using MAX mode.");
254 
255  std::string type;
256  if (layer_xml.second["type"].getType() == XmlRpc::XmlRpcValue::TypeString)
257  type = std::string(layer_xml.second["type"]);
258  else
259  {
260  ROS_FATAL("Layer type is not specified.");
261  throw std::runtime_error("Layer type is not specified.");
262  }
263 
264  if (!layer_xml.second.hasMember("footprint"))
265  layer_xml.second["footprint"] = footprint_xml;
266 
269  costmap_->addLayer(layer, overlay_mode);
270  layer->loadConfig(layer_xml.second);
271 
272  sub_map_overlay_.push_back(nh_.subscribe<nav_msgs::OccupancyGrid>(
273  layer_xml.first, 1,
274  boost::bind(&Costmap3DOFNode::cbMapOverlay, this, _1, layer)));
275  }
276  }
277 
278  auto static_output_layer = costmap_->addLayer<costmap_cspace::Costmap3dLayerOutput>();
279  static_output_layer->setHandler(boost::bind(&Costmap3DOFNode::cbUpdateStatic, this, _1, _2));
280 
281  sub_map_ = nh_.subscribe<nav_msgs::OccupancyGrid>(
282  "map", 1,
283  boost::bind(&Costmap3DOFNode::cbMap, this, _1, root_layer));
284 
285  if (pnh_.hasParam("layers"))
286  {
287  XmlRpc::XmlRpcValue layers_xml;
288  pnh_.getParam("layers", layers_xml);
289 
290  if (layers_xml.getType() != XmlRpc::XmlRpcValue::TypeArray || layers_xml.size() < 1)
291  {
292  ROS_FATAL("layers parameter must contain at least one layer config.");
293  ROS_ERROR(
294  "Migration from old version:\n"
295  "--- # Old\n"
296  "layers:\n"
297  " YOUR_LAYER_NAME:\n"
298  " type: LAYER_TYPE\n"
299  " parameters: values\n"
300  "--- # New\n"
301  "layers:\n"
302  " - name: YOUR_LAYER_NAME\n"
303  " type: LAYER_TYPE\n"
304  " parameters: values\n"
305  "---\n");
306  throw std::runtime_error("layers parameter must contain at least one layer config.");
307  }
308  for (int i = 0; i < layers_xml.size(); ++i)
309  {
310  auto layer_xml = std::pair<std::string, XmlRpc::XmlRpcValue>(
311  layers_xml[i]["name"], layers_xml[i]);
312  ROS_INFO("New layer: %s", layer_xml.first.c_str());
313 
315  if (layer_xml.second["overlay_mode"].getType() == XmlRpc::XmlRpcValue::TypeString)
316  overlay_mode = getMapOverlayModeFromString(
317  layer_xml.second["overlay_mode"]);
318  else
319  ROS_WARN("overlay_mode of the layer is not specified. Using MAX mode.");
320 
321  std::string type;
322  if (layer_xml.second["type"].getType() == XmlRpc::XmlRpcValue::TypeString)
323  type = std::string(layer_xml.second["type"]);
324  else
325  {
326  ROS_FATAL("Layer type is not specified.");
327  throw std::runtime_error("Layer type is not specified.");
328  }
329 
330  if (!layer_xml.second.hasMember("footprint"))
331  layer_xml.second["footprint"] = footprint_xml;
332 
335  costmap_->addLayer(layer, overlay_mode);
336  layer->loadConfig(layer_xml.second);
337 
338  sub_map_overlay_.push_back(nh_.subscribe<nav_msgs::OccupancyGrid>(
339  layer_xml.first, 1,
340  boost::bind(&Costmap3DOFNode::cbMapOverlay, this, _1, layer)));
341  }
342  }
343  else
344  {
345  // Single layer mode for backward-compatibility
346  costmap_cspace::MapOverlayMode overlay_mode;
347  std::string overlay_mode_str;
348  pnh_.param("overlay_mode", overlay_mode_str, std::string("max"));
349  if (overlay_mode_str.compare("overwrite") == 0)
351  else if (overlay_mode_str.compare("max") == 0)
353  else
354  {
355  ROS_FATAL("Unknown overlay_mode \"%s\"", overlay_mode_str.c_str());
356  throw std::runtime_error("Unknown overlay_mode.");
357  }
358  ROS_INFO("costmap_3d: %s mode", overlay_mode_str.c_str());
359 
360  XmlRpc::XmlRpcValue layer_xml;
361  layer_xml["footprint"] = footprint_xml;
362  layer_xml["linear_expand"] = linear_expand;
363  layer_xml["linear_spread"] = linear_spread;
364 
365  auto layer = costmap_->addLayer<costmap_cspace::Costmap3dLayerFootprint>(overlay_mode);
366  layer->loadConfig(layer_xml);
367  sub_map_overlay_.push_back(nh_.subscribe<nav_msgs::OccupancyGrid>(
368  "map_overlay", 1,
369  boost::bind(&Costmap3DOFNode::cbMapOverlay, this, _1, layer)));
370  }
371 
372  auto update_output_layer = costmap_->addLayer<costmap_cspace::Costmap3dLayerOutput>();
373  update_output_layer->setHandler(boost::bind(&Costmap3DOFNode::cbUpdate, this, _1, _2));
374 
375  const geometry_msgs::PolygonStamped footprint_msg = footprint.toMsg();
376  timer_footprint_ = nh_.createTimer(
377  ros::Duration(1.0),
378  boost::bind(&Costmap3DOFNode::cbPublishFootprint, this, _1, footprint_msg));
379  }
380 };
381 
382 int main(int argc, char* argv[])
383 {
384  ros::init(argc, argv, "costmap_3d");
385 
386  Costmap3DOFNode cm;
387  ros::spin();
388 
389  return 0;
390 }
#define ROS_FATAL(...)
void publish(const boost::shared_ptr< M > &message) const
f
int size() const
void setExpansion(const float linear_expand, const float linear_spread)
Definition: footprint.h:91
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void loadConfig(XmlRpc::XmlRpcValue config)
Definition: footprint.h:78
geometry_msgs::PolygonStamped toMsg() const
Definition: polygon.h:123
void cbMapOverlay(const nav_msgs::OccupancyGrid::ConstPtr &msg, const costmap_cspace::Costmap3dLayerBase::Ptr map)
Definition: costmap_3d.cpp:85
Type const & getType() const
std::vector< ros::Subscriber > sub_map_overlay_
Definition: costmap_3d.cpp:51
#define ROS_WARN(...)
bool cbUpdate(const costmap_cspace::CSpace3DMsg::Ptr map, const costmap_cspace_msgs::CSpace3DUpdate::Ptr update)
Definition: costmap_3d.cpp:112
ROSCPP_DECL void spin(Spinner &spinner)
ros::Timer timer_footprint_
Definition: costmap_3d.cpp:56
ros::NodeHandle pnh_
Definition: costmap_3d.cpp:49
MapOverlayMode
Definition: base.h:96
ros::Publisher pub_debug_
Definition: costmap_3d.cpp:55
#define ROS_INFO(...)
bool cbUpdateStatic(const costmap_cspace::CSpace3DMsg::Ptr map, const costmap_cspace_msgs::CSpace3DUpdate::Ptr update)
Definition: costmap_3d.cpp:104
ros::NodeHandle nh_
Definition: costmap_3d.cpp:48
std::vector< std::pair< nav_msgs::OccupancyGrid::ConstPtr, costmap_cspace::Costmap3dLayerBase::Ptr > > map_buffer_
Definition: costmap_3d.cpp:61
uint32_t getNumSubscribers() const
static costmap_cspace::MapOverlayMode getMapOverlayModeFromString(const std::string overlay_mode_str)
Definition: costmap_3d.cpp:158
std::shared_ptr< CSpace3DMsg > Ptr
Definition: base.h:52
ros::Publisher pub_footprint_
Definition: costmap_3d.cpp:54
void publishDebug(const costmap_cspace_msgs::CSpace3D &map)
Definition: costmap_3d.cpp:127
costmap_cspace::Costmap3d::Ptr costmap_
Definition: costmap_3d.cpp:58
static Time now()
std::shared_ptr< Costmap3dLayerBase > Ptr
Definition: base.h:255
ros::Publisher pub_costmap_update_
Definition: costmap_3d.cpp:53
void cbPublishFootprint(const ros::TimerEvent &event, const geometry_msgs::PolygonStamped msg)
Definition: costmap_3d.cpp:151
void setHandler(Callback cb)
Definition: output.h:58
std::shared_ptr< Costmap3d > Ptr
Definition: costmap_3d.h:55
#define ROS_ERROR(...)
ros::Subscriber sub_map_
Definition: costmap_3d.cpp:50
ros::Publisher pub_costmap_
Definition: costmap_3d.cpp:52
int main(int argc, char *argv[])
Definition: costmap_3d.cpp:382
static Costmap3dLayerBase::Ptr loadClass(const std::string &name)
Definition: class_loader.h:71
void cbMap(const nav_msgs::OccupancyGrid::ConstPtr &msg, const costmap_cspace::Costmap3dLayerBase::Ptr map)
Definition: costmap_3d.cpp:63
#define ROS_DEBUG(...)


costmap_cspace
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:20:29