hector_elevation_mapping.cpp
Go to the documentation of this file.
00001 #include <pluginlib/class_list_macros.h>
00002 #include <nodelet/nodelet.h>
00003 #include "hector_elevation_mapping.h" 
00004 
00005 // compute linear index for given map coords
00006 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
00007 
00008 namespace hector_elevation_mapping
00009 {
00010 PLUGINLIB_DECLARE_CLASS(hector_elevation_mapping, ElevationMapping, hector_elevation_mapping::ElevationMapping, nodelet::Nodelet)
00011 
00012 void ElevationMapping::onInit()
00013 {
00014     int width, height, zero_elevation;
00015 
00016     nPrivateHandle = getPrivateNodeHandle();
00017     nHandle = getNodeHandle();
00018 
00019     nPrivateHandle.param("elevation_resolution", elevation_map_meta.resolution_z, 0.01); //[m]
00020     nPrivateHandle.param("elevation_zero_level", zero_elevation, 16384); //[cell]
00021     elevation_map_meta.zero_elevation = zero_elevation;
00022     nPrivateHandle.param("min_observable_elevation", elevation_map_meta.min_elevation, -1.00); //[m]
00023     nPrivateHandle.param("max_observable_elevation", elevation_map_meta.max_elevation, 0.30); //[m]
00024     nPrivateHandle.param("max_observable_distance", max_observable_distance, 4.00); //[m]
00025 
00026     nPrivateHandle.param("map_resolution", elevation_map_meta.resolution_xy, 0.05); //[m]
00027     nPrivateHandle.param("max_grid_size_x", width, 1024); //[cell]
00028     elevation_map_meta.width = width;
00029     nPrivateHandle.param("max_grid_size_y", height, 1024); //[cell]
00030     elevation_map_meta.height = height;
00031 
00032     nPrivateHandle.param("publish_poseupdate", publish_poseupdate, true); //[]
00033 
00034     nPrivateHandle.param("sensor_variance", sensor_variance, 0.001); //[m^2]
00035 
00036     nPrivateHandle.param("origin_x",elevation_map_meta.origin.position.x, -(double)elevation_map_meta.width*(double)elevation_map_meta.resolution_xy/2.0f); //[m]
00037     nPrivateHandle.param("origin_y",elevation_map_meta.origin.position.y, -(double)elevation_map_meta.height*(double)elevation_map_meta.resolution_xy/2.0f); //[m]
00038     nPrivateHandle.param("origin_z",elevation_map_meta.origin.position.z, 0.0); //[m]
00039     nPrivateHandle.param("orientation_x",elevation_map_meta.origin.orientation.x, 0.0); //[]
00040     nPrivateHandle.param("orientation_y",elevation_map_meta.origin.orientation.y, 0.0); //[]
00041     nPrivateHandle.param("orientation_z",elevation_map_meta.origin.orientation.z, 0.0); //[]
00042     nPrivateHandle.param("orientation_w",elevation_map_meta.origin.orientation.w, 1.0); //[]
00043 
00044     local_elevation_map.info = elevation_map_meta;
00045     global_elevation_map.info = elevation_map_meta;
00046 
00047     map_meta.origin.position.x = elevation_map_meta.origin.position.x;
00048     map_meta.origin.position.y = elevation_map_meta.origin.position.y;
00049     map_meta.origin.position.z = elevation_map_meta.origin.position.z;
00050 
00051     map_meta.origin.orientation.x = elevation_map_meta.origin.orientation.x;
00052     map_meta.origin.orientation.y = elevation_map_meta.origin.orientation.y;
00053     map_meta.origin.orientation.z = elevation_map_meta.origin.orientation.z;
00054     map_meta.origin.orientation.w = elevation_map_meta.origin.orientation.w;
00055 
00056     map_meta.resolution = elevation_map_meta.resolution_xy;
00057     world_map_transform.setTransforms(map_meta);
00058 
00059     nPrivateHandle.param("map_frame_id", map_frame_id,std::string("map"));
00060     nPrivateHandle.param("local_map_frame_id", local_map_frame_id,std::string("base_stabilized"));
00061     nPrivateHandle.param("local_elevation_map_topic", local_elevation_map_topic, std::string("elevation_map_local"));
00062     nPrivateHandle.param("global_elevation_map_topic", global_elevation_map_topic, std::string("elevation_map_global"));
00063     nPrivateHandle.param("point_cloud_topic", point_cloud_topic, std::string("openni/depth/points"));
00064     nPrivateHandle.param("grid_map_topic", grid_map_topic, std::string("scanmatcher_map"));
00065     nPrivateHandle.param("pose_update_topic",pose_update_topic,std::string("poseupdate"));
00066 
00067     nPrivateHandle.param("sys_msg_topic", sys_msg_topic, std::string("syscommand"));
00068 
00069     nPrivateHandle.param("poseupdate_pub_period",poseupdate_pub_period,1.0); //[s]
00070     nPrivateHandle.param("poseupdate_height_covariance",poseupdate_height_covariance,0.25); //[m²]
00071     nPrivateHandle.param("poseupdate_used_pattern_size",poseupdate_used_pattern_size,3); //[]
00072 
00073     // allocate memory and set default values
00074     local_elevation_map.data = std::vector<int16_t>(elevation_map_meta.width * elevation_map_meta.height,(int16_t)-elevation_map_meta.zero_elevation);
00075     global_elevation_map.data = std::vector<int16_t>(elevation_map_meta.width * elevation_map_meta.height,(int16_t)-elevation_map_meta.zero_elevation);
00076     cell_variance = std::vector<double>(elevation_map_meta.width * elevation_map_meta.height,std::numeric_limits<double>::max());
00077 
00078     sub_pointCloud = nHandle.subscribe(point_cloud_topic,1,&ElevationMapping::cloudCallback,this);
00079     sub_sysMessage = nHandle.subscribe(sys_msg_topic, 10, &ElevationMapping::sysMessageCallback, this);
00080     sub_grid_map_info = nHandle.subscribe(grid_map_topic + "_metadata",1,&ElevationMapping::updateParamsCallback,this);
00081 
00082     pub_local_map = nPrivateHandle.advertise<hector_elevation_msgs::ElevationGrid>(local_elevation_map_topic, 1, true);
00083     pub_local_map_meta = nPrivateHandle.advertise<hector_elevation_msgs::ElevationMapMetaData>(local_elevation_map_topic + "_metadata", 1, true);
00084     pub_global_map = nPrivateHandle.advertise<hector_elevation_msgs::ElevationGrid>(global_elevation_map_topic, 1, true);
00085     pub_global_map_meta = nPrivateHandle.advertise<hector_elevation_msgs::ElevationMapMetaData>(global_elevation_map_topic + "_metadata", 1, true);
00086 
00087     pub_local_map_meta.publish(local_elevation_map.info);
00088     pub_global_map_meta.publish(global_elevation_map.info);
00089 
00090     pub_height_update = nPrivateHandle.advertise<geometry_msgs::PoseWithCovarianceStamped>(pose_update_topic, 1, true);
00091 
00092     if(publish_poseupdate)
00093         timer = nHandle.createTimer(ros::Duration(poseupdate_pub_period), &ElevationMapping::timerCallback,this);
00094 
00095     ROS_INFO("HectorEM: is up and running.");   
00096 }
00097 
00098 ElevationMapping::~ElevationMapping()
00099 {
00100     ROS_INFO("HectorEM: Shutting down!");
00101 }
00102 
00103 void ElevationMapping::timerCallback(const ros::TimerEvent& event)
00104 {
00105     tf::StampedTransform local_map_transform;
00106 
00107     // get local map transform
00108     try
00109     {
00110         listener.waitForTransform(map_frame_id, local_map_frame_id,ros::Time(0),ros::Duration(1.0));
00111         listener.lookupTransform(map_frame_id, local_map_frame_id,ros::Time(0), local_map_transform);
00112     }
00113     catch (tf::TransformException ex)
00114     {
00115         ROS_DEBUG("HectorEM: localmap transform in timer callback failed");
00116         ROS_ERROR("%s",ex.what());
00117         return;
00118     }
00119 
00120     // allign grid points
00121     Eigen::Vector2f index_world(local_map_transform.getOrigin().x(), local_map_transform.getOrigin().y());
00122     Eigen::Vector2f index_map = world_map_transform.getC2Coords(index_world);
00123 
00124     // check if elevation of current robot position is known, otherwise cancel pose update
00125     if(global_elevation_map.data[MAP_IDX(elevation_map_meta.width, (int)index_map(0), (int)index_map(1))] != (int16_t)-elevation_map_meta.zero_elevation)
00126     {
00127         geometry_msgs::PoseWithCovarianceStamped cell_height_average;
00128 
00129         int error_level  = 0;
00130         int pattern_cell_quantity = 4*poseupdate_used_pattern_size*poseupdate_used_pattern_size;
00131 
00133         // include neighbours
00134         for(int x=index_map(0)-poseupdate_used_pattern_size;x<index_map(0)+poseupdate_used_pattern_size;x++)
00135         {
00136             for(int y=index_map(1)-poseupdate_used_pattern_size;y<index_map(1)+poseupdate_used_pattern_size;y++)
00137             {
00138                 int cell_index = MAP_IDX(elevation_map_meta.width, x, y);
00139 
00140                 if(global_elevation_map.data[cell_index] == (int16_t)-elevation_map_meta.zero_elevation)
00141                 {
00142                     // unknown cell
00143                     error_level++;
00144                 }
00145                 else
00146                 {
00147                     // cell is knwon, accumulate cell hight
00148                     cell_height_average.pose.pose.position.z += (global_elevation_map.data[cell_index]-elevation_map_meta.zero_elevation)*elevation_map_meta.resolution_z;
00149                 }
00150             }
00151         }
00152 
00153         // only publish pose update, if more than 1/2 of pattern cells are known
00154         if(error_level < pattern_cell_quantity/2)
00155         {
00156             pattern_cell_quantity -= error_level;
00157 
00158             cell_height_average.pose.pose.position.z = cell_height_average.pose.pose.position.z/(double)pattern_cell_quantity;
00159 
00160             cell_height_average.header.frame_id = map_frame_id;
00161             cell_height_average.header.stamp = ros::Time::now();
00162 
00163             cell_height_average.pose.covariance.at(0) = 0.0; //no x-position information
00164             cell_height_average.pose.covariance.at(7) = 0.0; //no y-position information
00165             cell_height_average.pose.covariance.at(14) = 1.0/poseupdate_height_covariance;
00166 
00167             pub_height_update.publish(cell_height_average);
00168 
00169             ROS_DEBUG("HectorEM: published height update %f",cell_height_average.pose.pose.position.z);
00170 
00171         }
00172     }
00173 }
00174 
00175 void ElevationMapping::sysMessageCallback(const std_msgs::String& string)
00176 {
00177     ROS_DEBUG("HectorEM: sysMsgCallback, msg contents: %s", string.data.c_str());
00178 
00179     if (string.data == "reset")
00180     {
00181         ROS_INFO("HectorEM: reset");
00182 
00183         // allocate memory and set default values
00184         local_elevation_map.data = std::vector<int16_t>(elevation_map_meta.width * elevation_map_meta.height,(int16_t)-elevation_map_meta.zero_elevation);
00185         global_elevation_map.data = std::vector<int16_t>(elevation_map_meta.width * elevation_map_meta.height,(int16_t)-elevation_map_meta.zero_elevation);
00186         cell_variance = std::vector<double>(elevation_map_meta.width * elevation_map_meta.height,std::numeric_limits<double>::max());
00187 
00188     }
00189 }
00190 
00191 void ElevationMapping::updateParamsCallback(const nav_msgs::MapMetaData& map_meta_data)
00192 {
00193     ROS_DEBUG("HectorEM: received new grid map meta data -> overwrite old parameters");
00194 
00195     // set new parameters
00196     elevation_map_meta.width = map_meta_data.width;
00197     elevation_map_meta.height = map_meta_data.height;
00198     elevation_map_meta.resolution_xy = map_meta_data.resolution;
00199 
00200     elevation_map_meta.origin.position.x = map_meta_data.origin.position.x;
00201     elevation_map_meta.origin.position.y = map_meta_data.origin.position.y;
00202     elevation_map_meta.origin.position.z = map_meta_data.origin.position.z;
00203 
00204     elevation_map_meta.origin.orientation.x = map_meta_data.origin.orientation.x;
00205     elevation_map_meta.origin.orientation.y = map_meta_data.origin.orientation.y;
00206     elevation_map_meta.origin.orientation.z = map_meta_data.origin.orientation.z;
00207     elevation_map_meta.origin.orientation.w = map_meta_data.origin.orientation.w;
00208 
00209     local_elevation_map.info = elevation_map_meta;
00210     global_elevation_map.info = elevation_map_meta;
00211 
00212     map_meta = map_meta_data;
00213     world_map_transform.setTransforms(map_meta);
00214 
00215     // allocate memory and set default values
00216     local_elevation_map.data = std::vector<int16_t>(elevation_map_meta.width * elevation_map_meta.height,(int16_t)-elevation_map_meta.zero_elevation);
00217     global_elevation_map.data = std::vector<int16_t>(elevation_map_meta.width * elevation_map_meta.height,(int16_t)-elevation_map_meta.zero_elevation);
00218     cell_variance = std::vector<double>(elevation_map_meta.width * elevation_map_meta.height,std::numeric_limits<double>::max());
00219 
00220     pub_local_map_meta.publish(local_elevation_map.info);
00221     pub_global_map_meta.publish(global_elevation_map.info);
00222 }
00223 
00224 void ElevationMapping::cloudCallback(const sensor_msgs::PointCloud2ConstPtr& pointcloud2_sensor_msg)
00225 {
00226     pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud2_map_pcl (new pcl::PointCloud<pcl::PointXYZ> ()),
00227     pointcloud2_sensor_pcl (new pcl::PointCloud<pcl::PointXYZ> ());
00228     tf::StampedTransform local_map_transform;
00229 
00230     ROS_DEBUG("HectorEM received a point cloud.");
00231 
00232     // converting PointCloud2 msg format to pcl pointcloud format in order to read the 3d data
00233     pcl::fromROSMsg(*pointcloud2_sensor_msg, *pointcloud2_sensor_pcl);
00234 
00235 
00236     // transform cloud to /map frame
00237     try
00238     {
00239         listener.waitForTransform(map_frame_id, pointcloud2_sensor_msg->header.frame_id,pointcloud2_sensor_msg->header.stamp,ros::Duration(1.0));
00240         pcl_ros::transformPointCloud(map_frame_id,*pointcloud2_sensor_pcl,*pointcloud2_map_pcl,listener);
00241     }
00242     catch (tf::TransformException ex)
00243     {
00244         ROS_ERROR("%s",ex.what());
00245         ROS_DEBUG("HectorEM: pointcloud transform failed");
00246         return;
00247     }
00248 
00249     // get local map transform
00250     try
00251     {
00252         listener.waitForTransform(map_frame_id, local_map_frame_id,pointcloud2_sensor_msg->header.stamp,ros::Duration(1.0));
00253         listener.lookupTransform(map_frame_id, local_map_frame_id, pointcloud2_sensor_msg->header.stamp, local_map_transform);
00254     }
00255     catch (tf::TransformException ex)
00256     {
00257         ROS_ERROR("%s",ex.what());
00258         ROS_DEBUG("HectorEM: localmap transform in cloud callback failed");
00259         return;
00260     }
00261 
00262     bool local_map_is_subscribed = (pub_local_map.getNumSubscribers () > 0);
00263     bool global_map_is_subscribed = (pub_global_map.getNumSubscribers () > 0);
00264 
00265     if(local_map_is_subscribed)
00266         local_elevation_map.data = std::vector<int16_t>(elevation_map_meta.width * elevation_map_meta.height,(int16_t)-elevation_map_meta.zero_elevation);
00267 
00268     unsigned int size = (unsigned int)pointcloud2_map_pcl->points.size();
00269 
00270     // iterate trough all points
00271     for (unsigned int k = 0; k < size; ++k)
00272     {
00273         const pcl::PointXYZ& pt_cloud = pointcloud2_map_pcl->points[k];
00274 
00275         double measurement_distance = pointcloud2_sensor_pcl->points[k].z;
00276 
00277         // check for invalid measurements
00278         if (isnan(pt_cloud.x) || isnan(pt_cloud.y) || isnan(pt_cloud.z))
00279             continue;
00280 
00281         // check max distance (manhatten norm)
00282         if(max_observable_distance < measurement_distance)
00283             continue;
00284 
00285         // check min/max height
00286         if(elevation_map_meta.min_elevation+local_map_transform.getOrigin().z() > pt_cloud.z || elevation_map_meta.max_elevation+local_map_transform.getOrigin().z() < pt_cloud.z)
00287             continue;
00288 
00289         // allign grid points
00290         Eigen::Vector2f index_world(pt_cloud.x, pt_cloud.y);
00291         Eigen::Vector2f index_map (world_map_transform.getC2Coords(index_world));
00292 
00293         unsigned int cell_index = MAP_IDX(elevation_map_meta.width, (int)round(index_map(0)), (int)round(index_map(1)));
00294 
00295         int16_t* pt_local_map = &local_elevation_map.data[cell_index];
00296         int16_t* pt_global_map = &global_elevation_map.data[cell_index];
00297         double*  pt_var = &cell_variance[cell_index];
00298 
00299 
00300         if(local_map_is_subscribed)
00301         {
00302             // elevation in current cell in meter
00303             double cell_elevation = elevation_map_meta.resolution_z*(*pt_local_map-elevation_map_meta.zero_elevation);
00304 
00305             // store maximum of each cell
00306             if(pt_cloud.z > cell_elevation)
00307                 *pt_local_map = (int16_t)(round(pt_cloud.z/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
00308 
00309             // filter each cell localy
00310 //            double measurement_variance = sensor_variance*(measurement_distance*measurement_distance);
00311 //            if(*pt_local_map == (int16_t)-elevation_map_meta.zero_elevation)
00312 //            {
00313 //                // unknown cell -> use current measurement
00314 //                *pt_local_map = (int16_t)(round(pt_cloud.z/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
00315 //                *pt_var = measurement_variance;
00316 //            }
00317 //            else
00318 //            {
00319 //                // fuse cell_elevation with measurement
00320 //                *pt_local_map = (int16_t) (round(((measurement_variance * cell_elevation + *pt_var * pt_cloud.z)/(*pt_var + measurement_variance))/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
00321 //                *pt_var = (measurement_variance * *pt_var)/(measurement_variance + *pt_var);
00322 //            }
00323         }
00324 
00325         if(publish_poseupdate || global_map_is_subscribed)
00326         {
00327             // fuse new measurements with existing map
00328 
00329             // elevation in current cell in meter
00330             double cell_elevation = elevation_map_meta.resolution_z*(*pt_global_map-elevation_map_meta.zero_elevation);
00331 
00332             // measurement variance
00333             double measurement_variance = sensor_variance*(measurement_distance*measurement_distance);
00334 
00335             // mahalanobis distance
00336             double mahalanobis_distance = sqrt((pt_cloud.z - cell_elevation)*(pt_cloud.z - cell_elevation)/(measurement_variance*measurement_variance));
00337 
00338             if(pt_cloud.z > cell_elevation && (mahalanobis_distance > 5.0))
00339             {
00340                 *pt_global_map = (int16_t)(round(pt_cloud.z/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
00341                 *pt_var = measurement_variance;
00342                 continue;
00343             }
00344 
00345             if((pt_cloud.z < cell_elevation) && (mahalanobis_distance > 5.0))
00346             {
00347                 *pt_global_map = (int16_t) (round(((measurement_variance * cell_elevation + *pt_var * pt_cloud.z)/(*pt_var + measurement_variance))/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
00348                 //*pt_var = (measurement_variance * *pt_var)/(measurement_variance + *pt_var);
00349                 *pt_var = measurement_variance;
00350                 continue;
00351             }
00352 
00353             *pt_global_map = (int16_t) (round(((measurement_variance * cell_elevation + *pt_var * pt_cloud.z)/(*pt_var + measurement_variance))/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
00354             *pt_var = (measurement_variance * *pt_var)/(measurement_variance + *pt_var);
00355         }
00356     }
00357 
00358 
00359     if(local_map_is_subscribed)
00360     {
00361         // set the header information on the map
00362         local_elevation_map.header.stamp = pointcloud2_sensor_msg->header.stamp;
00363         local_elevation_map.header.frame_id = map_frame_id;
00364 
00365         pub_local_map.publish(local_elevation_map);
00366     }
00367 
00368     if(global_map_is_subscribed)
00369     {
00370         // set the header information on the map
00371         global_elevation_map.header.stamp = pointcloud2_sensor_msg->header.stamp;
00372         global_elevation_map.header.frame_id = map_frame_id;
00373 
00374         pub_global_map.publish(global_elevation_map);
00375     }
00376 
00377 }
00378 
00379 }


hector_elevation_mapping
Author(s): Thorsten Graber
autogenerated on Mon Oct 6 2014 00:26:57