collision_map_self_occ.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00037 #include <ros/ros.h>
00038 #include <std_srvs/Empty.h>
00039 #include <sensor_msgs/PointCloud.h>
00040 #include <arm_navigation_msgs/CollisionMap.h>
00041 #include <tf/transform_listener.h>
00042 #include <tf/message_filter.h>
00043 #include <message_filters/subscriber.h>
00044 //#include <robot_self_filter/self_see_filter.h>
00045 #include <boost/thread/mutex.hpp>
00046 #include <boost/bind.hpp>
00047 #include <vector>
00048 #include <algorithm>
00049 #include <set>
00050 #include <iterator>
00051 #include <cstdlib>
00052 #include <arm_navigation_msgs/MakeStaticCollisionMapAction.h>
00053 #include <actionlib/server/simple_action_server.h>
00054 
00055 struct CloudInfo 
00056 {
00057   CloudInfo(): cloud_name_(""), frame_subsample_(1.0),
00058                point_subsample_(1.0), sensor_frame_(""), counter_(0), 
00059                dynamic_buffer_size_(1), static_buffer_size_(1), 
00060                dynamic_buffer_duration_(0), static_buffer_duration_(0),
00061                dynamic_publish_(true), static_publish_(true), 
00062                dynamic_until_static_publish_(true)
00063   {
00064   };
00065   std::string cloud_name_;
00066   int frame_subsample_; //take every nth frame (1 is no discards)
00067   int point_subsample_; //take every nth point from the frames you do process
00068   std::string sensor_frame_;
00069   unsigned int counter_;
00070   
00071   unsigned int dynamic_buffer_size_, static_buffer_size_;
00072   ros::Duration dynamic_buffer_duration_, static_buffer_duration_;
00073 
00074   // Settings for publishing map data on the standard publisher.
00075   // Note that if static_publish is set to false, static map data WILL still be published on the dedicated
00076   // static map topic when a map is acquired, but it WON'T publish on the main publisher, which publishes
00077   // a union of all the maps in the node's buffer.  If dynamic_until_static_publish is set to true then 
00078   // the dynamic map will be published on the main topic until a static map from that source is created, 
00079   // at which point dynamic behavior will revert to that specified by dynamic_publish;
00080   bool dynamic_publish_, static_publish_, dynamic_until_static_publish_;  
00081 };
00082 
00083 class CollisionMapperOcc
00084 {
00085 public:
00086   
00087   CollisionMapperOcc(void): root_handle_(""), making_static_collision_map_(false), disregard_first_message_(false)
00088   {
00089     static_map_goal_ = NULL;
00090     
00091     ros::NodeHandle priv("~");
00092     
00093     // a frame that does not move with the robot
00094     priv.param<std::string>("fixed_frame", fixedFrame_, "odom");
00095 
00096     // a frame that moves with the robot
00097     priv.param<std::string>("robot_frame", robotFrame_, "base_link");
00098 
00099     // bounds of collision map in robot frame
00100     priv.param<double>("dimension_x", bi_.dimensionX, 1.0);
00101     priv.param<double>("dimension_y", bi_.dimensionY, 1.5);
00102     priv.param<double>("dimension_z", bi_.dimensionZ, 2.0);
00103 
00104     // origin of collision map in the robot frame
00105     priv.param<double>("origin_x", bi_.originX, 1.1);
00106     priv.param<double>("origin_y", bi_.originY, 0.0);
00107     priv.param<double>("origin_z", bi_.originZ, 0.0);
00108 
00109     // sensor frame
00110     //priv.param<std::string>("sensor_frame", bi_.sensor_frame, std::string());
00111         
00112     // resolution
00113     priv.param<double>("resolution", bi_.resolution, 0.015);
00114         
00115     ROS_INFO("Maintaining occlusion map in frame '%s', with origin at (%f, %f, %f) and dimension (%f, %f, %f), resolution of %f; "
00116              "sensor is in frame '%s', fixed fame is '%s'.",
00117              robotFrame_.c_str(), bi_.originX, bi_.originY, bi_.originZ, bi_.dimensionX, bi_.dimensionY, bi_.dimensionZ, 
00118              bi_.resolution, bi_.sensor_frame.c_str(), fixedFrame_.c_str());
00119         
00120     priv.param<bool>("publish_occlusion", publishOcclusion_, false);
00121     priv.param<bool>("publish_static_over_dynamic_map", publish_over_dynamic_map_, false);
00122 
00123 
00124     // compute some useful values
00125     bi_.real_minX = -bi_.dimensionX + bi_.originX;
00126     bi_.real_maxX =  bi_.dimensionX + bi_.originX;
00127     bi_.real_minY = -bi_.dimensionY + bi_.originY;
00128     bi_.real_maxY =  bi_.dimensionY + bi_.originY;
00129     bi_.real_minZ = -bi_.dimensionZ + bi_.originZ;
00130     bi_.real_maxZ =  bi_.dimensionZ + bi_.originZ;      
00131 
00132     //self_filter_ = new filters::SelfFilter<sensor_msgs::PointCloud>(priv);
00133 
00134     // advertise our topics: full map and updates
00135     cmapPublisher_ = root_handle_.advertise<arm_navigation_msgs::CollisionMap>("collision_map_occ", 1, true);
00136     cmapUpdPublisher_ = root_handle_.advertise<arm_navigation_msgs::CollisionMap>("collision_map_occ_update", 1);
00137     static_map_publisher_ = root_handle_.advertise<arm_navigation_msgs::CollisionMap>("collision_map_occ_static", 1);
00138         
00139     if(!priv.hasParam("cloud_sources")) {
00140       ROS_WARN("No links specified for self filtering.");
00141     } else {
00142       XmlRpc::XmlRpcValue cloud_vals;
00143       priv.getParam("cloud_sources", cloud_vals);
00144       
00145       if(cloud_vals.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00146         ROS_WARN("Cloud sources needs to be an array");
00147       } else {
00148         for(int i = 0; i < cloud_vals.size(); i++) {
00149           CloudInfo cps;
00150           if(cloud_vals[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) {
00151             ROS_WARN("Cloud source entry %d is not a structure.  Stopping processing of cloud sources",i);
00152             break;
00153           }
00154           if(!cloud_vals[i].hasMember("name")) {
00155             ROS_WARN("Cloud sources entry %d has no name.  Stopping processing of cloud sources",i);
00156             break;
00157           } 
00158           if(!cloud_vals[i].hasMember("sensor_frame")) {
00159             ROS_WARN("Cloud sources entry %d has no sensor_frame.  Stopping processing of cloud sources",i);
00160             break;
00161           } 
00162           cps.cloud_name_ = std::string(cloud_vals[i]["name"]);
00163           cps.sensor_frame_ = std::string(cloud_vals[i]["sensor_frame"]);
00164           if(!cloud_vals[i].hasMember("frame_subsample")) {
00165             ROS_DEBUG("Cloud sources entry %d has no frame subsample.  Assuming default subsample of %g",i,1.0);
00166             cps.frame_subsample_ = 1.0;;
00167           } else {
00168             cps.frame_subsample_ = cloud_vals[i]["frame_subsample"];
00169           }
00170           if(!cloud_vals[i].hasMember("point_subsample")) {
00171             cps.point_subsample_ = 1.0;;
00172           } else {
00173             cps.point_subsample_ = cloud_vals[i]["point_subsample"];
00174           }
00175         
00176           if(cloud_vals[i].hasMember("dynamic_buffer_size"))
00177             cps.dynamic_buffer_size_ = static_cast<unsigned int>(int(cloud_vals[i]["dynamic_buffer_size"]));
00178         
00179           if(cloud_vals[i].hasMember("static_buffer_size"))
00180             cps.static_buffer_size_ = static_cast<unsigned int>(int(cloud_vals[i]["static_buffer_size"]));
00181 
00182           if(cloud_vals[i].hasMember("dynamic_buffer_duration"))
00183             cps.dynamic_buffer_duration_ = ros::Duration(int(cloud_vals[i]["dynamic_buffer_duration"]));
00184 
00185           if(cloud_vals[i].hasMember("static_buffer_duration"))
00186             cps.static_buffer_duration_ = ros::Duration(int(cloud_vals[i]["static_buffer_duration"]));
00187 
00188           if(cloud_vals[i].hasMember("dynamic_publish"))
00189             cps.dynamic_publish_ = cloud_vals[i]["dynamic_publish"];
00190 
00191           if(cloud_vals[i].hasMember("static_publish"))
00192             cps.static_publish_ = cloud_vals[i]["static_publish"];
00193 
00194           if(cloud_vals[i].hasMember("dynamic_until_static_publish")) {
00195             cps.dynamic_until_static_publish_ = cloud_vals[i]["dynamic_until_static_publish"];
00196           }
00197 
00198           if(cloud_source_map_.find(cps.cloud_name_) != cloud_source_map_.end()) {
00199             ROS_WARN_STREAM("Already have a cloud defined with name " << cps.cloud_name_);
00200           } else {
00201             cloud_source_map_[cps.cloud_name_] = cps;
00202             mn_cloud_tf_sub_vector_.push_back(new message_filters::Subscriber<sensor_msgs::PointCloud>(root_handle_, cps.cloud_name_, 1));
00203             mn_cloud_tf_fil_vector_.push_back(new tf::MessageFilter<sensor_msgs::PointCloud>(*(mn_cloud_tf_sub_vector_.back()), tf_, "", 1));
00204             mn_cloud_tf_fil_vector_.back()->registerCallback(boost::bind(&CollisionMapperOcc::cloudCallback, this, _1, cps.cloud_name_));
00205             // if (publishOcclusion_) {
00206             //   std::string name = std::string("collision_map_occ_occlusion_")+cps.cloud_name_;
00207             //   occPublisherMap_[cps.cloud_name_] = root_handle_.advertise<arm_navigation_msgs::CollisionMap>(name, 1);
00208             // }
00209             static_map_published_[cps.cloud_name_] = false;
00210             ROS_INFO_STREAM("Source name " << cps.cloud_name_);
00211           }
00212         }
00213       }
00214     }
00215     // create a message notifier (and enable subscription) for both the full map and for the updates
00216     //mnCloud_ = new tf::MessageNotifier<sensor_msgs::PointCloud>(tf_, boost::bind(&CollisionMapperOcc::cloudCallback, this, _1), "cloud_in", "", 1);
00217     //mnCloudIncremental_ = new tf::MessageNotifier<sensor_msgs::PointCloud>(tf_, boost::bind(&CollisionMapperOcc::cloudIncrementalCallback, this, _1), "cloud_incremental_in", "", 1);
00218     
00219     // configure the self mask and the message notifier
00220     //sm_ = self_filter_->getSelfMask();
00221     std::vector<std::string> frames;
00222     //sm_->getLinkNames(frames);
00223     if (std::find(frames.begin(), frames.end(), robotFrame_) == frames.end()) {
00224       frames.push_back(robotFrame_);
00225     }
00226     for(unsigned int i = 0; i < mn_cloud_tf_fil_vector_.size(); i++) {
00227       mn_cloud_tf_fil_vector_[i]->setTargetFrames(frames);
00228     }
00229     //mnCloudIncremental_->setTargetFrame(frames);
00230     resetService_ = priv.advertiseService("reset", &CollisionMapperOcc::reset, this);
00231 
00232     action_server_.reset(new actionlib::SimpleActionServer<arm_navigation_msgs::MakeStaticCollisionMapAction>(root_handle_, "make_static_collision_map", 
00233                                                                                                                     boost::bind(&CollisionMapperOcc::makeStaticCollisionMap, this, _1)));
00234   }
00235   
00236   ~CollisionMapperOcc(void)
00237   {
00238 
00239     for(std::map<std::string, std::list<StampedCMap*> >::iterator it = currentMaps_.begin(); it != currentMaps_.end(); it++)
00240     {
00241       for(std::list<StampedCMap*>::iterator itbuff = it->second.begin(); itbuff != it->second.end(); itbuff++)
00242       {
00243         delete (*itbuff);
00244       }
00245     }
00246 
00247     for(std::map<std::string, StampedCMap*>::iterator it = tempMaps_.begin();
00248         it != tempMaps_.end();
00249         it++) {
00250       delete it->second;
00251     }
00252 
00253     for(unsigned int i = 0; i < mn_cloud_tf_sub_vector_.size(); i++) {
00254       delete mn_cloud_tf_sub_vector_[i];
00255     }
00256     for(unsigned int i = 0; i < mn_cloud_tf_fil_vector_.size(); i++) {
00257       delete mn_cloud_tf_fil_vector_[i];
00258     }
00259     //delete self_filter_;
00260     if(static_map_goal_) delete static_map_goal_;
00261     //delete mnCloudIncremental_;
00262   }
00263 
00264   void run(void)
00265   {
00266     //if (bi_.sensor_frame.empty())
00267     //  ROS_ERROR("No sensor frame specified. Cannot perform raytracing");
00268     //else
00269     ros::spin();
00270   }
00271         
00272 private:
00273   
00274   struct CollisionPoint
00275   {
00276     CollisionPoint(void) {}
00277     CollisionPoint(int _x, int _y, int _z) : x(_x), y(_y), z(_z) {}
00278         
00279     int x, y, z;
00280   };
00281 
00282   // define an order on points
00283   struct CollisionPointOrder
00284   {
00285     bool operator()(const CollisionPoint &a, const CollisionPoint &b) const
00286     {
00287       if (a.x < b.x)
00288         return true;
00289       if (a.x > b.x)
00290         return false;
00291       if (a.y < b.y)
00292         return true;
00293       if (a.y > b.y)
00294         return false;
00295       return a.z < b.z;
00296     }
00297   };
00298     
00299   typedef std::set<CollisionPoint, CollisionPointOrder> CMap;
00300 
00301   struct StampedCMap
00302   {
00303     std::string frame_id;
00304     ros::Time stamp;
00305     CMap cmap;
00306   };
00307   
00308 
00309   // parameters & precomputed values for the box that represents the collision map
00310   // around the robot
00311   struct BoxInfo
00312   {    
00313     double dimensionX, dimensionY, dimensionZ;
00314     double originX, originY, originZ;
00315     std::string sensor_frame;
00316     double resolution;
00317     double real_minX, real_minY, real_minZ;
00318     double real_maxX, real_maxY, real_maxZ;
00319   };
00320                         
00321   void cloudIncrementalCallback(const sensor_msgs::PointCloudConstPtr &cloud)
00322   {
00323     if (!mapProcessing_.try_lock())
00324       return;
00325 
00326     ros::WallTime tm = ros::WallTime::now();
00327                 
00328     ROS_DEBUG("Got pointcloud update that is %f seconds old", (ros::Time::now() - cloud->header.stamp).toSec());
00329         
00330     sensor_msgs::PointCloud out;
00331     //    if(point_subsample_ > 1) { 
00332     //       sensor_msgs::PointCloud sub;
00333     //       sub.header = (*cloud).header;
00334     //       for(unsigned int i = 0; i < (*cloud).points.size(); i += point_subsample_) {
00335     //         sub.points.push_back((*cloud).points[i]);
00336     //       }
00337     //       tf_.transformPointCloud(robotFrame_, sub, out);
00338     //     } else {
00339     // transform the pointcloud to the robot frame
00340     // since we need the points in this frame (around the robot)
00341     // to compute the collision map
00342     tf_.transformPointCloud(robotFrame_, *cloud, out);
00343 
00344 
00345     CMap obstacles;
00346     constructCollisionMap(out, obstacles);
00347 
00348     CMap diff;
00349     //set_difference(obstacles.begin(), obstacles.end(), currentMap_.begin(), currentMap_.end(),
00350     //               std::inserter(diff, diff.begin()), CollisionPointOrder());
00351     mapProcessing_.unlock();
00352         
00353     if (!diff.empty())
00354       publishCollisionMap(diff, out.header.frame_id, out.header.stamp, cmapUpdPublisher_);
00355   }
00356 
00357   void subsampleCloudPoints(const sensor_msgs::PointCloud &msg, sensor_msgs::PointCloud& myMsg, int subsampleNum) {
00358     myMsg.points.clear();
00359     myMsg.header = msg.header;
00360     
00361     //myMsg.channels.clear();        
00362     for(int i = 0; i < (int)msg.points.size(); i += subsampleNum) {
00363       myMsg.points.push_back(msg.points[i]);
00364       //if((*msg).channels.size() < i) {
00365       //  myMsg.channels.push_back((*msg).channels[i]);
00366       //}
00367     }
00368   }
00369 
00370   void updateBuffer(std::list<StampedCMap*> &buffer, const unsigned int buffer_size, const ros::Duration buffer_duration, const std::string sensor_frame)
00371   {
00372     if(buffer_size > 1)
00373     {
00374       while(buffer.size() > buffer_size)
00375       {
00376         ROS_DEBUG_STREAM("Deleting old map in frame " << sensor_frame << " total buffer size: " << buffer.size());
00377         delete buffer.back();
00378         buffer.pop_back();
00379       }
00380     }
00381 
00382     if(buffer_duration > ros::Duration(0))
00383     {
00384       ros::Time min_time = buffer.front()->stamp - buffer_duration;
00385 
00386       while(buffer.back()->stamp < min_time)
00387       {
00388         ROS_DEBUG_STREAM("Deleting old map in frame " << sensor_frame << " which is too old by: " << min_time - buffer.back()->stamp << " seconds" );
00389         delete buffer.back();
00390         buffer.pop_back();
00391       }
00392     }
00393 
00394     // Keep only most recent
00395     if(buffer_size <= 1 && buffer_duration <= ros::Duration(0))
00396     {
00397       ROS_DEBUG_STREAM("Keeping only most recent map in frame " << sensor_frame );
00398       while(buffer.size() > 1)
00399       {
00400         delete buffer.back();
00401         buffer.pop_back();
00402       }
00403     }
00404 
00405   }
00406 
00407   void composeMapUnion(std::map<std::string, std::list<StampedCMap*> > &maps, CMap &uni)
00408   {
00409     //composing the union of all the sensor maps
00410     for(std::map<std::string, std::list<StampedCMap*> >::iterator it = maps.begin(); it != maps.end(); it++)
00411     {
00412       int processed = 0;
00413       for(std::list<StampedCMap*>::iterator itbuff = it->second.begin(); itbuff != it->second.end(); itbuff++)
00414       {
00415         set_union(uni.begin(), uni.end(), (*itbuff)->cmap.begin(), (*itbuff)->cmap.end(), std::inserter(uni, uni.begin()), CollisionPointOrder());
00416         processed++;
00417       }
00418 
00419       ROS_DEBUG_STREAM("The buffer for sensor frame " << it->first << " had " << processed << " maps");
00420     }
00421 
00422   }
00423     
00424   void cloudCallback(const sensor_msgs::PointCloudConstPtr &cloud, const std::string topic_name)
00425   {
00426     CloudInfo settings =  cloud_source_map_[topic_name];
00427 
00428     if(!making_static_collision_map_ &&  !settings.dynamic_publish_ && (!settings.dynamic_until_static_publish_ || static_map_published_[topic_name])) {
00429       return;
00430     }
00431 
00432     //sensor_msgs::PointCloud sf_out;
00433     //self_filter_->updateWithSensorFrame(*cloud, sf_out, settings.sensor_frame_);
00434     //ROS_DEBUG_STREAM("Size before self filter " << (*cloud).points.size() << " after " << sf_out.points.size());
00435     
00436     ros::WallTime tm = ros::WallTime::now();
00437 
00438     sensor_msgs::PointCloud subCloud;
00439 
00440     ROS_DEBUG("Got pointcloud that is %f seconds old", (ros::Time::now() - cloud->header.stamp).toSec());
00441     ROS_DEBUG("Received %u data points.",(unsigned int)(*cloud).points.size ());
00442     subsampleCloudPoints(*cloud, subCloud, settings.point_subsample_);
00443     ROS_DEBUG("After subsampling we have %u data points.",(unsigned int)subCloud.points.size ());
00444 
00445 
00446     boost::recursive_mutex::scoped_lock lock(mapProcessing_);
00447 
00448     CMap obstacles;
00449 
00450     sensor_msgs::PointCloud transCloud;
00451     
00452     tf_.transformPointCloud(robotFrame_, subCloud, transCloud);
00453         
00454     // transform the pointcloud to the robot frame
00455     // since we need the points in this frame (around the robot)
00456     // to compute the collision map
00457     constructCollisionMap(transCloud, obstacles);
00458 
00459     if(making_static_collision_map_ && topic_name == static_map_goal_->cloud_source) {
00460       if(disregard_first_message_) {
00461         disregard_first_message_ = false;
00462       } else {
00463         //making new collision map for this message
00464         std::string map_name = topic_name+"_static_temp";
00465         StampedCMap* static_map;
00466         if(tempMaps_.find(map_name) != tempMaps_.end()) {
00467           static_map = tempMaps_[map_name];
00468         } else {
00469           static_map = new StampedCMap();
00470           static_map->frame_id = transCloud.header.frame_id;
00471           static_map->stamp = transCloud.header.stamp;
00472           tempMaps_[map_name] = static_map;
00473         } 
00474         updateMap(&static_map->cmap, obstacles, transCloud.header.frame_id, transCloud.header.stamp, settings.sensor_frame_, settings.cloud_name_);
00475         if(++cloud_count_ == static_map_goal_->number_of_clouds) {
00476 
00477             ROS_DEBUG("Publishing static map");
00478             publishCollisionMap(static_map->cmap, transCloud.header.frame_id, transCloud.header.stamp, static_map_publisher_);
00479 
00480           if(!settings.static_publish_)
00481           {
00482             delete static_map;
00483           }
00484           else
00485           {
00486             ROS_DEBUG("Saving static map for inclusion in future dynamic maps");
00487             currentMaps_[topic_name+"_static"].push_front(static_map);
00488 
00489             updateBuffer(currentMaps_[topic_name+"_static"], settings.static_buffer_size_, settings.static_buffer_duration_, topic_name+"_static");
00490 
00491             if(settings.dynamic_until_static_publish_) {
00492               //now if we are supposed to stop publishing dynamic maps get ride of the dynamic map for this topic
00493               std::list<StampedCMap*>& dyn_list = currentMaps_[topic_name+"_dynamic"];
00494               for(std::list<StampedCMap*>::iterator it = dyn_list.begin(); it != dyn_list.end(); it++) {
00495                 delete (*it);
00496               }
00497               currentMaps_.erase(topic_name+"_dynamic");
00498             }
00499 
00500             CMap uni;
00501             composeMapUnion(currentMaps_, uni);
00502 
00503             publishCollisionMap(uni, transCloud.header.frame_id, transCloud.header.stamp, cmapPublisher_);
00504           }
00505           
00506           tempMaps_.erase(map_name);
00507           making_static_collision_map_ = false;
00508           static_map_published_[topic_name] = true;
00509         }
00510       }
00511     }
00512 
00513     if(settings.dynamic_publish_ || (settings.dynamic_until_static_publish_ && !static_map_published_[topic_name]))
00514     {
00515 
00516       StampedCMap* current_map;
00517 
00518       // try to transform the previous map(s) (if it exists) to the new frame
00519       if(settings.dynamic_buffer_size_ == 1 && currentMaps_.find(topic_name+"_dynamic") != currentMaps_.end()) {
00520         current_map = currentMaps_[topic_name+"_dynamic"].front();
00521 //      if (!current_map->cmap.empty()) 
00522 //      {
00523 //      if (!transformMap(current_map->cmap, current_map->header, transCloud.header))  // Need to transform entire buffer, not just current_map!
00524 //      {
00525 //        current_map->cmap.clear();
00526 //      } 
00527 //      current_map->header = transCloud.header;
00528 //      }
00529       } else {
00530         current_map = new StampedCMap();
00531         current_map->frame_id = transCloud.header.frame_id;
00532         current_map->stamp = transCloud.header.stamp;
00533         currentMaps_[topic_name+"_dynamic"].push_front(current_map);
00534       }
00535 
00536       // update map
00537       updateMap(&current_map->cmap, obstacles, transCloud.header.frame_id, transCloud.header.stamp, settings.sensor_frame_, settings.cloud_name_);
00538       updateBuffer(currentMaps_[topic_name+"_dynamic"], settings.dynamic_buffer_size_, settings.dynamic_buffer_duration_, topic_name+"_dynamic");
00539 
00540       CMap uni;
00541       composeMapUnion(currentMaps_, uni);
00542 
00543       publishCollisionMap(uni, transCloud.header.frame_id, transCloud.header.stamp, cmapPublisher_);
00544 
00545     } 
00546 
00547     double sec = (ros::WallTime::now() - tm).toSec();
00548     ROS_DEBUG("Updated collision map took %g seconds",sec);
00549 
00550   }
00551 
00552 
00553 
00554   void updateMap(CMap* currentMap, CMap &obstacles, 
00555                  std::string &frame_id,
00556                  ros::Time &stamp,
00557                  std::string to_frame_id, 
00558                  std::string cloud_name)
00559   {
00560     if (currentMap->empty())
00561     {
00562       *currentMap = obstacles;
00563     }
00564     else
00565     {
00566       CMap diff;
00567             
00568       // find the points from the old map that are no longer visible
00569       set_difference(currentMap->begin(), currentMap->end(), obstacles.begin(), obstacles.end(),
00570                      std::inserter(diff, diff.begin()), CollisionPointOrder());
00571             
00572       // the current map will at least contain the new info
00573       *currentMap = obstacles;
00574 
00575       // find out which of these points are now occluded 
00576       //sm_->assumeFrame(header, to_frame_id, 0.05);
00577             
00578       // OpenMP need an int as the lookup variable, but for set,
00579       // this is not possible, so we copy to a vector
00580       int n = diff.size();
00581       std::vector<CollisionPoint> pts(n);
00582       std::copy(diff.begin(), diff.end(), pts.begin());
00583 
00584       //unsigned int count = 0;
00585 
00586       // // add points occluded by self
00587       // if (publishOcclusion_)
00588       // {
00589       //   CMap keep;
00590                 
00591       //   //#pragma omp parallel for
00592       //   for (int i = 0 ; i < n ; ++i)
00593       //   {
00594       //     tf::Vector3 p(((double)pts[i].x - 0.5) * bi_.resolution + bi_.originX,
00595       //                 ((double)pts[i].y - 0.5) * bi_.resolution + bi_.originY,
00596       //                 ((double)pts[i].z - 0.5) * bi_.resolution + bi_.originZ);
00597       //     if (sm_->getMaskIntersection(p) == robot_self_filter::SHADOW)
00598       //     {
00599       //       //#pragma omp critical
00600       //       {
00601       //         keep.insert(pts[i]);
00602       //         count++;
00603       //         currentMap->insert(pts[i]);
00604       //       }
00605       //     }
00606       //   }
00607       //   ROS_DEBUG("Occlusion topic %s num points %d", cloud_name.c_str(), count);
00608       //   publishCollisionMap(keep, header, occPublisherMap_[cloud_name]);
00609       // }
00610       // else
00611       // {
00612       //   //#pragma omp parallel for
00613       //   for (int i = 0 ; i < n ; ++i)
00614       //   {
00615       //     tf::Vector3 p(((double)pts[i].x - 0.5) * bi_.resolution + bi_.originX,
00616       //                 ((double)pts[i].y - 0.5) * bi_.resolution + bi_.originY,
00617       //                 ((double)pts[i].z - 0.5) * bi_.resolution + bi_.originZ);
00618       //     if (sm_->getMaskIntersection(p) == robot_self_filter::SHADOW)
00619       //     {
00620       //       //#pragma omp critical
00621       //       {
00622       //         currentMap->insert(pts[i]);
00623       //       }
00624       //     }          
00625       //   }
00626                 
00627       // }
00628             
00629     }
00630   }
00631     
00632   bool reset(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
00633   {
00634     boost::recursive_mutex::scoped_lock lock(mapProcessing_);
00635 
00636     for(std::map<std::string, std::list<StampedCMap*> >::iterator it = currentMaps_.begin(); it != currentMaps_.end(); it++)
00637     {
00638       for(std::list<StampedCMap*>::iterator itbuff = it->second.begin(); itbuff != it->second.end(); itbuff++)
00639       {
00640         delete (*itbuff);
00641       }
00642     }
00643 
00644     for(std::map<std::string, StampedCMap*>::iterator it = tempMaps_.begin();
00645         it != tempMaps_.end();
00646         it++) {
00647       delete it->second;
00648     }
00649 
00650     currentMaps_.clear();
00651     tempMaps_.clear();
00652 
00653     CMap uni;
00654     composeMapUnion(currentMaps_, uni);
00655 
00656     publishCollisionMap(uni, robotFrame_, ros::Time::now(), cmapPublisher_);
00657 
00658     return true;
00659   }
00660     
00661   bool transformMap(CMap &map, 
00662                     const std::string &from_frame_id,
00663                     const ros::Time &from_stamp,
00664                     const std::string &to_frame_id,
00665                     const ros::Time &to_stamp)
00666   {
00667     tf::StampedTransform transf;
00668     std::string err;
00669     ros::Time tm;
00670     ROS_DEBUG("Transforming old map");
00671     ROS_DEBUG_STREAM("From id: " << from_frame_id <<" at time " << from_stamp << " to id: " << to_frame_id << " at time " << to_stamp);
00672     if(from_frame_id == to_frame_id)
00673     {
00674       if(from_frame_id == fixedFrame_)
00675       {
00676         ROS_DEBUG("Nothing to transform, returning");
00677         return true;
00678       }
00679       try {
00680         tf_.lookupTransform(to_frame_id, to_stamp, from_frame_id, from_stamp, fixedFrame_, transf);
00681         ROS_DEBUG("Got transform!");
00682       } catch(...) {
00683         ROS_WARN("Unable to transform previous collision map into new frame");
00684         return false;
00685       }
00686     }
00687     else if (tf_.getLatestCommonTime(to_frame_id, from_frame_id, tm, &err) == tf::NO_ERROR) {
00688       try {
00689         tf_.lookupTransform(to_frame_id, tm, from_frame_id, tm, fixedFrame_, transf);
00690       } catch(...) {
00691         ROS_WARN("Unable to transform previous collision map into new frame");
00692         return false;
00693       }
00694     } else {
00695       ROS_WARN_STREAM("No common time between " << to_frame_id << " and " << from_frame_id);
00696     }
00697 
00698     float disp = transf.getOrigin().length();
00699     float angle = transf.getRotation().getAngle();
00700 
00701     ROS_DEBUG_STREAM("Old frame displaced by " << disp << " and angle " << angle << " to get new frame");
00702 
00703    
00704     // copy data to temporary location
00705     const int n = map.size();
00706     std::vector<CollisionPoint> pts(n);
00707     std::copy(map.begin(), map.end(), pts.begin());
00708     map.clear();
00709         
00710     //#pragma omp parallel for
00711     for (int i = 0 ; i < n ; ++i)
00712     {
00713       tf::Vector3 p(((double)pts[i].x - 0.5) * bi_.resolution + bi_.originX,
00714                   ((double)pts[i].y - 0.5) * bi_.resolution + bi_.originY,
00715                   ((double)pts[i].z - 0.5) * bi_.resolution + bi_.originZ);
00716       p = transf * p;
00717       if (p.x() > bi_.real_minX && p.x() < bi_.real_maxX && p.y() > bi_.real_minY && p.y() < bi_.real_maxY && p.z() > bi_.real_minZ && p.z() < bi_.real_maxZ)
00718       {
00719         CollisionPoint c((int)(0.5 + (p.x() - bi_.originX) / bi_.resolution),
00720                          (int)(0.5 + (p.y() - bi_.originY) / bi_.resolution),
00721                          (int)(0.5 + (p.z() - bi_.originZ) / bi_.resolution));
00722         //#pragma omp critical
00723         {
00724           map.insert(c);
00725         }
00726                 
00727       }
00728     }
00729         
00730     return true;
00731 
00732   }
00733 
00735   void constructCollisionMap(const sensor_msgs::PointCloud &cloud, CMap &map)
00736   {
00737     const unsigned int n = cloud.points.size();
00738     CollisionPoint c;
00739         
00740     for (unsigned int i = 0 ; i < n ; ++i)
00741     {
00742       const geometry_msgs::Point32 &p = cloud.points[i];
00743       if (p.x > bi_.real_minX && p.x < bi_.real_maxX && p.y > bi_.real_minY && p.y < bi_.real_maxY && p.z > bi_.real_minZ && p.z < bi_.real_maxZ)
00744       {
00745         c.x = (int)(0.5 + (p.x - bi_.originX) / bi_.resolution);
00746         c.y = (int)(0.5 + (p.y - bi_.originY) / bi_.resolution);
00747         c.z = (int)(0.5 + (p.z - bi_.originZ) / bi_.resolution);
00748         map.insert(c);
00749       }
00750     }
00751   }
00752     
00753   void publishCollisionMap(const CMap &map, 
00754                            const std::string &frame_id,
00755                            const ros::Time &stamp,
00756                            ros::Publisher &pub) const
00757   {
00758     arm_navigation_msgs::CollisionMap cmap;
00759     cmap.header.frame_id = frame_id;
00760     cmap.header.stamp = stamp;
00761     const unsigned int ms = map.size();
00762         
00763     for (CMap::const_iterator it = map.begin() ; it != map.end() ; ++it)
00764     {
00765       const CollisionPoint &cp = *it;
00766       arm_navigation_msgs::OrientedBoundingBox box;
00767       box.extents.x = box.extents.y = box.extents.z = bi_.resolution;
00768       box.axis.x = box.axis.y = 0.0; box.axis.z = 1.0;
00769       box.angle = 0.0;
00770       box.center.x = cp.x * bi_.resolution + bi_.originX;
00771       box.center.y = cp.y * bi_.resolution + bi_.originY;
00772       box.center.z = cp.z * bi_.resolution + bi_.originZ;
00773       cmap.boxes.push_back(box);
00774     } 
00775     pub.publish(cmap);
00776     
00777     ROS_DEBUG("Published collision map with %u boxes", ms);
00778   }
00779 
00780   void makeStaticCollisionMap(const arm_navigation_msgs::MakeStaticCollisionMapGoalConstPtr& goal) {
00781     
00782     if(cloud_source_map_.find(goal->cloud_source) == cloud_source_map_.end())
00783     {
00784       ROS_ERROR_STREAM("Could not make static map for source: "<<goal->cloud_source);
00785       ROS_ERROR_STREAM("Currently making maps from the following sources:");
00786 
00787       for(std::map<std::string,CloudInfo>::iterator it = cloud_source_map_.begin(); 
00788           it != cloud_source_map_.end();
00789           it++) 
00790       {
00791         ROS_ERROR_STREAM(it->first);
00792       } 
00793       action_server_->setAborted();
00794       return;
00795     }
00796 
00797     static_map_goal_ = new arm_navigation_msgs::MakeStaticCollisionMapGoal(*goal);
00798     cloud_count_ = 0;
00799     making_static_collision_map_ = true;
00800     disregard_first_message_ = true;
00801 
00802     ros::Rate r(10);
00803     while(making_static_collision_map_) {
00804       if(action_server_->isPreemptRequested()) {
00805         making_static_collision_map_ = false;
00806         // Clean up tempMaps before breaking
00807         std::string map_name = static_map_goal_->cloud_source + "_static_temp";
00808         if(tempMaps_.find(map_name) != tempMaps_.end())
00809         {
00810           delete tempMaps_[map_name];  
00811           tempMaps_.erase(map_name);
00812         }
00813 
00814         break;
00815       }      
00816       r.sleep();
00817     }
00818 
00819     if(publish_over_dynamic_map_)
00820     {
00821      // loop through cloud_source_map, set dynamic publish to false on all sources to preserve behavior of older collision_map_self_occ
00822       for(std::map<std::string,CloudInfo>::iterator it = cloud_source_map_.begin(); 
00823           it != cloud_source_map_.end();
00824           it++) 
00825       {
00826         it->second.dynamic_publish_ = false;    
00827       } 
00828     }
00829 
00830     delete static_map_goal_;
00831     if(action_server_->isPreemptRequested()) {
00832       action_server_->setPreempted();
00833     } else {
00834       action_server_->setSucceeded();
00835     }
00836   }
00837 
00838   boost::recursive_mutex                                  mapProcessing_;
00839   
00840   tf::TransformListener                         tf_;
00841   //robot_self_filter::SelfMask                  *sm_;
00842   //filters::SelfFilter<sensor_msgs::PointCloud> *self_filter_;
00843   std::vector<message_filters::Subscriber<sensor_msgs::PointCloud>* > mn_cloud_tf_sub_vector_;
00844   std::vector<tf::MessageFilter<sensor_msgs::PointCloud>* > mn_cloud_tf_fil_vector_;
00845   //tf::MessageNotifier<sensor_msgs::PointCloud> *mnCloudIncremental_;
00846   ros::NodeHandle                               root_handle_;
00847   ros::Publisher                                cmapPublisher_;
00848   ros::Publisher                                cmapUpdPublisher_;
00849   ros::Publisher static_map_publisher_;           
00850   std::map<std::string, ros::Publisher>         occPublisherMap_;
00851   ros::ServiceServer                            resetService_;
00852   bool                                          publishOcclusion_;
00853     
00854   arm_navigation_msgs::MakeStaticCollisionMapGoal *static_map_goal_;
00855   bool making_static_collision_map_;
00856   bool publish_over_dynamic_map_;
00857   bool disregard_first_message_;
00858   int cloud_count_;
00859 
00860 
00861   std::map<std::string, std::list<StampedCMap*> >                  currentMaps_;  //indexed by frame_ids
00862   std::map<std::string, StampedCMap*>                                   tempMaps_;  //indexed by frame_ids_static_save
00863     
00864   BoxInfo                                       bi_;
00865   std::string                                   fixedFrame_;
00866   std::string                                   robotFrame_;
00867 
00868   std::map<std::string,CloudInfo> cloud_source_map_;
00869   std::map<std::string,bool> static_map_published_;
00870   boost::shared_ptr<actionlib::SimpleActionServer<arm_navigation_msgs::MakeStaticCollisionMapAction> > action_server_;  
00871   
00872   ros::ServiceServer get_settings_server_;
00873   ros::ServiceServer set_settings_server_;
00874   
00875 
00876   
00877 };  
00878 
00879 void spinThread()
00880 {
00881   ros::spin();
00882 }
00883   
00884 int main (int argc, char** argv)
00885 {
00886   ros::init(argc, argv, "collision_map_self_occ");
00887 
00888   boost::thread spin_thread(&spinThread);
00889 
00890   CollisionMapperOcc cm;
00891   
00892   while(ros::ok()) {
00893     ros::Duration(0.1).sleep();    
00894   }
00895 
00896   ros::shutdown();
00897   spin_thread.join();
00898     
00899   return 0;
00900 }


collision_map
Author(s): Radu Bogdan Rusu, Ioan Sucan
autogenerated on Thu Dec 12 2013 11:10:51