hector_elevation_visualization: hector_elevation_visualization: hector_elevation

00001 #include "hector_elevation_visualization.h"
00002 
00003 
00004 // compute linear index for given map coords
00005 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
00006 
00007 
00008 ElevationVisualization::ElevationVisualization(): nHandle("~")
00009 {
00010     nHandle.param("max_height_levels",max_height_levels,10); //[cell]
00011     nHandle.param("min_height",min_height,-1.0); //[m]
00012     nHandle.param("max_height",max_height,1.5); //[m]
00013     nHandle.param("color_factor",color_factor,0.8); //[]
00014     double r, g, b, a;
00015     nHandle.param("color/r", r, 0.0);
00016     nHandle.param("color/g", g, 0.0);
00017     nHandle.param("color/b", b, 1.0);
00018     nHandle.param("color/a", a, 1.0);
00019     marker_color.r = r;
00020     marker_color.g = g;
00021     marker_color.b = b;
00022     marker_color.a = a;
00023     nHandle.param("use_color_map", use_color_map, true); //[]
00024 
00025     nHandle.param("elevation_map_frame_id", elevation_map_frame_id,std::string("/elevation_map_local"));
00026     nHandle.param("paramSysMsgTopic", sys_msg_topic, std::string("/syscommand"));
00027 
00028     map_marker_array_publisher = nHandle.advertise<visualization_msgs::MarkerArray>("elevation_map_marker_array", 1,true);
00029 
00030     sub_elevation_map = nHandle.subscribe(elevation_map_frame_id,1,&ElevationVisualization::map_callback,this);
00031     sub_sys_message_callback = nHandle.subscribe(sys_msg_topic,1,&ElevationVisualization::sys_message_callback,this);
00032 
00033     dyn_rec_server_.setCallback(boost::bind(&ElevationVisualization::dynRecParamCallback, this, _1, _2));
00034 
00035     ros::spin();
00036 }
00037 
00038 ElevationVisualization::~ElevationVisualization()
00039 {
00040 
00041 }
00042 
00043 void ElevationVisualization::dynRecParamCallback(hector_elevation_visualization::ElevationVisualizationConfig &config, uint32_t level)
00044 {
00045     max_height_levels = config.max_height_levels;
00046     min_height = config.min_height;
00047     max_height = config.max_height;
00048     color_factor = config.color_factor;
00049     use_color_map = config.use_color_map;
00050 
00051     if(std::strcmp(elevation_map_frame_id.c_str(),(config.elevation_map_frame_id).c_str()))
00052     {
00053         elevation_map_frame_id = config.elevation_map_frame_id;
00054 
00055         sub_elevation_map = nHandle.subscribe(elevation_map_frame_id,1,&ElevationVisualization::map_callback,this);
00056     }
00057 }
00058 
00059 
00060 void ElevationVisualization::visualize_map(const hector_elevation_msgs::ElevationGrid& elevation_map, tf::StampedTransform local_map_transform)
00061 {
00062     int current_height_level;
00063 
00064     for (unsigned i = 0; i < map_marker_array_msg.markers.size(); ++i)
00065     {
00066         map_marker_array_msg.markers[i].points.clear();
00067     }
00068     map_marker_array_msg.markers.clear();
00069 
00070 
00071     // each array stores all cubes of one height level:
00072     map_marker_array_msg.markers.resize(max_height_levels+1);
00073 
00074     for (int index_y = 0; index_y < (int)elevation_map.info.height; ++index_y)
00075     {
00076         for (int index_x = 0; index_x < (int)elevation_map.info.width; ++index_x)
00077         {
00078             // visualize only known cells
00079             if(elevation_map.data[MAP_IDX(elevation_map.info.width, index_x, index_y)] != (int16_t)-elevation_map.info.zero_elevation)
00080             {
00081                 geometry_msgs::Point cube_center;
00082                 Eigen::Vector2f index_map(index_x, index_y);
00083                 Eigen::Vector2f index_world = world_map_transform.getC1Coords(index_map);
00084 
00085                 cube_center.x = index_world(0);//+elevation_map.info.resolution_xy/2.0;
00086                 cube_center.y = index_world(1);//+elevation_map.info.resolution_xy/2.0;
00087                 cube_center.z = (elevation_map.data[MAP_IDX(elevation_map.info.width, index_x, index_y)]-elevation_map.info.zero_elevation)*elevation_map.info.resolution_z;
00088                 current_height_level = max_height_levels/2+(int)round(std::min(std::max((double)cube_center.z+local_map_transform.getOrigin().z(), -(double)max_height), (double)max_height)*(double)max_height_levels/((double)max_height*2.0f));
00089                 map_marker_array_msg.markers[current_height_level].points.push_back(cube_center);
00090 
00091                 if(use_color_map)
00092                 {
00093                     double h = (1.0 - std::min(std::max((cube_center.z-min_height)/ (max_height - min_height), 0.0), 1.0)) *color_factor;
00094                     map_marker_array_msg.markers[current_height_level].colors.push_back(heightMapColor(h));
00095                 }
00096             }
00097         }
00098     }
00099 
00100     for (unsigned i = 0; i < map_marker_array_msg.markers.size(); ++i)
00101     {
00102         std::stringstream ss;
00103         ss <<"Level "<<i;
00104         map_marker_array_msg.markers[i].ns = ss.str();
00105         map_marker_array_msg.markers[i].id = i;
00106         map_marker_array_msg.markers[i].header.frame_id = "/map";
00107         map_marker_array_msg.markers[i].header.stamp =  elevation_map.header.stamp;
00108         map_marker_array_msg.markers[i].lifetime = ros::Duration();
00109         map_marker_array_msg.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
00110         map_marker_array_msg.markers[i].scale.x = elevation_map.info.resolution_xy;
00111         map_marker_array_msg.markers[i].scale.y = elevation_map.info.resolution_xy;
00112         map_marker_array_msg.markers[i].scale.z = elevation_map.info.resolution_z;
00113         map_marker_array_msg.markers[i].color = marker_color;
00114 
00115         if (map_marker_array_msg.markers[i].points.size() > 0)
00116             map_marker_array_msg.markers[i].action = visualization_msgs::Marker::ADD;
00117         else
00118             map_marker_array_msg.markers[i].action = visualization_msgs::Marker::DELETE;
00119     }
00120 
00121 }
00122 
00123 
00124 void ElevationVisualization::map_callback(const hector_elevation_msgs::ElevationGrid& elevation_map)
00125 {
00126     // Visualization of Elevation Map
00127     if(map_marker_array_publisher.getNumSubscribers () > 0)
00128     {
00129         tf::TransformListener listener;
00130         tf::StampedTransform local_map_transform;
00131 
00132         // get local map transform
00133         try
00134         {
00135             listener.waitForTransform("/base_footprint", "/map",ros::Time(0),ros::Duration(5.0));
00136             listener.lookupTransform("/base_footprint", "/map", ros::Time(0), local_map_transform);
00137         }
00138         catch (tf::TransformException ex)
00139         {
00140             ROS_ERROR("%s",ex.what());
00141             return;
00142         }
00143 
00144         // init transform
00145         nav_msgs::MapMetaData map_meta;
00146         map_meta.resolution = elevation_map.info.resolution_xy;
00147         map_meta.height = elevation_map.info.height;
00148         map_meta.width = elevation_map.info.width;
00149         map_meta.origin = elevation_map.info.origin;
00150 
00151         world_map_transform.setTransforms(map_meta);
00152 
00153         // visualize map
00154         visualize_map(elevation_map, local_map_transform);
00155 
00156 
00157         // publish map
00158         map_marker_array_publisher.publish(map_marker_array_msg);
00159     }
00160 }
00161 
00162 void ElevationVisualization::sys_message_callback(const std_msgs::String& string)
00163 {
00164     ROS_DEBUG("sysMsgCallback, msg contents: %s", string.data.c_str());
00165 
00166     if (string.data == "reset")
00167     {
00168         ROS_INFO("reset");
00169 
00170         for (unsigned i = 0; i < map_marker_array_msg.markers.size(); ++i)
00171         {
00172             map_marker_array_msg.markers[i].points.clear();
00173         }
00174         map_marker_array_msg.markers.clear();
00175     }
00176 }
00177 
00178 std_msgs::ColorRGBA ElevationVisualization::heightMapColor(double h)
00179 {
00180 
00181     std_msgs::ColorRGBA color;
00182     color.a = 1.0;
00183     // blend over HSV-values (more colors)
00184 
00185     double s = 1.0;
00186     double v = 1.0;
00187 
00188     h -= floor(h);
00189     h *= 6;
00190     int i;
00191     double m, n, f;
00192 
00193     i = floor(h);
00194     f = h - i;
00195     if (!(i & 1))
00196         f = 1 - f; // if i is even
00197     m = v * (1 - s);
00198     n = v * (1 - s * f);
00199 
00200     switch (i) {
00201     case 6:
00202     case 0:
00203         color.r = v; color.g = n; color.b = m;
00204         break;
00205     case 1:
00206         color.r = n; color.g = v; color.b = m;
00207         break;
00208     case 2:
00209         color.r = m; color.g = v; color.b = n;
00210         break;
00211     case 3:
00212         color.r = m; color.g = n; color.b = v;
00213         break;
00214     case 4:
00215         color.r = n; color.g = m; color.b = v;
00216         break;
00217     case 5:
00218         color.r = v; color.g = m; color.b = n;
00219         break;
00220     default:
00221         color.r = 1; color.g = 0.5; color.b = 0.5;
00222         break;
00223     }
00224 
00225     return color;
00226 }
00227 
00228 
00229
hector_elevation_visualization.cpp
