nav_grid_publisher.h
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 #ifndef NAV_GRID_PUB_SUB_NAV_GRID_PUBLISHER_H
36 #define NAV_GRID_PUB_SUB_NAV_GRID_PUBLISHER_H
37 
42 #include <ros/ros.h>
43 #include <nav_grid/nav_grid.h>
45 #include <nav_core2/bounds.h>
46 #include <geometry_msgs/PolygonStamped.h>
47 #include <limits>
48 #include <string>
49 #include <vector>
50 
51 namespace nav_grid_pub_sub
52 {
53 // Helper function: needs temp double variables for conversion, then made into floats for Point32
54 inline geometry_msgs::Point32 makePoint(const nav_grid::NavGridInfo& info, int x, int y)
55 {
56  double point_x = 0.0;
57  double point_y = 0.0;
58  gridToWorld(info, x, y, point_x, point_y);
59  geometry_msgs::Point32 point;
60  point.x = point_x;
61  point.y = point_y;
62  return point;
63 }
64 
105 template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
107 {
108 public:
110 
128  const std::string& nav_grid_topic = "grid",
129  const std::string& occupancy_grid_topic = "costmap",
130  const std::string& update_area_topic = "update_area",
131  bool publish_updates = true,
132  ros::Duration full_publish_cycle = ros::Duration(0),
133  ros::Duration update_publish_cycle = ros::Duration(0))
134  {
135  full_publish_cycle_ = full_publish_cycle;
136  update_publish_cycle_ = update_publish_cycle;
139  publish_updates_ = publish_updates;
140 
141  createPublishers<NavGridOfX, NavGridOfXUpdate>(
142  nh, nav_grid_topic, boost::bind(&GenericGridPublisher::onNewSubscriptionNav, this, _1),
143  nav_pub_, nav_update_pub_, publish_updates);
144 
145  createPublishers<nav_msgs::OccupancyGrid, map_msgs::OccupancyGridUpdate>(
146  nh, occupancy_grid_topic, boost::bind(&GenericGridPublisher::onNewSubscriptionOcc, this, _1),
147  occ_pub_, occ_update_pub_, publish_updates);
148 
149  if (update_area_topic.length() > 0)
150  {
151  update_area_pub_ = nh.advertise<geometry_msgs::PolygonStamped>(update_area_topic, 1);
152  }
153  }
154 
158  void publish()
159  {
160  if (!shouldPublishFull()) return;
163  publishNav();
164  publishOcc();
165  }
166 
174  void publish(const nav_core2::UIntBounds& bounds)
175  {
176  if (!publish_updates_)
177  {
178  // Don't publish an update, publish the full grid if enough time has passed
179  publish();
180  return;
181  }
182 
183  update_bounds_.merge(bounds);
184 
185  if (!shouldPublishUpdate()) return;
186 
187  const nav_grid::NavGridInfo& info = data_.getInfo();
190 
191  if (saved_info_ != info)
192  {
193  // If the info has changed, force publish the whole grid
194  saved_info_ = info;
195  publishNav();
196  publishOcc();
197  }
198  else if (!update_bounds_.isEmpty())
199  {
200  // If actual data was updated, publish the updates
203  }
204 
205  // Publish the update area (or an empty polygon message if there was no update)
207 
209  }
210 
211 
212 protected:
213  template<class FullGridType, class UpdateType, class Callback>
214  void createPublishers(ros::NodeHandle& nh, const std::string& topic, Callback new_subscription_callback,
215  ros::Publisher& full_grid_pub, ros::Publisher& update_pub, bool publish_updates)
216  {
217  if (topic.length() > 0)
218  {
219  full_grid_pub = nh.advertise<FullGridType>(topic, 1, new_subscription_callback);
220  if (publish_updates)
221  {
222  update_pub = nh.advertise<UpdateType>(topic + "_updates", 1);
223  }
224  }
225  }
226 
227  virtual nav_msgs::OccupancyGrid toOccupancyGrid(const ros::Time& timestamp) = 0;
228  virtual map_msgs::OccupancyGridUpdate toOccupancyGridUpdate(const nav_core2::UIntBounds& bounds,
229  const ros::Time& timestamp) = 0;
230 
231  bool shouldPublishHelper(const ros::Time& last_publish, const ros::Duration& cycle) const
232  {
233  double cycle_secs = cycle.toSec();
234  if (cycle_secs < 0.0)
235  {
236  return false;
237  }
238  else if (cycle_secs == 0.0)
239  {
240  return true;
241  }
242  else
243  {
244  return last_publish + cycle < ros::Time::now();
245  }
246  }
247 
248  inline bool shouldPublishFull() const
249  {
251  }
252 
253  inline bool shouldPublishUpdate() const
254  {
256  }
257 
259  {
260  if (update_area_pub_.getNumSubscribers() == 0) return;
261 
262  geometry_msgs::PolygonStamped polygon;
263  polygon.header.frame_id = info.frame_id;
264  polygon.header.stamp = synced_time_stamp_;
265 
266  if (!bounds.isEmpty())
267  {
268  polygon.polygon.points.push_back(makePoint(info, bounds.getMinX(), bounds.getMinY()));
269  polygon.polygon.points.push_back(makePoint(info, bounds.getMaxX(), bounds.getMinY()));
270  polygon.polygon.points.push_back(makePoint(info, bounds.getMaxX(), bounds.getMaxY()));
271  polygon.polygon.points.push_back(makePoint(info, bounds.getMinX(), bounds.getMaxY()));
272  }
273  update_area_pub_.publish(polygon);
274  }
275 
277  {
279  }
280 
282  {
284  }
285 
286  void publishNav()
287  {
288  if (nav_pub_.getNumSubscribers() == 0) return;
290  }
291 
292  void publishOcc()
293  {
294  if (occ_pub_.getNumSubscribers() == 0) return;
296  }
297 
299  {
300  if (nav_update_pub_.getNumSubscribers() == 0) return;
302  }
303 
305  {
306  if (occ_update_pub_.getNumSubscribers() == 0) return;
308  }
309 
310  // Data
314 
315  // Track time
318 
319  // Track Update Bounds
321 
322  // Publishers
326 };
327 
335  : public GenericGridPublisher<unsigned char, nav_2d_msgs::NavGridOfChars, nav_2d_msgs::NavGridOfCharsUpdate>
336 {
337 public:
340 
341  void setCostInterpretation(const std::vector<unsigned char>& cost_interpretation_table)
342  {
343  cost_interpretation_table_ = cost_interpretation_table;
344  }
345 protected:
346  nav_msgs::OccupancyGrid toOccupancyGrid(const ros::Time& timestamp) override
347  {
348  return nav_grid_pub_sub::toOccupancyGrid(data_, timestamp, cost_interpretation_table_);
349  }
350 
351  map_msgs::OccupancyGridUpdate toOccupancyGridUpdate(const nav_core2::UIntBounds& bounds,
352  const ros::Time& timestamp) override
353  {
354  return nav_grid_pub_sub::toOccupancyGridUpdate(data_, bounds, timestamp, cost_interpretation_table_);
355  }
356  std::vector<unsigned char> cost_interpretation_table_ { OCC_GRID_PUBLISHING };
357 };
358 
363 template<typename NumericType>
365  : public GenericGridPublisher<NumericType, nav_2d_msgs::NavGridOfDoubles, nav_2d_msgs::NavGridOfDoublesUpdate>
366 {
367 public:
370 
371  void setIgnoreValue(NumericType ignore_value) { ignore_value_ = ignore_value; }
372 
373 protected:
374  nav_msgs::OccupancyGrid toOccupancyGrid(const ros::Time& timestamp) override
375  {
376  nav_grid_pub_sub::getExtremeValues(this->data_, ignore_value_, min_val_, max_val_);
377  return nav_grid_pub_sub::toOccupancyGrid(this->data_, min_val_, max_val_, ignore_value_, timestamp);
378  }
379 
380  map_msgs::OccupancyGridUpdate toOccupancyGridUpdate(const nav_core2::UIntBounds& bounds,
381  const ros::Time& timestamp) override
382  {
383  return nav_grid_pub_sub::toOccupancyGridUpdate(this->data_, bounds, min_val_, max_val_, ignore_value_, timestamp);
384  }
385 
386  NumericType ignore_value_ { std::numeric_limits<NumericType>::max() };
387  NumericType min_val_, max_val_;
388 };
389 
390 
391 } // namespace nav_grid_pub_sub
392 
393 #endif // NAV_GRID_PUB_SUB_NAV_GRID_PUBLISHER_H
ROSMsgType toUpdate(const nav_grid::NavGrid< NumericType > &grid, const nav_core2::UIntBounds &bounds, const ros::Time &stamp)
Generic conversion from a portion of a grid of arbitrary type to an update message of arbitrary type...
map_msgs::OccupancyGridUpdate toOccupancyGridUpdate(const nav_core2::UIntBounds &bounds, const ros::Time &timestamp) override
unsigned int getMaxX() const
void setCostInterpretation(const std::vector< unsigned char > &cost_interpretation_table)
void publish(const boost::shared_ptr< M > &message) const
static std::vector< unsigned char > OCC_GRID_PUBLISHING
Scale [0, 255] down to [0, 100] (except for a few special values)
map_msgs::OccupancyGridUpdate toOccupancyGridUpdate(const nav_core2::UIntBounds &bounds, const ros::Time &timestamp) override
unsigned int getMinX() const
void publishUpdateArea(const nav_grid::NavGridInfo &info, const nav_core2::UIntBounds &bounds)
geometry_msgs::Point32 makePoint(const nav_grid::NavGridInfo &info, int x, int y)
unsigned int getMinY() const
void publish(const nav_core2::UIntBounds &bounds)
Publish the full grid or updates, as dictated by parameters passed to init.
virtual map_msgs::OccupancyGridUpdate toOccupancyGridUpdate(const nav_core2::UIntBounds &bounds, const ros::Time &timestamp)=0
void publish(const boost::shared_ptr< M const > &message) const
An interface for publishing NavGridOfX/OccupancyGrid msgs and their updates periodically.
void createPublishers(ros::NodeHandle &nh, const std::string &topic, Callback new_subscription_callback, ros::Publisher &full_grid_pub, ros::Publisher &update_pub, bool publish_updates)
nav_msgs::OccupancyGrid toOccupancyGrid(const ros::Time &timestamp) override
void gridToWorld(const NavGridInfo &info, int mx, int my, double &wx, double &wy)
An interface for publishing NavGridOfChars/OccupancyGrid msgs and their updates periodically.
void init(ros::NodeHandle &nh, const std::string &nav_grid_topic="grid", const std::string &occupancy_grid_topic="costmap", const std::string &update_area_topic="update_area", bool publish_updates=true, ros::Duration full_publish_cycle=ros::Duration(0), ros::Duration update_publish_cycle=ros::Duration(0))
Initialize method for determining what gets published when.
void publish()
Publish the full grid if the full_publish_cycle allows.
GenericGridPublisher(nav_grid::NavGrid< NumericType > &data)
void publishNavUpdate(const nav_core2::UIntBounds &bounds)
void setIgnoreValue(NumericType ignore_value)
void onNewSubscriptionOcc(const ros::SingleSubscriberPublisher &pub)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
nav_msgs::OccupancyGrid toOccupancyGrid(const ros::Time &timestamp) override
void merge(const GenericBounds< unsigned int > &other)
uint32_t getNumSubscribers() const
bool shouldPublishHelper(const ros::Time &last_publish, const ros::Duration &cycle) const
NavGridInfo getInfo() const
void publishOccUpdate(const nav_core2::UIntBounds &bounds)
virtual nav_msgs::OccupancyGrid toOccupancyGrid(const ros::Time &timestamp)=0
void onNewSubscriptionNav(const ros::SingleSubscriberPublisher &pub)
static Time now()
nav_grid::NavGrid< NumericType > & data_
map_msgs::OccupancyGridUpdate toOccupancyGridUpdate(const nav_grid::NavGrid< unsigned char > &grid, const nav_core2::UIntBounds &bounds, const ros::Time &stamp=ros::Time(0), const std::vector< unsigned char > cost_interpretation_table=std::vector< unsigned char >())
NavGrid<unsigned char> to OccupancyGridUpdate using cost_interpretation_table.
unsigned int getMaxY() const
ROSMsgType toMsg(const nav_grid::NavGrid< NumericType > &grid, const ros::Time &stamp)
Utilities for converting NavGrid objects to NavGridOfX messages and updates.
void getExtremeValues(const nav_grid::NavGrid< NumericType > &grid, const NumericType unknown_value, NumericType &min_val, NumericType &max_val)
Retrieve the minimum and maximum values from a grid, ignoring the unknown_value.
nav_msgs::OccupancyGrid toOccupancyGrid(const nav_grid::NavGrid< unsigned char > &grid, const ros::Time &stamp=ros::Time(0), const std::vector< unsigned char > cost_interpretation_table=std::vector< unsigned char >())
NavGrid<unsigned char> to OccupancyGrid using cost_interpretation_table.
An interface for publishing NavGridOfDoubles/OccupancyGrid msgs and their updates periodically...


nav_grid_pub_sub
Author(s):
autogenerated on Sun Jan 10 2021 04:08:50