8 #include <boost/thread/lock_guard.hpp> 11 #include <cost_map_msgs/CostMap.h> 15 #include <sensor_msgs/Image.h> 17 #include <std_msgs/UInt8MultiArray.h> 20 #include "../../include/cost_map_ros/converter.hpp" 38 const std::string& layer_name,
50 std::cout << ecl::red <<
"[ERROR] cv_bridge exception: " << e.what() << ecl::reset << std::endl;
62 depth = std::pow(2, 8);
69 depth = std::pow(2, 16);
72 std::cout << ecl::red <<
"[ERROR] expected BGR, BGRA, or MONO image encoding." << ecl::reset << std::endl;
76 std::cout << ecl::red <<
"[ERROR] cv_bridge exception: " << e.what() << ecl::reset << std::endl;
80 cost_map.
add(layer_name);
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;
91 const auto& cvAlpha = cvPtrAlpha->image.at<cv::Vec4b>((*iterator)(0),
93 unsigned int alpha = cvAlpha[3];
94 if (cvAlpha[3] < depth / 2)
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)
106 unsigned int grayValue;
107 if (depth == std::pow(2, 8)) {
108 uchar cvGrayscale = cvPtrMono->image.at<uchar>((*iterator)(0),
110 grayValue = cvGrayscale;
112 if (depth == std::pow(2, 16)) {
113 const auto& cvGrayscale = cvPtrMono->image.at<cv::Vec2b>((*iterator)(0),
115 grayValue = (cvGrayscale[0] << 8) + cvGrayscale[1];
120 cost_map.
at(layer_name, *iterator) = value;
135 for (
const std::string& layer_name : cost_map.
getLayers()) {
139 float *grid_map_ptr = grid_map_data.data();
140 for (
unsigned int i = 0; i < cost_map_data.size(); ++i ) {
142 ++cost_map_ptr; ++grid_map_ptr;
144 grid_map.
add(layer_name, grid_map_data);
156 std::vector<std::string> layers = cost_map.
getLayers();
158 message.info.header.stamp.fromNSec(cost_map.
getTimestamp());
159 message.info.header.frame_id = cost_map.
getFrameId();
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;
171 message.layers = layers;
174 message.data.clear();
175 for (
const auto& layer : layers) {
177 std_msgs::UInt8MultiArray data_array;
179 message.data.push_back(data_array);
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));
193 if (message.layers.size() != message.data.size()) {
198 for (
unsigned int i = 0; i < message.layers.size(); i++) {
204 cost_map.
add(message.layers[i], data);
217 const std::string& layer_name,
221 boost::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(ros_costmap.
getCostmap()->
getMutex()));
231 const std::string& layer_name,
235 boost::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(ros_costmap.
getCostmap()->
getMutex()));
248 msg.info.map_load_time = msg.header.stamp;
250 msg.info.width = cost_map.
getSize()(0);
251 msg.info.height = cost_map.
getSize()(1);
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);
264 size_t nCells = cost_map.
getSize().prod();
265 msg.data.resize(nCells);
268 const float cellMin = 0;
269 const float cellMax = 98;
270 const float cellRange = cellMax - cellMin;
272 const float data_minimum = 0;
273 const float data_maximum = 252;
291 value = (cost_map.
at(layer, *iterator) - data_minimum) / (data_maximum - data_minimum);
292 value = cellMin + std::min(std::max(0.0
f, value), 1.0
f) * cellRange;
295 msg.data[nCells - index - 1] = value;
299 const std::string& service_name)
300 : ros_costmap(ros_costmap)
308 const std::string& service_name)
309 : ros_costmap(ros_costmap)
315 cost_map_msgs::GetCostMap::Request &request,
316 cost_map_msgs::GetCostMap::Response &response)
320 geometry << request.length_x, request.length_y;
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)
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)
Costmap2DROSServiceProvider(costmap_2d::Costmap2DROS *ros_costmap, const std::string &service_name="get_cost_map")
const unsigned char LETHAL_OBSTACLE
void setFrameId(const std::string &frameId)
void toMessage(const cost_map::CostMap &cost_map, cost_map_msgs::CostMap &message)
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)
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
const std::string & getFrameId() const
void setBasicLayers(const std::vector< std::string > &basicLayers)
Time getTimestamp() const
costmap_2d::Costmap2DROS * ros_costmap
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)
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
ros::ServiceServer service
bool matrixEigenCopyToMultiArrayMessage(const EigenType_ &e, MultiArrayMessageType_ &m)
void toOccupancyGrid(const cost_map::CostMap &cost_map, const std::string &layer, nav_msgs::OccupancyGrid &msg)
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.
size_t getLinearIndexFromIndex(const Index &index, const Size &bufferSize, const bool rowMajor=false)
grid_map::Position Position
const Index & getStartIndex() const
const std::vector< std::string > & getLayers() const
void setFrameId(const std::string &frameId)
#define ROS_ERROR_STREAM(args)
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.
void toGridMap(const cost_map::CostMap cost_map, grid_map::GridMap &grid_map)
Convert a cost map object into a grid map object.
bool fromMessage(const cost_map_msgs::CostMap &message, cost_map::CostMap &cost_map)