map_to_image_node.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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 
00037 #include <cv_bridge/cv_bridge.h>        
00038 
00039 #include <image_transport/image_transport.h>
00040 #include <Eigen/Geometry>
00041 
00042 #include <HectorMapTools.h>
00043 #include <math.h>
00044 #include <tf/transform_datatypes.h>
00045 
00046 using namespace std;
00047 using namespace cv_bridge;
00048 using namespace cv;
00049 
00054 class MapAsImageProvider
00055 {
00056 
00057 public:
00058 
00059 
00060   MapAsImageProvider()
00061     : pn_("~")
00062   {
00063 
00064     image_transport_ = new image_transport::ImageTransport(n_);
00065     image_transport_publisher_full_position_ = image_transport_->advertise("map_image/full_with_position", 1);
00066     image_transport_publisher_full_ = image_transport_->advertise("map_image/full", 1);
00067     image_transport_publisher_tile_ = image_transport_->advertise("map_image/tile", 1);
00068 
00069     pose_sub_ = n_.subscribe("pose", 1, &MapAsImageProvider::poseCallback, this);
00070     map_sub_ = n_.subscribe("map", 1, &MapAsImageProvider::mapCallback, this);
00071 
00072     //Which frame_id makes sense?
00073     cv_img_full_with_position_.header.frame_id = "map_image";
00074     cv_img_full_with_position_.encoding = sensor_msgs::image_encodings::RGB8;
00075 
00076     cv_img_full_.header.frame_id = "map_image";
00077     cv_img_full_.encoding = sensor_msgs::image_encodings::MONO8;
00078 
00079     cv_img_tile_.header.frame_id = "map_image";
00080     cv_img_tile_.encoding = sensor_msgs::image_encodings::RGB8;
00081 
00082     //Fixed cell width for tile based image, use dynamic_reconfigure for this later
00083     p_size_tiled_map_image_x_ = 64;
00084     p_size_tiled_map_image_y_ = 64;
00085 
00086     ROS_INFO("Map to Image node started.");
00087   }
00088 
00089   ~MapAsImageProvider()
00090   {
00091     delete image_transport_;
00092   }
00093 
00094   //We assume the robot position is available as a PoseStamped here (querying tf would be the more general option)
00095   void poseCallback(const geometry_msgs::PoseStampedConstPtr& pose)
00096   {
00097     pose_ptr_ = pose;
00098   }
00099 
00100   //The map->image conversion runs every time a new map is received at the moment
00101   void mapCallback(const nav_msgs::OccupancyGridConstPtr& map)
00102   {
00103     int size_x = map->info.width;
00104     int size_y = map->info.height;
00105 
00106     if ((size_x < 3) || (size_y < 3) ){
00107       ROS_INFO("Map size is only x: %d,  y: %d . Not running map to image conversion", size_x, size_y);
00108       return;
00109     }
00110 
00111     // Only if someone is subscribed to it, do work and publish full map image
00112     if (image_transport_publisher_full_.getNumSubscribers() > 0  && (pose_ptr_)){
00113       Mat* map_mat  = &cv_img_full_.image;
00114       Mat* map_mat_position  = &cv_img_full_with_position_.image;
00115 
00116       // resize cv image if it doesn't have the same dimensions as the map
00117       if ( (map_mat->rows != size_y) && (map_mat->cols != size_x)){
00118         *map_mat = Mat(size_y, size_x, CV_8U);
00119         *map_mat_position = Mat(size_y, size_x, CV_8UC3);
00120       }
00121 
00122       const std::vector<int8_t>& map_data (map->data);
00123 
00124       unsigned char *map_mat_data_p=(unsigned char*) map_mat->data;
00125       unsigned char *map_mat_position_data_p=(unsigned char*) map_mat_position->data;
00126 
00127       //We have to flip around the y axis, y for image starts at the top and y for map at the bottom
00128       int size_y_rev = size_y-1;
00129 
00130       for (int y = size_y_rev; y >= 0; --y){
00131 
00132         int idx_map_y = size_x * (size_y -y);
00133         int idx_img_y = size_x * y;
00134 
00135         for (int x = 0; x < size_x; ++x){
00136 
00137           int idx = idx_img_y + x;
00138 
00139           switch (map_data[idx_map_y + x])
00140           {
00141           case -1:
00142             map_mat_position_data_p[idx*3] = 127;
00143             map_mat_position_data_p[idx*3+1] = 127;
00144             map_mat_position_data_p[idx*3+2] = 127;
00145             map_mat_data_p[idx] = 127;
00146             break;
00147 
00148           case 0:
00149             map_mat_position_data_p[idx*3] = 255;
00150             map_mat_position_data_p[idx*3+1] = 255;
00151             map_mat_position_data_p[idx*3+2] = 255;
00152             map_mat_data_p[idx] = 255;
00153             break;
00154 
00155           case 100:
00156             map_mat_position_data_p[idx*3] = 0;
00157             map_mat_position_data_p[idx*3+1] = 0;
00158             map_mat_position_data_p[idx*3+2] = 0;
00159             map_mat_data_p[idx] = 0;
00160             break;
00161           }
00162         }
00163       }
00164 
00165       // publish the image representating the full map without the position of the robot
00166       image_transport_publisher_full_.publish(cv_img_full_.toImageMsg()); 
00167 
00168       world_map_transformer_.setTransforms(*map);
00169 
00170       Eigen::Vector2f rob_position_world (pose_ptr_->pose.position.x, pose_ptr_->pose.position.y);
00171       Eigen::Vector2f rob_position_map (world_map_transformer_.getC2Coords(rob_position_world));
00172 
00173       Eigen::Vector2i rob_position_mapi (rob_position_map.cast<int>());
00174 
00175 
00176         //draw the position of the robot on the map
00177         double yaw = tf::getYaw(pose_ptr_->pose.orientation);
00178         double radius = size_x / 100;
00179         circle(*map_mat_position, cvPoint(rob_position_mapi[0],size_y - rob_position_mapi[1]), radius, Scalar(255,0,0),3);
00180         line(*map_mat_position, cvPoint(rob_position_mapi[0],size_y - rob_position_mapi[1]), cvPoint(rob_position_mapi[0] + radius*cos(yaw),size_y - rob_position_mapi[1] - radius * sin(yaw)), Scalar(255,0,0),3);
00181 
00182         // publish the image with position
00183         image_transport_publisher_full_position_.publish(cv_img_full_with_position_.toImageMsg()); 
00184     }
00185 
00186     // Only if someone is subscribed to it, do work and publish tile-based map image Also check if pose_ptr_ is valid
00187     if ((image_transport_publisher_tile_.getNumSubscribers() > 0) && (pose_ptr_)){
00188 
00189       world_map_transformer_.setTransforms(*map);
00190 
00191       Eigen::Vector2f rob_position_world (pose_ptr_->pose.position.x, pose_ptr_->pose.position.y);
00192       Eigen::Vector2f rob_position_map (world_map_transformer_.getC2Coords(rob_position_world));
00193 
00194       Eigen::Vector2i rob_position_mapi (rob_position_map.cast<int>());
00195 
00196       Eigen::Vector2i tile_size_lower_halfi (p_size_tiled_map_image_x_ / 2, p_size_tiled_map_image_y_ / 2);
00197 
00198       Eigen::Vector2i min_coords_map (rob_position_mapi - tile_size_lower_halfi);
00199 
00200       //Clamp to lower map coords
00201       if (min_coords_map[0] < 0){
00202         min_coords_map[0] = 0;
00203       }
00204 
00205       if (min_coords_map[1] < 0){
00206         min_coords_map[1] = 0;
00207       }
00208 
00209       Eigen::Vector2i max_coords_map (min_coords_map + Eigen::Vector2i(p_size_tiled_map_image_x_,p_size_tiled_map_image_y_));
00210 
00211       //Clamp to upper map coords
00212       if (max_coords_map[0] > size_x){
00213 
00214         int diff = max_coords_map[0] - size_x;
00215         min_coords_map[0] -= diff;
00216 
00217         max_coords_map[0] = size_x;
00218       }
00219 
00220       if (max_coords_map[1] > size_y){
00221 
00222         int diff = max_coords_map[1] - size_y;
00223         min_coords_map[1] -= diff;
00224 
00225         max_coords_map[1] = size_y;
00226       }
00227 
00228       //Clamp lower again (in case the map is smaller than the selected visualization window)
00229       if (min_coords_map[0] < 0){
00230         min_coords_map[0] = 0;
00231       }
00232 
00233       if (min_coords_map[1] < 0){
00234         min_coords_map[1] = 0;
00235       }
00236 
00237       Eigen::Vector2i actual_map_dimensions(max_coords_map - min_coords_map);
00238 
00239       Mat* map_mat  = &cv_img_tile_.image;
00240 
00241       // resize cv image if it doesn't have the same dimensions as the selected visualization window
00242       if ( (map_mat->rows != actual_map_dimensions[0]) || (map_mat->cols != actual_map_dimensions[1])){
00243         *map_mat = Mat(actual_map_dimensions[0], actual_map_dimensions[1], CV_8U);
00244       }
00245 
00246       const std::vector<int8_t>& map_data (map->data);
00247 
00248       unsigned char *map_mat_data_p=(unsigned char*) map_mat->data;
00249 
00250       //We have to flip around the y axis, y for image starts at the top and y for map at the bottom
00251       int y_img = max_coords_map[1]-1;
00252 
00253       for (int y = min_coords_map[1]; y < max_coords_map[1];++y){
00254 
00255         int idx_map_y = y_img-- * size_x;
00256         int idx_img_y = (y-min_coords_map[1]) * actual_map_dimensions.x();
00257 
00258         for (int x = min_coords_map[0]; x < max_coords_map[0];++x){
00259 
00260           int img_index = idx_img_y + (x-min_coords_map[0]);
00261 
00262           switch (map_data[idx_map_y+x])
00263           {
00264           case 0:
00265             map_mat_data_p[img_index] = 255;
00266             break;
00267 
00268           case -1:
00269             map_mat_data_p[img_index] = 127;
00270             break;
00271 
00272           case 100:
00273             map_mat_data_p[img_index] = 0;
00274             break;
00275           }
00276         }        
00277       }
00278       image_transport_publisher_tile_.publish(cv_img_tile_.toImageMsg());
00279     }
00280   }
00281 
00282   ros::Subscriber map_sub_;
00283   ros::Subscriber pose_sub_;
00284 
00285   image_transport::Publisher image_transport_publisher_full_;
00286   image_transport::Publisher image_transport_publisher_full_position_;
00287   image_transport::Publisher image_transport_publisher_tile_;
00288 
00289   image_transport::ImageTransport* image_transport_;
00290 
00291   geometry_msgs::PoseStampedConstPtr pose_ptr_;
00292 
00293   CvImage cv_img_full_;
00294   CvImage cv_img_full_with_position_;
00295   CvImage cv_img_tile_;
00296 
00297   ros::NodeHandle n_;
00298   ros::NodeHandle pn_;
00299 
00300   int p_size_tiled_map_image_x_;
00301   int p_size_tiled_map_image_y_;
00302 
00303   HectorMapTools::CoordinateTransformer<float> world_map_transformer_;
00304 
00305 };
00306 
00307 int main(int argc, char** argv)
00308 {
00309   ros::init(argc, argv, "map_to_image_node");
00310 
00311   MapAsImageProvider map_image_provider;
00312 
00313   ros::spin();
00314 
00315   return 0;
00316 }


map_to_jpeg
Author(s):
autogenerated on Wed Aug 26 2015 11:10:09