costmap_2d_publisher.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  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author: Eitan Marder-Eppstein
36  * David V. Lu!!
37  *********************************************************************/
38 #include <boost/bind.hpp>
40 #include <costmap_2d/cost_values.h>
41 
42 namespace costmap_2d
43 {
44 
46 
47 Costmap2DPublisher::Costmap2DPublisher(ros::NodeHandle * ros_node, Costmap2D* costmap, std::string global_frame,
48  std::string topic_name, bool always_send_full_costmap) :
49  node(ros_node), costmap_(costmap), global_frame_(global_frame), active_(false),
50  always_send_full_costmap_(always_send_full_costmap)
51 {
52  costmap_pub_ = ros_node->advertise<nav_msgs::OccupancyGrid>(topic_name, 1,
53  boost::bind(&Costmap2DPublisher::onNewSubscription, this, _1));
54  costmap_update_pub_ = ros_node->advertise<map_msgs::OccupancyGridUpdate>(topic_name + "_updates", 1);
55 
56  if (cost_translation_table_ == NULL)
57  {
58  cost_translation_table_ = new char[256];
59 
60  // special values:
61  cost_translation_table_[0] = 0; // NO obstacle
62  cost_translation_table_[253] = 99; // INSCRIBED obstacle
63  cost_translation_table_[254] = 100; // LETHAL obstacle
64  cost_translation_table_[255] = -1; // UNKNOWN
65 
66  // regular cost values scale the range 1 to 252 (inclusive) to fit
67  // into 1 to 98 (inclusive).
68  for (int i = 1; i < 253; i++)
69  {
70  cost_translation_table_[ i ] = char(1 + (97 * (i - 1)) / 251);
71  }
72  }
73 
74  xn_ = yn_ = 0;
77 }
78 
80 {
81 }
82 
84 {
85  prepareGrid();
86  pub.publish(grid_);
87 }
88 
89 // prepare grid_ message for publication.
91 {
92  boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
93  double resolution = costmap_->getResolution();
94 
95  grid_.header.frame_id = global_frame_;
96  grid_.header.stamp = ros::Time::now();
97  grid_.info.resolution = resolution;
98 
99  grid_.info.width = costmap_->getSizeInCellsX();
100  grid_.info.height = costmap_->getSizeInCellsY();
101 
102  double wx, wy;
103  costmap_->mapToWorld(0, 0, wx, wy);
104  grid_.info.origin.position.x = wx - resolution / 2;
105  grid_.info.origin.position.y = wy - resolution / 2;
106  grid_.info.origin.position.z = 0.0;
107  grid_.info.origin.orientation.w = 1.0;
110 
111  grid_.data.resize(grid_.info.width * grid_.info.height);
112 
113  unsigned char* data = costmap_->getCharMap();
114  for (unsigned int i = 0; i < grid_.data.size(); i++)
115  {
116  grid_.data[i] = cost_translation_table_[ data[ i ]];
117  }
118 }
119 
121 {
122  if (costmap_pub_.getNumSubscribers() == 0)
123  {
124  // No subscribers, so why do any work?
125  return;
126  }
127 
128  float resolution = costmap_->getResolution();
129 
130  if (always_send_full_costmap_ || grid_.info.resolution != resolution ||
131  grid_.info.width != costmap_->getSizeInCellsX() ||
132  grid_.info.height != costmap_->getSizeInCellsY() ||
135  {
136  prepareGrid();
138  }
139  else if (x0_ < xn_)
140  {
141  boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
142  // Publish Just an Update
143  map_msgs::OccupancyGridUpdate update;
144  update.header.stamp = ros::Time::now();
145  update.header.frame_id = global_frame_;
146  update.x = x0_;
147  update.y = y0_;
148  update.width = xn_ - x0_;
149  update.height = yn_ - y0_;
150  update.data.resize(update.width * update.height);
151 
152  unsigned int i = 0;
153  for (unsigned int y = y0_; y < yn_; y++)
154  {
155  for (unsigned int x = x0_; x < xn_; x++)
156  {
157  unsigned char cost = costmap_->getCost(x, y);
158  update.data[i++] = cost_translation_table_[ cost ];
159  }
160  }
162  }
163 
164  xn_ = yn_ = 0;
167 }
168 
169 } // end namespace costmap_2d
mutex_t * getMutex()
Definition: costmap_2d.h:295
void publish(const boost::shared_ptr< M > &message) const
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 publish(const boost::shared_ptr< M const > &message) const
double getOriginX() const
Accessor for the x origin of the costmap.
Definition: costmap_2d.cpp:446
TFSIMD_FORCE_INLINE const tfScalar & y() const
unsigned char * getCharMap() const
Will return a pointer to the underlying unsigned char array used as the costmap.
Definition: costmap_2d.cpp:187
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
unsigned char getCost(unsigned int mx, unsigned int my) const
Get the cost of a cell in the costmap.
Definition: costmap_2d.cpp:192
void onNewSubscription(const ros::SingleSubscriberPublisher &pub)
Publish the latest full costmap to the new subscriber.
void prepareGrid()
Prepare grid_ message for publication.
TFSIMD_FORCE_INLINE const tfScalar & x() const
double getOriginY() const
Accessor for the y origin of the costmap.
Definition: costmap_2d.cpp:451
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
Definition: costmap_2d.cpp:431
static char * cost_translation_table_
Translate from 0-255 values in costmap to -1 to 100 values in message.
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
Definition: costmap_2d.cpp:426
uint32_t getNumSubscribers() const
static Time now()
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
Costmap2DPublisher(ros::NodeHandle *ros_node, Costmap2D *costmap, std::string global_frame, std::string topic_name, bool always_send_full_costmap=false)
Constructor for the Costmap2DPublisher.
void publishCostmap()
Publishes the visualization data over ROS.


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:44:17