$search
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 #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 //Which frame_id makes sense? 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 //Fixed cell width for tile based image, use dynamic_reconfigure for this later 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 //We assume the robot position is available as a PoseStamped here (querying tf would be the more general option) 00082 void poseCallback(const geometry_msgs::PoseStampedConstPtr& pose) 00083 { 00084 pose_ptr_ = pose; 00085 } 00086 00087 //The map->image conversion runs every time a new map is received at the moment 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 // Only if someone is subscribed to it, do work and publish full map image 00099 if (image_transport_publisher_full_.getNumSubscribers() > 0){ 00100 cv::Mat* map_mat = &cv_img_full_.image; 00101 00102 // resize cv image if it doesn't have the same dimensions as the map 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 //We have to flip around the y axis, y for image starts at the top and y for map at the bottom 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 // Only if someone is subscribed to it, do work and publish tile-based map image Also check if pose_ptr_ is valid 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 //Clamp to lower map coords 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 //Clamp to upper map coords 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 //Clamp lower again (in case the map is smaller than the selected visualization window) 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 // resize cv image if it doesn't have the same dimensions as the selected visualization window 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 //We have to flip around the y axis, y for image starts at the top and y for map at the bottom 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 }