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;
75  x0_ = costmap_->getSizeInCellsX();
76  y0_ = costmap_->getSizeInCellsY();
77 }
78 
79 Costmap2DPublisher::~Costmap2DPublisher()
80 {
81 }
82 
83 void Costmap2DPublisher::onNewSubscription(const ros::SingleSubscriberPublisher& pub)
84 {
85  prepareGrid();
86  pub.publish(grid_);
87 }
88 
89 // prepare grid_ message for publication.
90 void Costmap2DPublisher::prepareGrid()
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;
108  saved_origin_x_ = costmap_->getOriginX();
109  saved_origin_y_ = costmap_->getOriginY();
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 
120 void Costmap2DPublisher::publishCostmap()
121 {
122  if (costmap_pub_.getNumSubscribers() == 0)
123  {
124  // No subscribers, so why do any work?
125  return;
126  }
127 
128  boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
129  float resolution = costmap_->getResolution();
130 
131  if (always_send_full_costmap_ || grid_.info.resolution != resolution ||
132  grid_.info.width != costmap_->getSizeInCellsX() ||
133  grid_.info.height != costmap_->getSizeInCellsY() ||
134  saved_origin_x_ != costmap_->getOriginX() ||
135  saved_origin_y_ != costmap_->getOriginY())
136  {
137  prepareGrid();
138  costmap_pub_.publish(grid_);
139  }
140  else if (x0_ < xn_)
141  {
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  }
161  costmap_update_pub_.publish(update);
162  }
163 
164  xn_ = yn_ = 0;
165  x0_ = costmap_->getSizeInCellsX();
166  y0_ = costmap_->getSizeInCellsY();
167 }
168 
169 } // end namespace costmap_2d
ros::SingleSubscriberPublisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
costmap_2d::Costmap2DPublisher::Costmap2DPublisher
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.
Definition: costmap_2d_publisher.cpp:83
ros::SingleSubscriberPublisher
update
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
costmap_2d::Costmap2DPublisher::cost_translation_table_
static char * cost_translation_table_
Translate from 0-255 values in costmap to -1 to 100 values in message.
Definition: costmap_2d_publisher.h:141
costmap_2d_publisher.h
cost_values.h
costmap_2d
Definition: array_parser.h:37
ros::NodeHandle
ros::Time::now
static Time now()


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Wed Jan 18 2023 03:49:32