rosmain.cpp
Go to the documentation of this file.
00001 
00025 #include "ros/ros.h"
00026 #include "std_msgs/String.h"
00027 #include "nav_msgs/OccupancyGrid.h"
00028 #include "mapstitch/mapstitch.h"
00029 #include <tf/transform_broadcaster.h>
00030 #include <opencv/highgui.h>
00031 #include <unistd.h>
00032 #include <string>
00033 
00034 using namespace cv;
00035 using namespace tf;
00036 using namespace ros;
00037 using namespace nav_msgs;
00038 
00039 std::string save_stitch("");
00040 double max_distance = 5.;
00041 bool   debug = false;
00042 
00043 struct stitch_maps {
00044   Mat   asimage;
00045   float resolution,
00046         origin_x,
00047         origin_y;
00048   char* frame_id;
00049 } old_world, old_map;
00050 
00051 Transform ourtf;
00052 bool tf_valid = false;
00053 
00054 Mat toMat(const nav_msgs::OccupancyGrid *map)
00055 {
00056   uint8_t *data = (uint8_t*) map->data.data(),
00057            testpoint = data[0];
00058   bool mapHasPoints = false;
00059 
00060   Mat im(map->info.height, map->info.width, CV_8UC1);
00061 
00062   // transform the map in the same way the map_saver component does
00063   for (size_t i=0; i<map->data.size(); i++)
00064   {
00065     if (data[i] == 0)        im.data[i] = 254;
00066     else if (data[i] == 100) im.data[i] = 0;
00067     else im.data[i] = 205;
00068 
00069     // just check if there is actually something in the map
00070     if (i!=0) mapHasPoints = mapHasPoints || (data[i] != testpoint);
00071     testpoint = data[i];
00072   }
00073 
00074   // sanity check
00075   if (!mapHasPoints) { ROS_WARN("map is empty, ignoring update."); }
00076 
00077   return im;
00078 }
00079 
00080 void publish_stitch()
00081 {
00082   static tf::TransformBroadcaster br;
00083 
00084   if (tf_valid)
00085     br.sendTransform(
00086       StampedTransform(ourtf,Time::now()+Duration(3.), old_world.frame_id, old_map.frame_id));
00087 }
00088 
00089 void update_tf(struct stitch_maps *w, struct stitch_maps *m)
00090 {
00091   if (w == NULL) { ROS_INFO("world map not loaded"); return; }
00092   if (m == NULL) { ROS_INFO("map not loaded"); return; }
00093 
00094   // sanity check on input
00095   if (w->resolution != m->resolution) {
00096     ROS_WARN("map (%.3f) resolution differs from world resolution(%.3f)"
00097              "  and scaling not implemented, ignoring update",
00098              m->resolution, w->resolution);
00099     return;
00100   }
00101 
00102   if (debug) { // write images if debug
00103     imwrite("current_map.pgm", m->asimage);
00104     imwrite("current_world.pgm", w->asimage);
00105   }
00106 
00107   StitchedMap c(w->asimage,m->asimage, max_distance);
00108 
00109   // sanity checks
00110   if ((c.rotation == 0. && (int) c.transx == 0 && (int) c.transy == 0) ||
00111       (int) c.transx == INT_MAX || (int) c.transx == INT_MIN ||
00112       (int) c.transy == INT_MAX || (int) c.transy == INT_MIN)
00113   {
00114     ROS_INFO("homography estimation didn't work, not stitching.");
00115     return;
00116   }
00117 
00118   if (debug) { // write images if debug
00119     imwrite("current_stitch.pgm", c.get_stitch());
00120   }
00121 
00122   if (save_stitch.size() > 0) {
00123     imwrite(save_stitch.c_str(), c.get_stitch());
00124   }
00125 
00126   // publish this as the transformation between /world -> /map
00127   // The point-of-reference for opencv is the edge of the image, for ROS
00128   // this is the centerpoint of the image, which is why we translate each
00129   // point to the edge, apply rotation+translation from opencv and move
00130   // back to the center.
00131   float res = m->resolution;
00132   Mat H  = c.H;
00133   Mat E  = (Mat_<double>(3,3) << 1, 0,  m->origin_x,
00134                                  0, 1,  m->origin_y,
00135                                  0, 0, 1),
00136       E_ = (Mat_<double>(3,3) << 1, 0, -m->origin_x,
00137                                  0, 1, -m->origin_y,
00138                                  0, 0, 1);
00139   H.resize(3);
00140   H.at<double>(2,2) = 1.;
00141   H.at<double>(2,0) = H.at<double>(2,1) = 0.;
00142   H.at<double>(1,2) *= res;
00143   H.at<double>(0,2) *= res;
00144   H = E*H*E_;
00145 
00146   double rot = -1 *atan2(H.at<double>(0,1),H.at<double>(1,1)),
00147       transx = H.at<double>(0,2),
00148       transy = H.at<double>(1,2);
00149 
00150   ROS_INFO("stichted map with rotation %.5f radians and (%f,%f) translation",
00151       rot,transx,transy);
00152 
00153   ourtf = Transform( Quaternion(Vector3(0,0,1), rot),
00154                      Vector3(transx,transy,0) );
00155   tf_valid = true;
00156 
00157   if (m->frame_id  == w->frame_id)
00158     ROS_WARN("frame_id for world and map are the same, this will probably not work"
00159              "If map_server publishes your maps you might want to use _frame_id:=/world");
00160 }
00161 
00162 void update_stitch(struct stitch_maps *old_map,
00163                    const nav_msgs::OccupancyGrid& new_map)
00164 {
00165   old_map->asimage    = toMat(&new_map);
00166   old_map->resolution = new_map.info.resolution;
00167   old_map->origin_x   = new_map.info.origin.position.x;
00168   old_map->origin_y   = new_map.info.origin.position.y;
00169   if (old_map->frame_id == NULL)
00170     old_map->frame_id   = strdup(new_map.header.frame_id.c_str());
00171   else if (strcmp(old_map->frame_id, new_map.header.frame_id.c_str())!=0) {
00172     free(old_map->frame_id);
00173     old_map->frame_id  = strdup(new_map.header.frame_id.c_str());
00174   }
00175 }
00176 
00177 void worldCallback(const nav_msgs::OccupancyGrid& new_world)
00178 {
00179   update_stitch(&old_world, new_world);
00180   update_tf(&old_world, &old_map);
00181   publish_stitch();
00182 }
00183 
00184 void mapCallback(const nav_msgs::OccupancyGrid& new_map)
00185 {
00186   publish_stitch();
00187 }
00188 
00189 void alignCallback(const nav_msgs::OccupancyGrid& new_map)
00190 {
00191   update_stitch(&old_map, new_map);
00192   update_tf(&old_world, &old_map);
00193   publish_stitch();
00194 }
00195 
00196 int main(int argc, char **argv)
00197 {
00198   ros::init(argc, argv, "map_stitcher");
00199   ros::NodeHandle n;
00200   ros::NodeHandle param("~");
00201 
00202   // load the parameters
00203   param.getParam("save_stitch", save_stitch);
00204   param.getParam("max_distance", max_distance);
00205   param.getParam("debug", debug);
00206 
00207   // make sure these are initialized so we don't get a memory hole
00208   old_world.frame_id = old_map.frame_id = NULL;
00209 
00210   ros::Subscriber submap   = n.subscribe("map", 1000, mapCallback),
00211                   subworld = n.subscribe("world", 1000, worldCallback),
00212                   subalign = n.subscribe("align", 1000, alignCallback);
00213   ros::spin();
00214 
00215   return 0;
00216 }


mapstitch
Author(s): Philipp M. Scholl
autogenerated on Mon Oct 6 2014 02:09:53