converter.cpp
Go to the documentation of this file.
1 
4 /*****************************************************************************
5 ** Includes
6 *****************************************************************************/
7 
8 #include <boost/thread/lock_guard.hpp>
9 #include <cmath>
11 #include <cost_map_msgs/CostMap.h>
12 #include <ecl/console.hpp>
13 #include <limits>
14 #include <map>
15 #include <sensor_msgs/Image.h>
17 #include <std_msgs/UInt8MultiArray.h>
18 #include <stdexcept>
19 #include <string>
20 #include "../../include/cost_map_ros/converter.hpp"
21 
23 
24 /*****************************************************************************
25 ** Namespaces
26 *****************************************************************************/
27 
28 namespace cost_map {
29 
30 /*****************************************************************************
31 ** ROS Images
32 *****************************************************************************/
33 
34 //
35 // almost a carbon copy of grid_map::addLayerFromImage
36 //
37 bool addLayerFromROSImage(const sensor_msgs::Image& image,
38  const std::string& layer_name,
40  )
41 {
42  cv_bridge::CvImagePtr cvPtrAlpha, cvPtrMono;
43 
44  // If alpha channel exist, read it.
45  if (image.encoding == sensor_msgs::image_encodings::BGRA8
46  || image.encoding == sensor_msgs::image_encodings::BGRA16) {
47  try {
48  cvPtrAlpha = cv_bridge::toCvCopy(image, image.encoding);
49  } catch (cv_bridge::Exception& e) {
50  std::cout << ecl::red << "[ERROR] cv_bridge exception: " << e.what() << ecl::reset << std::endl;
51  return false;
52  }
53  }
54 
55  unsigned int depth; // Convert color image to grayscale.
56  try {
57  if (image.encoding == sensor_msgs::image_encodings::BGRA8
58  || image.encoding == sensor_msgs::image_encodings::BGR8
59  || image.encoding == sensor_msgs::image_encodings::MONO8) {
60  cvPtrMono = cv_bridge::toCvCopy(image,
62  depth = std::pow(2, 8);
63  // std::cout << "Color image converted to mono8" << std::endl;
64  } else if (image.encoding == sensor_msgs::image_encodings::BGRA16
65  || image.encoding == sensor_msgs::image_encodings::BGR16
66  || image.encoding == sensor_msgs::image_encodings::MONO16) {
67  cvPtrMono = cv_bridge::toCvCopy(image,
69  depth = std::pow(2, 16);
70  // std::cout << "Color image converted to mono16" << std::endl;
71  } else {
72  std::cout << ecl::red << "[ERROR] expected BGR, BGRA, or MONO image encoding." << ecl::reset << std::endl;
73  return false;
74  }
75  } catch (cv_bridge::Exception& e) {
76  std::cout << ecl::red << "[ERROR] cv_bridge exception: " << e.what() << ecl::reset << std::endl;
77  return false;
78  }
79 
80  cost_map.add(layer_name);
81 
82  if (cost_map.getSize()(0) != image.height
83  || cost_map.getSize()(1) != image.width) {
84  std::cout << ecl::red << "[ERROR] Image size does not correspond to grid map size!" << ecl::reset << std::endl;
85  return false;
86  }
87 
88  for (CostMapIterator iterator(cost_map); !iterator.isPastEnd(); ++iterator) {
89  // skip transparent values (they will typically be set to the default (cv_map::NO_INFORMATION/255)
90  if (image.encoding == sensor_msgs::image_encodings::BGRA8) {
91  const auto& cvAlpha = cvPtrAlpha->image.at<cv::Vec4b>((*iterator)(0),
92  (*iterator)(1));
93  unsigned int alpha = cvAlpha[3];
94  if (cvAlpha[3] < depth / 2)
95  continue;
96  }
97  if (image.encoding == sensor_msgs::image_encodings::BGRA16) {
98  const auto& cvAlpha = cvPtrAlpha->image.at<cv::Vec<uchar, 8>>(
99  (*iterator)(0), (*iterator)(1));
100  int alpha = (cvAlpha[6] << 8) + cvAlpha[7];
101  if (alpha < depth / 2)
102  continue;
103  }
104 
105  // Compute non-transparent values.
106  unsigned int grayValue;
107  if (depth == std::pow(2, 8)) {
108  uchar cvGrayscale = cvPtrMono->image.at<uchar>((*iterator)(0),
109  (*iterator)(1));
110  grayValue = cvGrayscale;
111  }
112  if (depth == std::pow(2, 16)) {
113  const auto& cvGrayscale = cvPtrMono->image.at<cv::Vec2b>((*iterator)(0),
114  (*iterator)(1));
115  grayValue = (cvGrayscale[0] << 8) + cvGrayscale[1];
116  }
117  // RULE 1 : scale only from 0-254 (remember 255 is reserved for NO_INFORMATION)
118  // RULE 2 : invert the value as black on an image (grayscale: 0) typically represents an obstacle (cost: 254)
119  cost_map::DataType value = static_cast<cost_map::DataType>(254.0 * ( 1.0 - (static_cast<double>(grayValue) / static_cast<double>(depth))));
120  cost_map.at(layer_name, *iterator) = value;
121  }
122  return true;
123 }
124 
125 /*****************************************************************************
126 ** CostMap and GridMap
127 *****************************************************************************/
128 
129 
131 {
132  grid_map.setGeometry(cost_map.getLength(), cost_map.getResolution(), cost_map.getPosition());
133  grid_map.setFrameId(cost_map.getFrameId());
134  grid_map.setTimestamp(cost_map.getTimestamp());
135  for (const std::string& layer_name : cost_map.getLayers()) {
136  const cost_map::Matrix& cost_map_data = cost_map[layer_name];
137  grid_map::Matrix grid_map_data(cost_map_data.rows(), cost_map_data.cols());
138  const cost_map::DataType *cost_map_ptr = cost_map_data.data();
139  float *grid_map_ptr = grid_map_data.data();
140  for (unsigned int i = 0; i < cost_map_data.size(); ++i ) {
141  *grid_map_ptr = 100.0 * static_cast<double>(*cost_map_ptr) / static_cast<double>(cost_map::NO_INFORMATION);
142  ++cost_map_ptr; ++grid_map_ptr;
143  }
144  grid_map.add(layer_name, grid_map_data);
145  }
146 }
147 
148 /*****************************************************************************
149 ** Messages
150 *****************************************************************************/
151 //
152 // almost a carbon copy of grid_map::toMessage
153 //
154 void toMessage(const cost_map::CostMap& cost_map, cost_map_msgs::CostMap& message)
155 {
156  std::vector<std::string> layers = cost_map.getLayers();
157 
158  message.info.header.stamp.fromNSec(cost_map.getTimestamp());
159  message.info.header.frame_id = cost_map.getFrameId();
160  message.info.resolution = cost_map.getResolution();
161  message.info.length_x = cost_map.getLength().x();
162  message.info.length_y = cost_map.getLength().y();
163  message.info.pose.position.x = cost_map.getPosition().x();
164  message.info.pose.position.y = cost_map.getPosition().y();
165  message.info.pose.position.z = 0.0;
166  message.info.pose.orientation.x = 0.0;
167  message.info.pose.orientation.y = 0.0;
168  message.info.pose.orientation.z = 0.0;
169  message.info.pose.orientation.w = 1.0;
170 
171  message.layers = layers;
172  message.basic_layers = cost_map.getBasicLayers();
173 
174  message.data.clear();
175  for (const auto& layer : layers) {
176  const cost_map::Matrix& data = cost_map.get(layer);
177  std_msgs::UInt8MultiArray data_array;
178  grid_map::matrixEigenCopyToMultiArrayMessage(cost_map.get(layer), data_array);
179  message.data.push_back(data_array);
180  }
181 
182  message.outer_start_index = cost_map.getStartIndex()(0);
183  message.inner_start_index = cost_map.getStartIndex()(1);
184 }
185 
186 bool fromMessage(const cost_map_msgs::CostMap& message, cost_map::CostMap& cost_map)
187 {
188  cost_map.setTimestamp(message.info.header.stamp.toNSec());
189  cost_map.setFrameId(message.info.header.frame_id);
190  cost_map.setGeometry(Length(message.info.length_x, message.info.length_y), message.info.resolution,
191  Position(message.info.pose.position.x, message.info.pose.position.y));
192 
193  if (message.layers.size() != message.data.size()) {
194  // ROS_ERROR("Different number of layers and data in grid map message.");
195  return false;
196  }
197 
198  for (unsigned int i = 0; i < message.layers.size(); i++) {
199  Matrix data;
200  // this is not a template function
201  // grid_map::multiArrayMessageCopyToMatrixEigen(message.data[i], data); // TODO Could we use the data mapping (instead of copying) method here?
202  grid_map::multiArrayMessageCopyToMatrixEigen(message.data[i], data);
203  // TODO Check if size is good. size_ << getRows(message.data[0]), getCols(message.data[0]);
204  cost_map.add(message.layers[i], data);
205  }
206 
207  cost_map.setBasicLayers(message.basic_layers);
208  cost_map.setStartIndex(Index(message.outer_start_index, message.inner_start_index));
209  return true;
210 }
211 
212 /*****************************************************************************
213 ** CostMap2DROS and Occupancy Grids
214 *****************************************************************************/
215 
217  const std::string& layer_name,
219 
221  boost::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(ros_costmap.getCostmap()->getMutex()));
222  converter.initializeFromCostmap2D(ros_costmap, cost_map);
223  if ( !converter.addLayerFromCostmap2D(ros_costmap, layer_name, cost_map) ) {
224  return false;
225  }
226  return true;
227 }
228 
230  const cost_map::Length& geometry,
231  const std::string& layer_name,
233 {
235  boost::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(ros_costmap.getCostmap()->getMutex()));
236  if ( !converter.initializeFromCostmap2DAtRobotPose(ros_costmap, geometry, cost_map) ) {
237  return false;
238  }
239  if (!converter.addLayerFromCostmap2DAtRobotPose(ros_costmap, layer_name, cost_map) ) {
240  return false;
241  }
242  return true;
243 }
244 
245 void toOccupancyGrid(const cost_map::CostMap& cost_map, const std::string& layer, nav_msgs::OccupancyGrid& msg) {
246  msg.header.frame_id = cost_map.getFrameId();
247  msg.header.stamp.fromNSec(cost_map.getTimestamp());
248  msg.info.map_load_time = msg.header.stamp; // Same as header stamp as we do not load the map.
249  msg.info.resolution = cost_map.getResolution();
250  msg.info.width = cost_map.getSize()(0);
251  msg.info.height = cost_map.getSize()(1);
252 
253  // adjust for difference in center
254  Position positionOfOrigin = cost_map.getPosition() - 0.5 * cost_map.getLength().matrix();
255  msg.info.origin.position.x = positionOfOrigin.x();
256  msg.info.origin.position.y = positionOfOrigin.y();
257  msg.info.origin.position.z = 0.0;
258  msg.info.origin.orientation.x = 0.0;
259  msg.info.origin.orientation.y = 0.0;
260  msg.info.origin.orientation.z = 0.0;
261  msg.info.origin.orientation.w = 1.0;
262  msg.data.resize(msg.info.width * msg.info.height);
263 
264  size_t nCells = cost_map.getSize().prod();
265  msg.data.resize(nCells);
266 
267  // Occupancy probabilities are in the range [0,100]. Unknown is -1.
268  const float cellMin = 0;
269  const float cellMax = 98;
270  const float cellRange = cellMax - cellMin;
271 
272  const float data_minimum = 0;
273  const float data_maximum = 252;
274  for (CostMapIterator iterator(cost_map); !iterator.isPastEnd(); ++iterator)
275  {
276  float value;
277  if (cost_map.at(layer, *iterator) == cost_map::NO_INFORMATION) // 255
278  {
279  value = -1;
280  }
281  else if (cost_map.at(layer, *iterator) == cost_map::LETHAL_OBSTACLE) // 254
282  {
283  value = 100;
284  }
285  else if (cost_map.at(layer, *iterator) == cost_map::INSCRIBED_OBSTACLE) // 253
286  {
287  value = 99;
288  }
289  else
290  {
291  value = (cost_map.at(layer, *iterator) - data_minimum) / (data_maximum - data_minimum);
292  value = cellMin + std::min(std::max(0.0f, value), 1.0f) * cellRange;
293  }
294  unsigned int index = grid_map::getLinearIndexFromIndex(*iterator, cost_map.getSize(), false);
295  msg.data[nCells - index - 1] = value;
296  }
297 }
299  const std::string& service_name)
300 : ros_costmap(ros_costmap)
301 {
302  ros::NodeHandle private_nodehandle("~");
303  service = private_nodehandle.advertiseService(service_name, &Costmap2DROSServiceProvider::callback, this);
304 }
305 
307  ros::NodeHandle& node_handle,
308  const std::string& service_name)
309 : ros_costmap(ros_costmap)
310 {
311  service = node_handle.advertiseService(service_name, &Costmap2DROSServiceProvider::callback, this);
312 }
313 
315  cost_map_msgs::GetCostMap::Request &request,
316  cost_map_msgs::GetCostMap::Response &response)
317 {
319  cost_map::Length geometry;
320  geometry << request.length_x, request.length_y;
321  if ( !fromCostmap2DROSAtRobotPose(*ros_costmap, geometry, "obstacle_costs", cost_map) ) {
322  ROS_ERROR_STREAM("CostMap Service : failed to convert from Costmap2DROS");
323  }
324  toMessage(cost_map, response.map);
325  return true;
326 }
327 
328 /*****************************************************************************
329  ** Trailers
330  *****************************************************************************/
331 
332 } // namespace cost_map
Eigen::Matrix< unsigned char, Eigen::Dynamic, Eigen::Dynamic > Matrix
const unsigned char NO_INFORMATION
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
const Length & getLength() const
DataType & at(const std::string &layer, const Index &index)
f
void initializeFromCostmap2D(costmap_2d::Costmap2DROS &costmap2d, MapType &outputMap)
const std::vector< std::string > & getBasicLayers() const
const Size & getSize() const
double getResolution() const
void setStartIndex(const Index &startIndex)
void add(const std::string &layer, const DataType value=NO_INFORMATION)
Eigen::MatrixXf Matrix
Costmap2DROSServiceProvider(costmap_2d::Costmap2DROS *ros_costmap, const std::string &service_name="get_cost_map")
Definition: converter.cpp:298
const unsigned char LETHAL_OBSTACLE
void setFrameId(const std::string &frameId)
void toMessage(const cost_map::CostMap &cost_map, cost_map_msgs::CostMap &message)
Definition: converter.cpp:154
const unsigned char INSCRIBED_OBSTACLE
const Matrix & get(const std::string &layer) const
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool addLayerFromCostmap2DAtRobotPose(costmap_2d::Costmap2DROS &costmap2d, const std::string &layer, MapType &outputMap)
bool callback(cost_map_msgs::GetCostMap::Request &req, cost_map_msgs::GetCostMap::Response &res)
Definition: converter.cpp:314
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
const std::string & getFrameId() const
const std::string BGRA16
void setBasicLayers(const std::vector< std::string > &basicLayers)
Time getTimestamp() const
costmap_2d::Costmap2DROS * ros_costmap
Definition: converter.hpp:148
bool multiArrayMessageCopyToMatrixEigen(const MultiArrayMessageType_ &m, EigenType_ &e)
void setTimestamp(const Time timestamp)
bool addLayerFromROSImage(const sensor_msgs::Image &image, const std::string &layer_name, cost_map::CostMap &cost_map)
Definition: converter.cpp:37
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
bool initializeFromCostmap2DAtRobotPose(costmap_2d::Costmap2DROS &costmap2d, const Length &length, MapType &outputMap)
bool getPosition(const Index &index, Position &position) const
const std::string MONO16
bool matrixEigenCopyToMultiArrayMessage(const EigenType_ &e, MultiArrayMessageType_ &m)
void toOccupancyGrid(const cost_map::CostMap &cost_map, const std::string &layer, nav_msgs::OccupancyGrid &msg)
Definition: converter.cpp:245
grid_map::Index Index
void add(const std::string &layer, const double value=NAN)
bool addLayerFromCostmap2D(const costmap_2d::Costmap2D &costmap2d, const std::string &layer, MapType &outputMap)
bool fromCostmap2DROS(costmap_2d::Costmap2DROS &ros_costmap, const std::string &layer_name, cost_map::CostMap &cost_map)
Converts a ROS costmap to a costmap object.
Definition: converter.cpp:216
size_t getLinearIndexFromIndex(const Index &index, const Size &bufferSize, const bool rowMajor=false)
grid_map::Position Position
Matrix::Scalar DataType
const Index & getStartIndex() const
const std::vector< std::string > & getLayers() const
Costmap2D * getCostmap()
void setFrameId(const std::string &frameId)
#define ROS_ERROR_STREAM(args)
grid_map::Length Length
void setTimestamp(const Time timestamp)
bool fromCostmap2DROSAtRobotPose(costmap_2d::Costmap2DROS &ros_costmap, const cost_map::Length &geometry, const std::string &layer_name, cost_map::CostMap &cost_map)
Converts a ROS costmap around the robot to a costmap object.
Definition: converter.cpp:229
void toGridMap(const cost_map::CostMap cost_map, grid_map::GridMap &grid_map)
Convert a cost map object into a grid map object.
Definition: converter.cpp:130
bool fromMessage(const cost_map_msgs::CostMap &message, cost_map::CostMap &cost_map)
Definition: converter.cpp:186


cost_map_ros
Author(s): Daniel Stonier
autogenerated on Mon Jun 10 2019 13:03:48