Use custom robot collision padding in occupancy map shape padding

perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h

@@ -59,7 +59,7 @@ class DepthImageOctomapUpdater : public OccupancyMapUpdater
   virtual bool initialize();
   virtual void start();
   virtual void stop();
-  virtual ShapeHandle excludeShape(const shapes::ShapeConstPtr &shape);
+  virtual ShapeHandle excludeShape(const shapes::ShapeConstPtr &shape, const double &scale, const double &padding);
   virtual void forgetShape(ShapeHandle handle);
 
 private:

perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp

@@ -144,7 +144,7 @@ void DepthImageOctomapUpdater::stopHelper()
   sub_depth_image_.shutdown();
 }
 
-mesh_filter::MeshHandle DepthImageOctomapUpdater::excludeShape(const shapes::ShapeConstPtr &shape)
+mesh_filter::MeshHandle DepthImageOctomapUpdater::excludeShape(const shapes::ShapeConstPtr &shape, const double &scale, const double &padding)
 {
   mesh_filter::MeshHandle h = 0;
   if (mesh_filter_)

perception/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h

@@ -102,7 +102,7 @@ class OccupancyMapMonitor
   void addUpdater(const OccupancyMapUpdaterPtr &updater);
 
   /** \brief Add this shape to the set of shapes to be filtered out from the octomap */
-  ShapeHandle excludeShape(const shapes::ShapeConstPtr &shape);
+  ShapeHandle excludeShape(const shapes::ShapeConstPtr &shape, const double &scale = 1, const double &padding = 0);
 
   /** \brief Forget about this shape handle and the shapes it corresponds to */
   void forgetShape(ShapeHandle handle);

perception/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h

@@ -75,7 +75,7 @@ class OccupancyMapUpdater
 
   virtual void stop() = 0;
 
-  virtual ShapeHandle excludeShape(const shapes::ShapeConstPtr &shape) = 0;
+  virtual ShapeHandle excludeShape(const shapes::ShapeConstPtr &shape, const double &scale = 1, const double &padding = 0) = 0;
 
   virtual void forgetShape(ShapeHandle handle) = 0;
 

perception/occupancy_map_monitor/src/occupancy_map_monitor.cpp

@@ -205,16 +205,16 @@ void OccupancyMapMonitor::setMapFrame(const std::string &frame)
   map_frame_ = frame;
 }
 
-ShapeHandle OccupancyMapMonitor::excludeShape(const shapes::ShapeConstPtr &shape)
+ShapeHandle OccupancyMapMonitor::excludeShape(const shapes::ShapeConstPtr &shape, const double &scale, const double &padding)
 {
   // if we have just one updater, remove the additional level of indirection
   if (map_updaters_.size() == 1)
-    return map_updaters_[0]->excludeShape(shape);
+    return map_updaters_[0]->excludeShape(shape, scale, padding);
 
   ShapeHandle h = 0;
   for (std::size_t i = 0 ; i < map_updaters_.size() ; ++i)
   {
-    ShapeHandle mh = map_updaters_[i]->excludeShape(shape);
+    ShapeHandle mh = map_updaters_[i]->excludeShape(shape, scale, padding);
     if (mh)
     {
       if (h == 0)

perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h

@@ -60,7 +60,7 @@ class PointCloudOctomapUpdater : public OccupancyMapUpdater
   virtual bool initialize();
   virtual void start();
   virtual void stop();
-  virtual ShapeHandle excludeShape(const shapes::ShapeConstPtr &shape);
+  virtual ShapeHandle excludeShape(const shapes::ShapeConstPtr &shape, const double &scale, const double &padding);
   virtual void forgetShape(ShapeHandle handle);
 
 protected:
@@ -79,8 +79,6 @@ class PointCloudOctomapUpdater : public OccupancyMapUpdater
 
   /* params */
   std::string point_cloud_topic_;
-  double scale_;
-  double padding_;
   double max_range_;
   unsigned int point_subsample_;
   std::string filtered_cloud_topic_;

perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp

@@ -46,8 +46,6 @@ namespace occupancy_map_monitor
 
 PointCloudOctomapUpdater::PointCloudOctomapUpdater() : OccupancyMapUpdater("PointCloudUpdater"),
                                                        private_nh_("~"),
-                                                       scale_(1.0),
-                                                       padding_(0.0),
                                                        max_range_(std::numeric_limits<double>::infinity()),
                                                        point_subsample_(1),
                                                        point_cloud_subscriber_(NULL),
@@ -69,8 +67,6 @@ bool PointCloudOctomapUpdater::setParams(XmlRpc::XmlRpcValue &params)
     point_cloud_topic_ = static_cast<const std::string&>(params["point_cloud_topic"]);
 
     readXmlParam(params, "max_range", &max_range_);
-    readXmlParam(params, "padding_offset", &padding_);
-    readXmlParam(params, "padding_scale", &scale_);
     readXmlParam(params, "point_subsample", &point_subsample_);
     if (params.hasMember("filtered_cloud_topic"))
       filtered_cloud_topic_ = static_cast<const std::string&>(params["filtered_cloud_topic"]);
@@ -126,11 +122,11 @@ void PointCloudOctomapUpdater::stop()
   point_cloud_subscriber_ = NULL;
 }
 
-ShapeHandle PointCloudOctomapUpdater::excludeShape(const shapes::ShapeConstPtr &shape)
+ShapeHandle PointCloudOctomapUpdater::excludeShape(const shapes::ShapeConstPtr &shape, const double &scale, const double &padding)
 {
   ShapeHandle h = 0;
   if (shape_mask_)
-    h = shape_mask_->addShape(shape, scale_, padding_);
+    h = shape_mask_->addShape(shape, scale, padding);
   else
     ROS_ERROR("Shape filter not yet initialized!");
   return h;

planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -593,6 +593,9 @@ void planning_scene_monitor::PlanningSceneMonitor::excludeRobotLinksFromOctree()
   const std::vector<const robot_model::LinkModel*> &links = getRobotModel()->getLinkModelsWithCollisionGeometry();
   for (std::size_t i = 0 ; i < links.size() ; ++i)
   {
+    const double &link_padding = getPlanningScene()->getCollisionRobot()->getLinkPadding(links[i]->getName());
+    const double &link_scale = getPlanningScene()->getCollisionRobot()->getLinkScale(links[i]->getName());    
+
     std::vector<shapes::ShapeConstPtr> shapes = links[i]->getShapes(); // copy shared ptrs on purpuse
     for (std::size_t j = 0 ; j < shapes.size() ; ++j)
     {
@@ -604,7 +607,7 @@ void planning_scene_monitor::PlanningSceneMonitor::excludeRobotLinksFromOctree()
         shapes[j].reset(m);
       }
 
-      occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(shapes[j]);
+      occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(shapes[j], link_scale, link_padding);
       if (h)
         link_shape_handles_[links[i]].push_back(std::make_pair(h, j));
     }
@@ -684,7 +687,10 @@ void planning_scene_monitor::PlanningSceneMonitor::excludeAttachedBodyFromOctree
   {
     if (attached_body->getShapes()[i]->type == shapes::PLANE || attached_body->getShapes()[i]->type == shapes::OCTREE)
       continue;
-    occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(attached_body->getShapes()[i]);
+
+    static const double scale = 1; // this is not configurable at the moment
+    occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(attached_body->getShapes()[i],
+                                                                          scale, default_attached_padd_);
     if (h)
     {
       found = true;
@@ -724,7 +730,10 @@ void planning_scene_monitor::PlanningSceneMonitor::excludeWorldObjectFromOctree(
   {
     if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
       continue;
-    occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i]);
+
+    static const double scale = 1; // this is not configurable at the moment
+    occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i],
+                                                                          scale, default_object_padd_);
     if (h)
     {
       collision_body_shape_handles_[obj->id_].push_back(std::make_pair(h, &obj->shape_poses_[i]));