costmap_converter_node.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2016,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the institute nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author: Christoph Rösmann, Otniel Rinaldo
37  *********************************************************************/
38 
39 #include <ros/ros.h>
40 
41 #include <costmap_2d/costmap_2d.h>
42 #include <nav_msgs/OccupancyGrid.h>
43 #include <geometry_msgs/PolygonStamped.h>
44 #include <visualization_msgs/Marker.h>
45 
47 #include <pluginlib/class_loader.h>
48 
49 
50 
52 {
53 public:
54  CostmapStandaloneConversion() : converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"), n_("~")
55  {
56  // load converter plugin from parameter server, otherwise set default
57  std::string converter_plugin = "costmap_converter::CostmapToPolygonsDBSMCCH";
58  n_.param("converter_plugin", converter_plugin, converter_plugin);
59 
60  try
61  {
62  converter_ = converter_loader_.createInstance(converter_plugin);
63  }
64  catch(const pluginlib::PluginlibException& ex)
65  {
66  ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
67  ros::shutdown();
68  }
69 
70  ROS_INFO_STREAM("Standalone costmap converter:" << converter_plugin << " loaded.");
71 
72  std::string costmap_topic = "/move_base/local_costmap/costmap";
73  n_.param("costmap_topic", costmap_topic, costmap_topic);
74 
75  std::string costmap_update_topic = "/move_base/local_costmap/costmap_updates";
76  n_.param("costmap_update_topic", costmap_update_topic, costmap_update_topic);
77 
78  std::string obstacles_topic = "costmap_obstacles";
79  n_.param("obstacles_topic", obstacles_topic, obstacles_topic);
80 
81  std::string polygon_marker_topic = "costmap_polygon_markers";
82  n_.param("polygon_marker_topic", polygon_marker_topic, polygon_marker_topic);
83 
86  obstacle_pub_ = n_.advertise<costmap_converter::ObstacleArrayMsg>(obstacles_topic, 1000);
87  marker_pub_ = n_.advertise<visualization_msgs::Marker>(polygon_marker_topic, 10);
88 
89  occupied_min_value_ = 100;
90  n_.param("occupied_min_value", occupied_min_value_, occupied_min_value_);
91 
92  std::string odom_topic = "/odom";
93  n_.param("odom_topic", odom_topic, odom_topic);
94 
95  if (converter_)
96  {
97  converter_->setOdomTopic(odom_topic);
98  converter_->initialize(n_);
99  converter_->setCostmap2D(&map_);
100  //converter_->startWorker(ros::Rate(5), &map, true);
101  }
102  }
103 
104 
105  void costmapCallback(const nav_msgs::OccupancyGridConstPtr& msg)
106  {
107  ROS_INFO_ONCE("Got first costmap callback. This message will be printed once");
108 
109  if (msg->info.width != map_.getSizeInCellsX() || msg->info.height != map_.getSizeInCellsY() || msg->info.resolution != map_.getResolution())
110  {
111  ROS_INFO("New map format, resizing and resetting map...");
112  map_.resizeMap(msg->info.width, msg->info.height, msg->info.resolution, msg->info.origin.position.x, msg->info.origin.position.y);
113  }
114  else
115  {
116  map_.updateOrigin(msg->info.origin.position.x, msg->info.origin.position.y);
117  }
118 
119 
120  for (std::size_t i=0; i < msg->data.size(); ++i)
121  {
122  unsigned int mx, my;
123  map_.indexToCells((unsigned int)i, mx, my);
124  map_.setCost(mx, my, msg->data[i] >= occupied_min_value_ ? 255 : 0 );
125  }
126 
127  // convert
128  converter_->updateCostmap2D();
129  converter_->compute();
130  costmap_converter::ObstacleArrayConstPtr obstacles = converter_->getObstacles();
131 
132  if (!obstacles)
133  return;
134 
135  obstacle_pub_.publish(obstacles);
136 
137  frame_id_ = msg->header.frame_id;
138 
139  publishAsMarker(frame_id_, *obstacles, marker_pub_);
140  }
141 
142  void costmapUpdateCallback(const map_msgs::OccupancyGridUpdateConstPtr& update)
143  {
144  unsigned int di = 0;
145  for (unsigned int y = 0; y < update->height ; ++y)
146  {
147  for (unsigned int x = 0; x < update->width ; ++x)
148  {
149  map_.setCost(x, y, update->data[di++] >= occupied_min_value_ ? 255 : 0 );
150  }
151  }
152 
153  // convert
154  // TODO(roesmann): currently, the converter updates the complete costmap and not the part which is updated in this callback
155  converter_->updateCostmap2D();
156  converter_->compute();
157  costmap_converter::ObstacleArrayConstPtr obstacles = converter_->getObstacles();
158 
159  if (!obstacles)
160  return;
161 
162  obstacle_pub_.publish(obstacles);
163 
164  publishAsMarker(frame_id_, *obstacles, marker_pub_);
165  }
166 
167  void publishAsMarker(const std::string& frame_id, const std::vector<geometry_msgs::PolygonStamped>& polygonStamped, ros::Publisher& marker_pub)
168  {
169  visualization_msgs::Marker line_list;
170  line_list.header.frame_id = frame_id;
171  line_list.header.stamp = ros::Time::now();
172  line_list.ns = "Polygons";
173  line_list.action = visualization_msgs::Marker::ADD;
174  line_list.pose.orientation.w = 1.0;
175 
176  line_list.id = 0;
177  line_list.type = visualization_msgs::Marker::LINE_LIST;
178 
179  line_list.scale.x = 0.1;
180  line_list.color.g = 1.0;
181  line_list.color.a = 1.0;
182 
183  for (std::size_t i=0; i<polygonStamped.size(); ++i)
184  {
185  for (int j=0; j< (int)polygonStamped[i].polygon.points.size()-1; ++j)
186  {
187  geometry_msgs::Point line_start;
188  line_start.x = polygonStamped[i].polygon.points[j].x;
189  line_start.y = polygonStamped[i].polygon.points[j].y;
190  line_list.points.push_back(line_start);
191  geometry_msgs::Point line_end;
192  line_end.x = polygonStamped[i].polygon.points[j+1].x;
193  line_end.y = polygonStamped[i].polygon.points[j+1].y;
194  line_list.points.push_back(line_end);
195  }
196  // close loop for current polygon
197  if (!polygonStamped[i].polygon.points.empty() && polygonStamped[i].polygon.points.size() != 2 )
198  {
199  geometry_msgs::Point line_start;
200  line_start.x = polygonStamped[i].polygon.points.back().x;
201  line_start.y = polygonStamped[i].polygon.points.back().y;
202  line_list.points.push_back(line_start);
203  if (line_list.points.size() % 2 != 0)
204  {
205  geometry_msgs::Point line_end;
206  line_end.x = polygonStamped[i].polygon.points.front().x;
207  line_end.y = polygonStamped[i].polygon.points.front().y;
208  line_list.points.push_back(line_end);
209  }
210  }
211 
212 
213  }
214  marker_pub.publish(line_list);
215  }
216 
217  void publishAsMarker(const std::string& frame_id, const costmap_converter::ObstacleArrayMsg& obstacles, ros::Publisher& marker_pub)
218  {
219  visualization_msgs::Marker line_list;
220  line_list.header.frame_id = frame_id;
221  line_list.header.stamp = ros::Time::now();
222  line_list.ns = "Polygons";
223  line_list.action = visualization_msgs::Marker::ADD;
224  line_list.pose.orientation.w = 1.0;
225 
226  line_list.id = 0;
227  line_list.type = visualization_msgs::Marker::LINE_LIST;
228 
229  line_list.scale.x = 0.1;
230  line_list.color.g = 1.0;
231  line_list.color.a = 1.0;
232 
233  for (const costmap_converter::ObstacleMsg& obstacle : obstacles.obstacles)
234  {
235  for (int j=0; j< (int)obstacle.polygon.points.size()-1; ++j)
236  {
237  geometry_msgs::Point line_start;
238  line_start.x = obstacle.polygon.points[j].x;
239  line_start.y = obstacle.polygon.points[j].y;
240  line_list.points.push_back(line_start);
241  geometry_msgs::Point line_end;
242  line_end.x = obstacle.polygon.points[j+1].x;
243  line_end.y = obstacle.polygon.points[j+1].y;
244  line_list.points.push_back(line_end);
245  }
246  // close loop for current polygon
247  if (!obstacle.polygon.points.empty() && obstacle.polygon.points.size() != 2 )
248  {
249  geometry_msgs::Point line_start;
250  line_start.x = obstacle.polygon.points.back().x;
251  line_start.y = obstacle.polygon.points.back().y;
252  line_list.points.push_back(line_start);
253  if (line_list.points.size() % 2 != 0)
254  {
255  geometry_msgs::Point line_end;
256  line_end.x = obstacle.polygon.points.front().x;
257  line_end.y = obstacle.polygon.points.front().y;
258  line_list.points.push_back(line_end);
259  }
260  }
261 
262 
263  }
264  marker_pub.publish(line_list);
265  }
266 
267 private:
270 
276 
277  std::string frame_id_;
279 
281 
282 };
283 
284 
285 int main(int argc, char** argv)
286 {
287  ros::init(argc, argv, "costmap_converter");
288 
289  CostmapStandaloneConversion convert_process;
290 
291  ros::spin();
292 
293  return 0;
294 }
295 
296 
void setCost(unsigned int mx, unsigned int my, unsigned char cost)
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y)
void publish(const boost::shared_ptr< M > &message) const
void publishAsMarker(const std::string &frame_id, const costmap_converter::ObstacleArrayMsg &obstacles, ros::Publisher &marker_pub)
#define ROS_INFO_ONCE(...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int main(int argc, char **argv)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
boost::shared_ptr< costmap_converter::BaseCostmapToPolygons > converter_
TFSIMD_FORCE_INLINE const tfScalar & y() const
ROSCPP_DECL void spin(Spinner &spinner)
void costmapCallback(const nav_msgs::OccupancyGridConstPtr &msg)
#define ROS_INFO(...)
void publishAsMarker(const std::string &frame_id, const std::vector< geometry_msgs::PolygonStamped > &polygonStamped, ros::Publisher &marker_pub)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
pluginlib::ClassLoader< costmap_converter::BaseCostmapToPolygons > converter_loader_
TFSIMD_FORCE_INLINE const tfScalar & x() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
unsigned int getSizeInCellsY() const
unsigned int getSizeInCellsX() const
#define ROS_INFO_STREAM(args)
static Time now()
ROSCPP_DECL void shutdown()
virtual void updateOrigin(double new_origin_x, double new_origin_y)
void indexToCells(unsigned int index, unsigned int &mx, unsigned int &my) const
void costmapUpdateCallback(const map_msgs::OccupancyGridUpdateConstPtr &update)
double getResolution() const
#define ROS_ERROR(...)


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Fri Jun 7 2019 21:48:43