collider.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2010, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *  \author Adam Harmat, Gil E. Jones, Kai M. Wurm, Armin Hornung
00036  *********************************************************************/
00037 
00038 #include <ros/ros.h>
00039 
00040 #include <std_srvs/Empty.h>
00041 #include <std_msgs/Header.h>
00042 #include <sensor_msgs/PointCloud2.h>
00043 #include <arm_navigation_msgs/CollisionMap.h>
00044 #include <tf/transform_listener.h>
00045 #include <tf/message_filter.h>
00046 #include <message_filters/subscriber.h>
00047 #include <message_filters/synchronizer.h>
00048 #include <message_filters/sync_policies/exact_time.h>
00049 #include <message_filters/sync_policies/approximate_time.h>
00050 
00051 #include <pcl/point_types.h>
00052 #include <pcl/point_cloud.h>
00053 #include <pcl/ros/conversions.h>
00054 //#include <pcl_tf/transforms.h> // cturtle
00055 #include <pcl_ros/transforms.h>  // dturtle
00056 
00057 #include <boost/thread/mutex.hpp>
00058 #include <boost/bind.hpp>
00059 #include <visualization_msgs/MarkerArray.h>
00060 #include <visualization_msgs/Marker.h>
00061 #include <arm_navigation_msgs/AttachedCollisionObject.h>
00062 #include <arm_navigation_msgs/MakeStaticCollisionMapAction.h>
00063 #include <collider/OccupancyPointQuery.h>
00064 #include <collider/OccupancyBBXQuery.h>
00065 #include <collider/OccupancyBBXSizeQuery.h>
00066 #include <actionlib/server/simple_action_server.h>
00067 #include <image_transport/image_transport.h>
00068 #include <image_geometry/pinhole_camera_model.h>
00069 #include <robot_self_filter/self_mask.h>
00070 #include <planning_environment/models/collision_models.h>
00071 
00072 #include <vector>
00073 #include <algorithm>
00074 
00075 #include <octomap/OcTreeStamped.h>
00076 #include <octomap_ros/OctomapROS.h>
00077 #include <octomap_msgs/Octomap.h>
00078 #include <octomap_msgs/GetOctomap.h>
00079 
00080 
00081 class Collider {
00082 
00083  public:
00084 
00085   Collider();
00086   ~Collider();
00087 
00088  public:
00089 
00090   typedef octomap::OcTreeStamped OcTreeType;
00091   
00093   struct CloudInfo 
00094   {
00095     CloudInfo(): cloud_name_(""), raw_cloud_name_(""), frame_subsample_(1.0),
00096                  point_subsample_(1.0), sensor_frame_(""), sensor_stereo_other_frame_(""), counter_(0),
00097                  dynamic_buffer_size_(1), static_buffer_size_(1), 
00098                  dynamic_buffer_duration_(0), static_buffer_duration_(0),
00099                  dynamic_publish_(true), static_publish_(true), 
00100                  dynamic_until_static_publish_(true)
00101                  
00102     {
00103     };
00104     std::string cloud_name_;
00105     std::string raw_cloud_name_;
00106     int frame_subsample_; 
00107     int point_subsample_; 
00108     std::string sensor_frame_;
00109     std::string sensor_stereo_other_frame_;
00110     unsigned int counter_;
00111     
00112     unsigned int dynamic_buffer_size_, static_buffer_size_;
00113     ros::Duration dynamic_buffer_duration_, static_buffer_duration_;
00114 
00115     // Settings for publishing map data on the standard publisher.
00116     // Note that if static_publish is set to false, static map data WILL still be published on the dedicated
00117     // static map topic when a map is acquired, but it WON'T publish on the main publisher, which publishes
00118     // a union of all the maps in the node's buffer.  If dynamic_until_static_publish is set to true then 
00119     // the dynamic map will be published on the main topic until a static map from that source is created, 
00120     // at which point dynamic behavior will revert to that specified by dynamic_publish;
00121     bool dynamic_publish_, static_publish_, dynamic_until_static_publish_;  
00122   };
00123 
00124   // parameters & precomputed values for the box that represents the collision map in the fixed frame
00125   struct BoxInfo
00126   {    
00127     double dimensionX, dimensionY, dimensionZ;
00128     double originX, originY, originZ;
00129     double real_minX, real_minY, real_minZ;
00130     double real_maxX, real_maxY, real_maxZ;
00131   };
00132 
00133 
00134  protected:
00135 
00136   // msg callbacks
00137   void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud, const std::string topic_name);
00138   void cloudCombinedCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud,
00139                     const sensor_msgs::PointCloud2::ConstPtr &cloud_raw, const std::string topic_name);
00140   void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr &cam_info);
00141 
00142   void attachedObjectCallback(const arm_navigation_msgs::AttachedCollisionObjectConstPtr& attached_object);
00143   void objectCallback(const arm_navigation_msgs::CollisionObjectConstPtr& object);
00144 
00145   // obstacle cleaning
00146   void degradeOutdatedRaycasting(const std_msgs::Header& sensor_header, const octomap::point3d& sensor_origin, octomap::OcTreeStamped& tree);
00147   void degradeOutdatedRaw(const std_msgs::Header& sensor_header, const tf::Point& sensor_origin,
00148                           const std::string& other_stereo_frame,
00149                           const pcl::PointCloud<pcl::PointXYZ>& pcl_cloud_raw);
00150   void degradeSingleSpeckles();
00151   void computeBBX(const std_msgs::Header& sensor_header, octomap::point3d& bbx_min, octomap::point3d& bbx_max);
00152   bool inSensorCone(const cv::Point2d& uv) const;
00153   bool isOccludedMap(const octomap::point3d& sensor_origin, const octomap::point3d& p) const;
00154   octomap::point3d getSensorOrigin(const std_msgs::Header& sensor_header);
00155   bool isOccludedRaw (const cv::Point2d& uv, double range, const pcl::PointCloud<pcl::PointXYZ>& pcl_cloud_raw);
00156 
00157   //const tf::Transform& transform, 
00158 
00159   // publish map
00160   void publishCollisionMap(const std::vector<geometry_msgs::Point>& pointlist, const std_msgs::Header &header, ros::Publisher &pub);
00161   void publishPointCloud(const std::vector<geometry_msgs::Point>& pointlist, const std_msgs::Header &header, ros::Publisher &pub);
00162   void publishMarkerArray(const std::vector<geometry_msgs::Point>& pointlist, const std_msgs::Header &header, const std_msgs::ColorRGBA& color, ros::Publisher &pub);
00163 
00164   // action server
00165   bool reset(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
00166   void makeStaticCollisionMap(const arm_navigation_msgs::MakeStaticCollisionMapGoalConstPtr& goal);
00167   bool dummyReset(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
00168   // occupancy queries:
00169   bool octomapSrv(octomap_msgs::GetOctomap::Request  &req, octomap_msgs::GetOctomap::Response &res);
00170   bool occupancyPointSrv(collider::OccupancyPointQuery::Request &req, collider::OccupancyPointQuery::Response &res);
00171   bool occupancyBBXSrv(collider::OccupancyBBXQuery::Request &req, collider::OccupancyBBXQuery::Response &res);
00172   bool occupancyBBXSizeSrv(collider::OccupancyBBXSizeQuery::Request &req, collider::OccupancyBBXSizeQuery::Response &res);
00173 
00174 
00175 
00176  protected:
00177 
00178   tf::TransformListener tf_;
00179   std::vector<message_filters::Subscriber<sensor_msgs::PointCloud2>* > message_filter_subscribers_;
00180   std::vector<tf::MessageFilter<sensor_msgs::PointCloud2>* > message_filters_;
00181 
00182   planning_environment::CollisionModels *cm_;
00183   message_filters::Subscriber<arm_navigation_msgs::AttachedCollisionObject> *attached_collision_object_subscriber_;
00184   message_filters::Subscriber<arm_navigation_msgs::CollisionObject> *collision_object_subscriber_;
00185 
00186   ros::NodeHandle root_handle_;
00187 
00188   ros::Publisher cmap_publisher_, static_map_publisher_, pointcloud_publisher_, marker_pub_,
00189                   octomap_visualization_pub_, octomap_visualization_free_pub_;
00190 
00191   ros::Publisher octomap_visualization_attached_pub_, octomap_visualization_attached_array_pub_;
00192   std_msgs::ColorRGBA attached_color_;
00193 
00194   ros::ServiceServer reset_service_, dummy_reset_service_, transparent_service_,
00195                       get_octomap_service_, occupancy_point_service_,
00196                       occupancy_bbx_service_, occupancy_bbx_size_service_;
00197 
00198   ros::Subscriber*  camera_info_subscriber_;
00199   
00200   OcTreeType*   collision_octree_;
00201   std::map<std::string,CloudInfo>               cloud_sources_;
00202   robot_self_filter::SelfMask* robot_mask_right_;
00203   robot_self_filter::SelfMask* robot_mask_left_;
00204 
00205   image_geometry::PinholeCameraModel cam_model_;
00206   bool cam_model_initialized_;
00207 
00208   bool publish_over_dynamic_map_;
00209 
00210   std::string fixed_frame_;
00211   double resolution_, max_range_, self_filter_min_dist_;
00212   int pruning_period_, pruning_counter_;
00213   bool transparent_freespace_;
00214   int camera_stereo_offset_left_;
00215   int camera_stereo_offset_right_;
00216   std_msgs::ColorRGBA color_free_, color_occupied_;
00217   cv::Size cam_size_;
00218 
00219   BoxInfo workspace_box_;
00220 
00221   inline void getOccupiedPoints(std::vector<geometry_msgs::Point>& pointlist) const{
00222           pointlist.reserve(collision_octree_->size() / 2.0);
00223           for (OcTreeType::iterator it = collision_octree_->begin(),
00224                           end = collision_octree_->end(); it != end; ++it){
00225                   if (collision_octree_->isNodeOccupied(*it)){
00226                           geometry_msgs::Point p;
00227                           p.x = it.getX();
00228                           p.y = it.getY();
00229                           p.z = it.getZ();
00230                           pointlist.push_back(p);
00231                   }
00232           }
00233   }
00234 
00235   inline void getFreePoints(std::vector<geometry_msgs::Point>& pointlist) const{
00236           pointlist.reserve(collision_octree_->size() / 2.0);
00237           for (OcTreeType::iterator it = collision_octree_->begin(),
00238                           end = collision_octree_->end(); it != end; ++it){
00239                   if (!collision_octree_->isNodeOccupied(*it)){
00240                           geometry_msgs::Point p;
00241                           p.x = it.getX();
00242                           p.y = it.getY();
00243                           p.z = it.getZ();
00244                           pointlist.push_back(p);
00245                           if (it.getSize() != collision_octree_->getResolution())
00246                                   ROS_WARN("%f", it.getSize());
00247 
00248                   }
00249           }
00250   }
00251 
00252   boost::shared_ptr<actionlib::SimpleActionServer<arm_navigation_msgs::MakeStaticCollisionMapAction> > action_server_;  
00253 
00254   // Time synchronized, dual input (raw+filtered)
00255   std::vector<message_filters::Subscriber<sensor_msgs::PointCloud2>* > sub_filtered_clouds_;
00256   std::vector<message_filters::Subscriber<sensor_msgs::PointCloud2>* > sub_raw_clouds_;
00257   //message_filters::Subscriber<sensor_msgs::PointCloud2> sub_filtered_;
00258   //message_filters::Subscriber<sensor_msgs::PointCloud2> sub_raw_;
00259   //message_filters::Synchronizer<message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> > sync_;
00260   std::vector<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> >* > sync_vector_;
00261 };
00262 


collider
Author(s): Adam Harmat, Gil E. Jones, Kai M. Wurm, Armin Hornung. Maintained by Gil E. Jones
autogenerated on Fri Dec 6 2013 21:08:23