otimal deep coppy obj

plugins/obstacle_layer.cpp

@@ -269,10 +269,11 @@ void ObstacleLayer::handleImpl(const void* data,
                               const std::type_info& type,
                               const std::string& topic)  
 {
-  if(!stop_receiving_data_)
+  if (!enabled_ || stop_receiving_data_)
+    return;
+
+  if (type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr))
   {
-    if (type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr) )
-    {
       const robot_sensor_msgs::DepthCameraData::ConstPtr& depth_camera_data_ptr =
           *static_cast<const robot_sensor_msgs::DepthCameraData::ConstPtr*>(data);
       if (!depth_camera_data_ptr)
@@ -333,7 +334,8 @@ void ObstacleLayer::handleImpl(const void* data,
 
       // std::lock_guard<std::mutex> lock(depth_camera_data_mutex_);
       // pending_depth_camera_data_ = depth_camera_data_ptr;
-      if(depth_observation_buffers_.empty() || callback_depth_infos_.empty()) return;
+      if (depth_observation_buffers_.empty() || callback_depth_infos_.empty())
+        return;
 
       int size_callback_depth = static_cast<int>(callback_depth_infos_.size());
       for(int i = 0; i < size_callback_depth; i++)
@@ -343,17 +345,17 @@ void ObstacleLayer::handleImpl(const void* data,
             topic == callback_depth_infos_[i].topic)
         {
           // robot::log_error_throttle(1.0,"TEST");
-          depthImageCallback(depth_camera_data, buffer);
+          depthImageCallback(depth_camera_data_ptr, buffer);
         }
       }
-      // return;
-    }
-    else
-    {
-    if(observation_buffers_.empty() || callback_infos_.empty()) return;
-    
+  }
+  else
+  {
+    if (observation_buffers_.empty() || callback_infos_.empty())
+      return;
+
     int size_callback = static_cast<int>(callback_infos_.size());
-    for(int i = 0; i < size_callback; i++)
+    for (int i = 0; i < size_callback; i++)
     {
       boost::shared_ptr<ObservationBuffer>& buffer = observation_buffers_[i];
 
@@ -403,12 +405,6 @@ void ObstacleLayer::handleImpl(const void* data,
       //     << "topic check: " << callback_infos_[i].topic << std::endl << std::endl; 
       // }
     }
-    }
-  }
-  else 
-  {
-    robot::log_info("Stop receiving data!\n");
-    return;
   }
 }
 
@@ -510,12 +506,11 @@ void ObstacleLayer::pointCloud2Callback(const robot_sensor_msgs::PointCloud2& me
   buffer->unlock();
 }
 
-void ObstacleLayer::depthImageCallback(const robot_sensor_msgs::DepthCameraData& message,
+void ObstacleLayer::depthImageCallback(robot_sensor_msgs::DepthCameraData::ConstPtr message,
                                        const boost::shared_ptr<ObservationBuffer>& buffer)
 {
   buffer->lock();
-  // robot::log_error_throttle(1.0, "depth data size 1: %d", (int)message.depth.data.size());
-  buffer->bufferDepthCamera(message);
+  buffer->bufferDepthCamera(std::move(message));
   buffer->unlock();
 }
 
@@ -557,6 +552,9 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
     robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
     robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
 
+    std::size_t rejected_height = 0;
+    std::size_t rejected_range = 0;
+    std::size_t rejected_bounds = 0;
     for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
     {
       double px = *iter_x, py = *iter_y, pz = *iter_z;
@@ -564,7 +562,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
       // if the obstacle is too high or too far away from the robot we won't add it
       if (pz > max_obstacle_height_)
       {
-        robot::log_error("The point is too high\n");
+        ++rejected_height;
         continue;
       }
 
@@ -575,7 +573,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
       // if the point is far enough away... we won't consider it
       if (sq_dist >= sq_obstacle_range)
       {
-        robot::log_error("The point is too far away\n");
+        ++rejected_range;
         continue;
       }
 
@@ -583,7 +581,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
       unsigned int mx, my;
       if (!worldToMap(px, py, mx, my))
       {
-        robot::log_error("Computing map coords failed\n");
+        ++rejected_bounds;
         continue;
       }
 
@@ -591,6 +589,14 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
       costmap_[index] = LETHAL_OBSTACLE;
       touch(px, py, min_x, min_y, max_x, max_y);
     }
+
+    if (rejected_height + rejected_range + rejected_bounds > 0)
+    {
+      robot::log_info_throttle(
+          5.0,
+          "ObstacleLayer filtered points: height=%zu range=%zu outside_map=%zu\n",
+          rejected_height, rejected_range, rejected_bounds);
+    }
   }
 
   updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);