00001
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <octomap_server/OctomapServer.h>
00039
00040 namespace octomap{
00041
00042 OctomapServer::OctomapServer()
00043 : m_nh(),
00044 m_octoMap(0.05),
00045 m_maxRange(-1.0),
00046 m_frameId("/map"),
00047 m_useHeightMap(true),
00048 m_colorFactor(0.8),
00049 m_visMinZ(-1.0*std::numeric_limits<double>::max()), m_visMaxZ(std::numeric_limits<double>::max())
00050 {
00051 ros::NodeHandle private_nh("~");
00052 private_nh.param("frame_id", m_frameId, m_frameId);
00053 private_nh.param("height_map", m_useHeightMap, m_useHeightMap);
00054 private_nh.param("color_factor", m_colorFactor, m_colorFactor);
00055 private_nh.param("vis_min_z", m_visMinZ, m_visMinZ);
00056 private_nh.param("vis_max_z", m_visMaxZ, m_visMaxZ);
00057
00058 double res = 0.05;
00059 private_nh.param("resolution", res, res);
00060 m_octoMap.octree.setResolution(res);
00061
00062 private_nh.param("sensor_model/max_range", m_maxRange, m_maxRange);
00063
00064 double probHit = 0.7;
00065 double probMiss = 0.4;
00066 double thresMin = 0.12;
00067 double thresMax = 0.97;
00068 private_nh.param("sensor_model/hit", probHit, probHit);
00069 private_nh.param("sensor_model/miss", probMiss, probMiss);
00070 private_nh.param("sensor_model/min", thresMin, thresMin);
00071 private_nh.param("sensor_model/max", thresMax, thresMax);
00072 m_octoMap.octree.setProbHit(probHit);
00073 m_octoMap.octree.setProbMiss(probMiss);
00074 m_octoMap.octree.setClampingThresMin(thresMin);
00075 m_octoMap.octree.setClampingThresMax(thresMax);
00076
00077 double r, g, b, a;
00078 private_nh.param("color/r", r, 0.0);
00079 private_nh.param("color/g", g, 0.0);
00080 private_nh.param("color/b", b, 1.0);
00081 private_nh.param("color/a", a, 1.0);
00082 m_color.r = r;
00083 m_color.g = g;
00084 m_color.b = b;
00085 m_color.a = a;
00086
00087
00088 m_markerPub = m_nh.advertise<visualization_msgs::MarkerArray>("occupied_cells_vis_array", 1,
00089 boost::bind(&OctomapServer::connectCallback, this, _1), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), true);
00090 m_binaryMapPub = m_nh.advertise<octomap_ros::OctomapBinary>("octomap_binary", 1,
00091 boost::bind(&OctomapServer::connectCallback, this, _1), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), true);
00092 m_pointCloudPub = m_nh.advertise<sensor_msgs::PointCloud2>("octomap_point_cloud_centers", 1,
00093 boost::bind(&OctomapServer::connectCallback, this, _1), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), true);
00094 m_collisionObjectPub = m_nh.advertise<mapping_msgs::CollisionObject>("octomap_collision_object", 1,
00095 boost::bind(&OctomapServer::connectCallback, this, _1), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), true);
00096
00097 m_service = m_nh.advertiseService("octomap_binary", &OctomapServer::serviceCallback, this);
00098
00099
00100 m_pointCloudSub = new message_filters::Subscriber<sensor_msgs::PointCloud2> (m_nh, "cloud_in", 5);
00101 m_tfPointCloudSub = new tf::MessageFilter<sensor_msgs::PointCloud2> (*m_pointCloudSub, m_tfListener, m_frameId, 5);
00102 m_tfPointCloudSub->registerCallback(boost::bind(&OctomapServer::insertCloudCallback, this, _1));
00103
00104
00105 }
00106
00107 OctomapServer::~OctomapServer(){
00108 delete m_tfPointCloudSub;
00109 delete m_pointCloudSub;
00110
00111 }
00112
00113 bool OctomapServer::loadInitialMap(const std::string& filename) {
00114 if (!filename.empty()){
00115 m_octoMap.octree.readBinary(filename);
00116 ROS_INFO("Octomap file %s loaded (%zu nodes).", filename.c_str(),m_octoMap.octree.size());
00117
00118 if (m_binaryMapPub.getNumSubscribers() > 0)
00119 publishMap();
00120 if (m_markerPub.getNumSubscribers() > 0)
00121 publishMarkers();
00122 if (m_pointCloudPub.getNumSubscribers() > 0)
00123 publishPointCloud();
00124 if (m_collisionObjectPub.getNumSubscribers() > 0)
00125 publishCollisionObject();
00126 }
00127 return true;
00128 }
00129
00130
00131 void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud){
00132 ros::WallTime startTime = ros::WallTime::now();
00133
00134 tf::StampedTransform trans;
00135 try {
00136 m_tfListener.waitForTransform(m_frameId, cloud->header.frame_id, cloud->header.stamp, ros::Duration(0.5));
00137 m_tfListener.lookupTransform (m_frameId, cloud->header.frame_id, cloud->header.stamp, trans);
00138 } catch(tf::TransformException& ex){
00139 ROS_WARN_STREAM( "Transform error from " << cloud->header.frame_id
00140 << " to " << m_frameId << ", quitting callback");
00141 return;
00142 }
00143
00144 Eigen::Matrix4f eigen_transform;
00145 sensor_msgs::PointCloud2 transformed_cloud;
00146 pcl_ros::transformAsMatrix (trans, eigen_transform);
00147 pcl_ros::transformPointCloud(eigen_transform, *cloud, transformed_cloud);
00148
00149 geometry_msgs::Point origin;
00150 tf::pointTFToMsg(trans.getOrigin(), origin);
00151 m_octoMap.insertScan(transformed_cloud, origin, m_maxRange, true, true);
00152 m_octoMap.octree.updateInnerOccupancy();
00153
00154 if (m_binaryMapPub.getNumSubscribers() > 0)
00155 publishMap(cloud->header.stamp);
00156 if (m_markerPub.getNumSubscribers() > 0)
00157 publishMarkers(cloud->header.stamp);
00158 if (m_pointCloudPub.getNumSubscribers() > 0)
00159 publishPointCloud(cloud->header.stamp);
00160 if (m_collisionObjectPub.getNumSubscribers() > 0)
00161 publishCollisionObject(cloud->header.stamp);
00162
00163 double total_elapsed = (ros::WallTime::now() - startTime).toSec();
00164 ROS_DEBUG("Pointcloud insertion in OctomapServer done (%d pts, %f sec)", int(cloud->height*cloud->width), total_elapsed);
00165
00166
00167 }
00168
00169 void OctomapServer::connectCallback(const ros::SingleSubscriberPublisher& pub){
00170 ROS_INFO("New subscriber on topic %s to OctomapServer connected", pub.getTopic().c_str());
00171
00172
00173 publishMap();
00174 publishMarkers();
00175 publishPointCloud();
00176 publishCollisionObject();
00177 }
00178
00179 void OctomapServer::publishCollisionObject(const ros::Time& rostime){
00180 mapping_msgs::CollisionObject collisionObject;
00181
00182 collisionObject.header.frame_id = m_frameId;
00183 collisionObject.header.stamp = rostime;
00184 collisionObject.id = "map";
00185
00186 geometric_shapes_msgs::Shape shape;
00187 shape.type = geometric_shapes_msgs::Shape::BOX;
00188 shape.dimensions.resize(3);
00189
00190 geometry_msgs::Pose pose;
00191 pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
00192
00193 for (OcTreeROS::OcTreeType::iterator it = m_octoMap.octree.begin(),
00194 end = m_octoMap.octree.end(); it != end; ++it)
00195 {
00196 if (m_octoMap.octree.isNodeOccupied(*it)){
00197 double z = it.getZ();
00198 if (z > m_visMinZ && z < m_visMaxZ)
00199 {
00200 double size = it.getSize();
00201 shape.dimensions[0] = shape.dimensions[1] = shape.dimensions[2] = size;
00202 collisionObject.shapes.push_back(shape);
00203 pose.position.x = it.getX();
00204 pose.position.y = it.getY();
00205 pose.position.z = z;
00206 collisionObject.poses.push_back(pose);
00207 }
00208 }
00209 }
00210
00211 m_collisionObjectPub.publish(collisionObject);
00212
00213
00214 }
00215
00216
00217 void OctomapServer::publishMarkers(const ros::Time& rostime){
00218 if (m_octoMap.octree.size() <= 1)
00219 return;
00220
00221 double x, y, minZ, maxZ;
00222 m_octoMap.octree.getMetricMin(x, y, minZ);
00223 m_octoMap.octree.getMetricMax(y, y, maxZ);
00224
00225 visualization_msgs::MarkerArray occupiedNodesVis;
00226
00227 occupiedNodesVis.markers.resize(m_octoMap.octree.getTreeDepth());
00228 double lowestRes = m_octoMap.octree.getResolution();
00229
00230
00231 unsigned numVoxels = 0;
00232 for (OcTreeROS::OcTreeType::iterator it = m_octoMap.octree.begin(),
00233 end = m_octoMap.octree.end(); it != end; ++it)
00234 {
00235 if (m_octoMap.octree.isNodeOccupied(*it)){
00236 double z = it.getZ();
00237 if (z > m_visMinZ && z < m_visMaxZ)
00238 {
00239
00240 int idx = int(log2(it.getSize() / lowestRes) +0.5);
00241 assert (idx >= 0 && unsigned(idx) < occupiedNodesVis.markers.size());
00242 geometry_msgs::Point cubeCenter;
00243 cubeCenter.x = it.getX();
00244 cubeCenter.y = it.getY();
00245 cubeCenter.z = z;
00246
00247 occupiedNodesVis.markers[idx].points.push_back(cubeCenter);
00248 if (m_useHeightMap){
00249 double h = (1.0 - std::min(std::max((cubeCenter.z-minZ)/ (maxZ - minZ), 0.0), 1.0)) *m_colorFactor;
00250 occupiedNodesVis.markers[idx].colors.push_back(heightMapColor(h));
00251 }
00252 numVoxels++;
00253 }
00254
00255 }
00256
00257 }
00258
00259 for (unsigned i= 0; i < occupiedNodesVis.markers.size(); ++i){
00260 double size = lowestRes * pow(2,i);
00261
00262 occupiedNodesVis.markers[i].header.frame_id = m_frameId;
00263 occupiedNodesVis.markers[i].header.stamp = rostime;
00264 occupiedNodesVis.markers[i].ns = "map";
00265 occupiedNodesVis.markers[i].id = i;
00266 occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
00267 occupiedNodesVis.markers[i].scale.x = size;
00268 occupiedNodesVis.markers[i].scale.y = size;
00269 occupiedNodesVis.markers[i].scale.z = size;
00270 occupiedNodesVis.markers[i].color = m_color;
00271
00272
00273 if (occupiedNodesVis.markers[i].points.size() > 0)
00274 occupiedNodesVis.markers[i].action = visualization_msgs::Marker::ADD;
00275 else
00276 occupiedNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
00277 }
00278
00279 m_markerPub.publish(occupiedNodesVis);
00280 ROS_DEBUG("%d occupied voxels visualized.", numVoxels);
00281 }
00282
00283 void OctomapServer::publishPointCloud(const ros::Time& rostime){
00284 if (m_octoMap.octree.size() <= 1)
00285 return;
00286
00287 pcl::PointCloud<pcl::PointXYZ> pclCloud;
00288 for (OcTreeROS::OcTreeType::iterator it = m_octoMap.octree.begin(),
00289 end = m_octoMap.octree.end(); it != end; ++it)
00290 {
00291 if (m_octoMap.octree.isNodeOccupied(*it)){
00292 double z = it.getZ();
00293 if (z > m_visMinZ && z < m_visMaxZ)
00294 pclCloud.push_back(pcl::PointXYZ(it.getX(), it.getY(), z));
00295 }
00296 }
00297
00298 sensor_msgs::PointCloud2 cloud;
00299 pcl::toROSMsg (pclCloud, cloud);
00300 cloud.header.frame_id = m_frameId;
00301 cloud.header.stamp = rostime;
00302 m_pointCloudPub.publish(cloud);
00303 }
00304
00305 bool OctomapServer::serviceCallback(octomap_ros::GetOctomap::Request &req,
00306 octomap_ros::GetOctomap::Response &res)
00307 {
00308 ROS_INFO("Sending map data on service request");
00309 res.map.header.frame_id = m_frameId;
00310 res.map.header.stamp = ros::Time::now();
00311 octomap::octomapMapToMsgData(m_octoMap.octree, res.map.data);
00312
00313 return true;
00314 }
00315
00316 void OctomapServer::publishMap(const ros::Time& rostime){
00317
00318 octomap_ros::OctomapBinary map;
00319 map.header.frame_id = m_frameId;
00320 map.header.stamp = rostime;
00321
00322 octomap::octomapMapToMsgData(m_octoMap.octree, map.data);
00323
00324 m_binaryMapPub.publish(map);
00325 }
00326
00327 std_msgs::ColorRGBA OctomapServer::heightMapColor(double h) const {
00328
00329 std_msgs::ColorRGBA color;
00330 color.a = 1.0;
00331
00332
00333 double s = 1.0;
00334 double v = 1.0;
00335
00336 h -= floor(h);
00337 h *= 6;
00338 int i;
00339 double m, n, f;
00340
00341 i = floor(h);
00342 f = h - i;
00343 if (!(i & 1))
00344 f = 1 - f;
00345 m = v * (1 - s);
00346 n = v * (1 - s * f);
00347
00348 switch (i) {
00349 case 6:
00350 case 0:
00351 color.r = v; color.g = n; color.b = m;
00352 break;
00353 case 1:
00354 color.r = n; color.g = v; color.b = m;
00355 break;
00356 case 2:
00357 color.r = m; color.g = v; color.b = n;
00358 break;
00359 case 3:
00360 color.r = m; color.g = n; color.b = v;
00361 break;
00362 case 4:
00363 color.r = n; color.g = m; color.b = v;
00364 break;
00365 case 5:
00366 color.r = v; color.g = m; color.b = n;
00367 break;
00368 default:
00369 color.r = 1; color.g = 0.5; color.b = 0.5;
00370 break;
00371 }
00372
00373 return color;
00374 }
00375 }
00376
00377
00378
