static_layer.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2008, 2013, Willow Garage, Inc.
6  * Copyright (c) 2015, Fetch Robotics, Inc.
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 Willow Garage, Inc. 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: Eitan Marder-Eppstein
37  * David V. Lu!!
38  *********************************************************************/
42 
44 
47 using costmap_2d::FREE_SPACE;
48 
49 namespace costmap_2d
50 {
51 
52 StaticLayer::StaticLayer() : dsrv_(NULL) {}
53 
55 {
56  if (dsrv_)
57  delete dsrv_;
58 }
59 
61 {
62  ros::NodeHandle nh("~/" + name_), g_nh;
63  current_ = true;
64 
66 
67  std::string map_topic;
68  nh.param("map_topic", map_topic, std::string("map"));
69  nh.param("first_map_only", first_map_only_, false);
70  nh.param("subscribe_to_updates", subscribe_to_updates_, false);
71 
72  nh.param("track_unknown_space", track_unknown_space_, true);
73  nh.param("use_maximum", use_maximum_, false);
74 
75  int temp_lethal_threshold, temp_unknown_cost_value;
76  nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
77  nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
78  nh.param("trinary_costmap", trinary_costmap_, true);
79 
80  lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
81  unknown_cost_value_ = temp_unknown_cost_value;
82 
83  // Only resubscribe if topic has changed
84  if (map_sub_.getTopic() != ros::names::resolve(map_topic))
85  {
86  // we'll subscribe to the latched topic that the map server uses
87  ROS_INFO("Requesting the map...");
88  map_sub_ = g_nh.subscribe(map_topic, 1, &StaticLayer::incomingMap, this);
89  map_received_ = false;
90  has_updated_data_ = false;
91 
92  ros::Rate r(10);
93  while (!map_received_ && g_nh.ok())
94  {
95  ros::spinOnce();
96  r.sleep();
97  }
98 
99  ROS_INFO("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
100 
102  {
103  ROS_INFO("Subscribing to updates");
104  map_update_sub_ = g_nh.subscribe(map_topic + "_updates", 10, &StaticLayer::incomingUpdate, this);
105 
106  }
107  }
108  else
109  {
110  has_updated_data_ = true;
111  }
112 
113  if (dsrv_)
114  {
115  delete dsrv_;
116  }
117 
118  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
119  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
120  &StaticLayer::reconfigureCB, this, _1, _2);
121  dsrv_->setCallback(cb);
122 }
123 
124 void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
125 {
126  if (config.enabled != enabled_)
127  {
128  enabled_ = config.enabled;
129  has_updated_data_ = true;
130  x_ = y_ = 0;
131  width_ = size_x_;
132  height_ = size_y_;
133  }
134 }
135 
137 {
138  // If we are using rolling costmap, the static map size is
139  // unrelated to the size of the layered costmap
140  if (!layered_costmap_->isRolling())
141  {
143  resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
144  master->getOriginX(), master->getOriginY());
145  }
146 }
147 
148 unsigned char StaticLayer::interpretValue(unsigned char value)
149 {
150  // check if the static value is above the unknown or lethal thresholds
152  return NO_INFORMATION;
153  else if (!track_unknown_space_ && value == unknown_cost_value_)
154  return FREE_SPACE;
155  else if (value >= lethal_threshold_)
156  return LETHAL_OBSTACLE;
157  else if (trinary_costmap_)
158  return FREE_SPACE;
159 
160  double scale = (double) value / lethal_threshold_;
161  return scale * LETHAL_OBSTACLE;
162 }
163 
164 void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
165 {
166  unsigned int size_x = new_map->info.width, size_y = new_map->info.height;
167 
168  ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);
169 
170  // resize costmap if size, resolution or origin do not match
172  if (!layered_costmap_->isRolling() &&
173  (master->getSizeInCellsX() != size_x ||
174  master->getSizeInCellsY() != size_y ||
175  master->getResolution() != new_map->info.resolution ||
176  master->getOriginX() != new_map->info.origin.position.x ||
177  master->getOriginY() != new_map->info.origin.position.y))
178  {
179  // Update the size of the layered costmap (and all layers, including this one)
180  ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
181  layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
182  new_map->info.origin.position.y,
183  true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
184  }
185  else if (size_x_ != size_x || size_y_ != size_y ||
186  resolution_ != new_map->info.resolution ||
187  origin_x_ != new_map->info.origin.position.x ||
188  origin_y_ != new_map->info.origin.position.y)
189  {
190  // only update the size of the costmap stored locally in this layer
191  ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
192  resizeMap(size_x, size_y, new_map->info.resolution,
193  new_map->info.origin.position.x, new_map->info.origin.position.y);
194  }
195 
196  unsigned int index = 0;
197 
198  // initialize the costmap with static data
199  for (unsigned int i = 0; i < size_y; ++i)
200  {
201  for (unsigned int j = 0; j < size_x; ++j)
202  {
203  unsigned char value = new_map->data[index];
204  costmap_[index] = interpretValue(value);
205  ++index;
206  }
207  }
208  map_frame_ = new_map->header.frame_id;
209 
210  // we have a new map, update full size of map
211  x_ = y_ = 0;
212  width_ = size_x_;
213  height_ = size_y_;
214  map_received_ = true;
215  has_updated_data_ = true;
216 
217  // shutdown the map subscrber if firt_map_only_ flag is on
218  if (first_map_only_)
219  {
220  ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
221  map_sub_.shutdown();
222  }
223 }
224 
225 void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update)
226 {
227  unsigned int di = 0;
228  for (unsigned int y = 0; y < update->height ; y++)
229  {
230  unsigned int index_base = (update->y + y) * size_x_;
231  for (unsigned int x = 0; x < update->width ; x++)
232  {
233  unsigned int index = index_base + x + update->x;
234  costmap_[index] = interpretValue(update->data[di++]);
235  }
236  }
237  x_ = update->x;
238  y_ = update->y;
239  width_ = update->width;
240  height_ = update->height;
241  has_updated_data_ = true;
242 }
243 
245 {
246  onInitialize();
247 }
248 
250 {
251  map_sub_.shutdown();
254 }
255 
257 {
258  if (first_map_only_)
259  {
260  has_updated_data_ = true;
261  }
262  else
263  {
264  onInitialize();
265  }
266 }
267 
268 void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
269  double* max_x, double* max_y)
270 {
271 
272  if( !layered_costmap_->isRolling() ){
274  return;
275  }
276 
277  useExtraBounds(min_x, min_y, max_x, max_y);
278 
279  double wx, wy;
280 
281  mapToWorld(x_, y_, wx, wy);
282  *min_x = std::min(wx, *min_x);
283  *min_y = std::min(wy, *min_y);
284 
285  mapToWorld(x_ + width_, y_ + height_, wx, wy);
286  *max_x = std::max(wx, *max_x);
287  *max_y = std::max(wy, *max_y);
288 
289  has_updated_data_ = false;
290 }
291 
292 void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
293 {
294  if (!map_received_)
295  return;
296 
297  if (!enabled_)
298  return;
299 
300  if (!layered_costmap_->isRolling())
301  {
302  // if not rolling, the layered costmap (master_grid) has same coordinates as this layer
303  if (!use_maximum_)
304  updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
305  else
306  updateWithMax(master_grid, min_i, min_j, max_i, max_j);
307  }
308  else
309  {
310  // If rolling window, the master_grid is unlikely to have same coordinates as this layer
311  unsigned int mx, my;
312  double wx, wy;
313  // Might even be in a different frame
314  tf::StampedTransform transform;
315  try
316  {
318  }
319  catch (tf::TransformException ex)
320  {
321  ROS_ERROR("%s", ex.what());
322  return;
323  }
324  // Copy map data given proper transformations
325  for (unsigned int i = min_i; i < max_i; ++i)
326  {
327  for (unsigned int j = min_j; j < max_j; ++j)
328  {
329  // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
330  layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
331  // Transform from global_frame_ to map_frame_
332  tf::Point p(wx, wy, 0);
333  p = transform(p);
334  // Set master_grid with cell from map
335  if (worldToMap(p.x(), p.y(), mx, my))
336  {
337  if (!use_maximum_)
338  master_grid.setCost(i, j, getCost(mx, my));
339  else
340  master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
341  }
342  }
343  }
344  }
345 }
346 
347 } // namespace costmap_2d
void setCost(unsigned int mx, unsigned int my, unsigned char cost)
Set the cost of a cell in the costmap.
Definition: costmap_2d.cpp:197
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y)
This is called by the LayeredCostmap to poll this plugin as to how much of the costmap it needs to up...
LayeredCostmap * layered_costmap_
Definition: layer.h:122
virtual void activate()
Restart publishers if they&#39;ve been stopped.
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y)
Definition: costmap_2d.cpp:72
virtual void onInitialize()
This is called at the end of initialize(). Override to implement subclass-specific initialization...
virtual void updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
Actually update the underlying costmap, only within the bounds calculated during UpdateBounds().
void mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy) const
Convert from map coordinates to world coordinates.
Definition: costmap_2d.cpp:202
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, bool size_locked=false)
unsigned int size_y_
Definition: costmap_2d.h:422
virtual void deactivate()
Stop publishers.
std::string getGlobalFrameID() const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
dynamic_reconfigure::Server< costmap_2d::GenericPluginConfig > * dsrv_
Definition: static_layer.h:96
double getOriginX() const
Accessor for the x origin of the costmap.
Definition: costmap_2d.cpp:446
unsigned char lethal_threshold_
Definition: static_layer.h:94
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
unsigned int size_x_
Definition: costmap_2d.h:421
TFSIMD_FORCE_INLINE const tfScalar & y() const
static const unsigned char FREE_SPACE
Definition: cost_values.h:45
bool first_map_only_
Store the first static map and reuse it on reinitializing.
Definition: static_layer.h:90
void useExtraBounds(double *min_x, double *min_y, double *max_x, double *max_y)
std::string name_
Definition: layer.h:125
bool subscribe_to_updates_
frame that map is located in
Definition: static_layer.h:84
tf::TransformListener * tf_
Definition: layer.h:126
TFSIMD_FORCE_INLINE const tfScalar & x() const
unsigned char getCost(unsigned int mx, unsigned int my) const
Get the cost of a cell in the costmap.
Definition: costmap_2d.cpp:192
std::string global_frame_
The global frame for the costmap.
Definition: static_layer.h:82
#define ROS_INFO(...)
void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
std::string getTopic() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
void updateWithTrueOverwrite(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
TFSIMD_FORCE_INLINE const tfScalar & x() const
double getOriginY() const
Accessor for the y origin of the costmap.
Definition: costmap_2d.cpp:451
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
Definition: costmap_2d.cpp:431
void incomingMap(const nav_msgs::OccupancyGridConstPtr &new_map)
Callback to update the costmap&#39;s map from the map_server.
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
bool sleep()
unsigned char interpretValue(unsigned char value)
bool enabled_
Currently this var is managed by subclasses. TODO: make this managed by this class and/or container c...
Definition: layer.h:124
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
Definition: costmap_2d.cpp:426
void incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr &update)
static const unsigned char LETHAL_OBSTACLE
Definition: cost_values.h:43
static const unsigned char NO_INFORMATION
Definition: cost_values.h:42
ros::Subscriber map_sub_
Definition: static_layer.h:92
bool ok() const
void updateWithMax(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.h:60
double getResolution() const
Accessor for the resolution of the costmap.
Definition: costmap_2d.cpp:456
unsigned char * costmap_
Definition: costmap_2d.h:426
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
Convert from world coordinates to map coordinates.
Definition: costmap_2d.cpp:208
ros::Subscriber map_update_sub_
Definition: static_layer.h:92
unsigned char unknown_cost_value_
Definition: static_layer.h:94
virtual void matchSize()
Implement this to make this layer match the size of the parent costmap.
#define ROS_DEBUG(...)


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:42