Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include "ros/ros.h"
00030
00031 #include <nav_msgs/GetMap.h>
00032 #include <geometry_msgs/Quaternion.h>
00033 #include <geometry_msgs/PoseStamped.h>
00034 #include <sensor_msgs/image_encodings.h>
00035
00036 #include <cv_bridge/cv_bridge.h>
00037 #include <image_transport/image_transport.h>
00038 #include <Eigen/Geometry>
00039
00040 #include <hector_map_tools/HectorMapTools.h>
00041
00042 using namespace std;
00043
00048 class MapAsImageProvider
00049 {
00050 public:
00051 MapAsImageProvider()
00052 : pn_("~")
00053 {
00054
00055 image_transport_ = new image_transport::ImageTransport(n_);
00056 image_transport_publisher_full_ = image_transport_->advertise("map_image/full", 1);
00057 image_transport_publisher_tile_ = image_transport_->advertise("map_image/tile", 1);
00058
00059 pose_sub_ = n_.subscribe("pose", 1, &MapAsImageProvider::poseCallback, this);
00060 map_sub_ = n_.subscribe("map", 1, &MapAsImageProvider::mapCallback, this);
00061
00062
00063 cv_img_full_.header.frame_id = "map_image";
00064 cv_img_full_.encoding = sensor_msgs::image_encodings::MONO8;
00065
00066 cv_img_tile_.header.frame_id = "map_image";
00067 cv_img_tile_.encoding = sensor_msgs::image_encodings::MONO8;
00068
00069
00070 p_size_tiled_map_image_x_ = 64;
00071 p_size_tiled_map_image_y_ = 64;
00072
00073 ROS_INFO("Map to Image node started.");
00074 }
00075
00076 ~MapAsImageProvider()
00077 {
00078 delete image_transport_;
00079 }
00080
00081
00082 void poseCallback(const geometry_msgs::PoseStampedConstPtr& pose)
00083 {
00084 pose_ptr_ = pose;
00085 }
00086
00087
00088 void mapCallback(const nav_msgs::OccupancyGridConstPtr& map)
00089 {
00090 int size_x = map->info.width;
00091 int size_y = map->info.height;
00092
00093 if ((size_x < 3) || (size_y < 3) ){
00094 ROS_INFO("Map size is only x: %d, y: %d . Not running map to image conversion", size_x, size_y);
00095 return;
00096 }
00097
00098
00099 if (image_transport_publisher_full_.getNumSubscribers() > 0){
00100 cv::Mat* map_mat = &cv_img_full_.image;
00101
00102
00103 if ( (map_mat->rows != size_y) && (map_mat->cols != size_x)){
00104 *map_mat = cv::Mat(size_y, size_x, CV_8U);
00105 }
00106
00107 const std::vector<int8_t>& map_data (map->data);
00108
00109 unsigned char *map_mat_data_p=(unsigned char*) map_mat->data;
00110
00111
00112 int size_y_rev = size_y-1;
00113
00114 for (int y = size_y_rev; y >= 0; --y){
00115
00116 int idx_map_y = size_x * (size_y -y);
00117 int idx_img_y = size_x * y;
00118
00119 for (int x = 0; x < size_x; ++x){
00120
00121 int idx = idx_img_y + x;
00122
00123 switch (map_data[idx_map_y + x])
00124 {
00125 case -1:
00126 map_mat_data_p[idx] = 127;
00127 break;
00128
00129 case 0:
00130 map_mat_data_p[idx] = 255;
00131 break;
00132
00133 case 100:
00134 map_mat_data_p[idx] = 0;
00135 break;
00136 }
00137 }
00138 }
00139 image_transport_publisher_full_.publish(cv_img_full_.toImageMsg());
00140 }
00141
00142
00143 if ((image_transport_publisher_tile_.getNumSubscribers() > 0) && (pose_ptr_)){
00144
00145 world_map_transformer_.setTransforms(*map);
00146
00147 Eigen::Vector2f rob_position_world (pose_ptr_->pose.position.x, pose_ptr_->pose.position.y);
00148 Eigen::Vector2f rob_position_map (world_map_transformer_.getC2Coords(rob_position_world));
00149
00150 Eigen::Vector2i rob_position_mapi (rob_position_map.cast<int>());
00151
00152 Eigen::Vector2i tile_size_lower_halfi (p_size_tiled_map_image_x_ / 2, p_size_tiled_map_image_y_ / 2);
00153
00154 Eigen::Vector2i min_coords_map (rob_position_mapi - tile_size_lower_halfi);
00155
00156
00157 if (min_coords_map[0] < 0){
00158 min_coords_map[0] = 0;
00159 }
00160
00161 if (min_coords_map[1] < 0){
00162 min_coords_map[1] = 0;
00163 }
00164
00165 Eigen::Vector2i max_coords_map (min_coords_map + Eigen::Vector2i(p_size_tiled_map_image_x_,p_size_tiled_map_image_y_));
00166
00167
00168 if (max_coords_map[0] > size_x){
00169
00170 int diff = max_coords_map[0] - size_x;
00171 min_coords_map[0] -= diff;
00172
00173 max_coords_map[0] = size_x;
00174 }
00175
00176 if (max_coords_map[1] > size_y){
00177
00178 int diff = max_coords_map[1] - size_y;
00179 min_coords_map[1] -= diff;
00180
00181 max_coords_map[1] = size_y;
00182 }
00183
00184
00185 if (min_coords_map[0] < 0){
00186 min_coords_map[0] = 0;
00187 }
00188
00189 if (min_coords_map[1] < 0){
00190 min_coords_map[1] = 0;
00191 }
00192
00193 Eigen::Vector2i actual_map_dimensions(max_coords_map - min_coords_map);
00194
00195 cv::Mat* map_mat = &cv_img_tile_.image;
00196
00197
00198 if ( (map_mat->rows != actual_map_dimensions[0]) || (map_mat->cols != actual_map_dimensions[1])){
00199 *map_mat = cv::Mat(actual_map_dimensions[0], actual_map_dimensions[1], CV_8U);
00200 }
00201
00202 const std::vector<int8_t>& map_data (map->data);
00203
00204 unsigned char *map_mat_data_p=(unsigned char*) map_mat->data;
00205
00206
00207 int y_img = max_coords_map[1]-1;
00208
00209 for (int y = min_coords_map[1]; y < max_coords_map[1];++y){
00210
00211 int idx_map_y = y_img-- * size_x;
00212 int idx_img_y = (y-min_coords_map[1]) * actual_map_dimensions.x();
00213
00214 for (int x = min_coords_map[0]; x < max_coords_map[0];++x){
00215
00216 int img_index = idx_img_y + (x-min_coords_map[0]);
00217
00218 switch (map_data[idx_map_y+x])
00219 {
00220 case 0:
00221 map_mat_data_p[img_index] = 255;
00222 break;
00223
00224 case -1:
00225 map_mat_data_p[img_index] = 127;
00226 break;
00227
00228 case 100:
00229 map_mat_data_p[img_index] = 0;
00230 break;
00231 }
00232 }
00233 }
00234 image_transport_publisher_tile_.publish(cv_img_tile_.toImageMsg());
00235 }
00236 }
00237
00238 ros::Subscriber map_sub_;
00239 ros::Subscriber pose_sub_;
00240
00241 image_transport::Publisher image_transport_publisher_full_;
00242 image_transport::Publisher image_transport_publisher_tile_;
00243
00244 image_transport::ImageTransport* image_transport_;
00245
00246 geometry_msgs::PoseStampedConstPtr pose_ptr_;
00247
00248 cv_bridge::CvImage cv_img_full_;
00249 cv_bridge::CvImage cv_img_tile_;
00250
00251 ros::NodeHandle n_;
00252 ros::NodeHandle pn_;
00253
00254 int p_size_tiled_map_image_x_;
00255 int p_size_tiled_map_image_y_;
00256
00257 HectorMapTools::CoordinateTransformer<float> world_map_transformer_;
00258
00259 };
00260
00261 int main(int argc, char** argv)
00262 {
00263 ros::init(argc, argv, "map_to_image_node");
00264
00265 MapAsImageProvider map_image_provider;
00266
00267 ros::spin();
00268
00269 return 0;
00270 }