add multi camera depth

include/robot_costmap_2d/obstacle_layer.h

@@ -46,6 +46,9 @@
 
 #include <robot_nav_msgs/OccupancyGrid.h>
 
+#include <mutex>
+
+#include <robot_sensor_msgs/DepthCameraData.h>
 #include <robot_sensor_msgs/LaserScan.h>
 #include <robot_laser_geometry/laser_geometry.hpp>
 #include <robot_sensor_msgs/PointCloud.h>
@@ -128,6 +131,12 @@ protected:
   void pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message,
                            const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);
 
+  /**
+   * @brief Buffer a depth image and its camera model for frustum clearing.
+   */
+  void depthImageCallback(const robot_sensor_msgs::DepthCameraData& message,
+                          const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);
+
   /**
    * @brief  Get the observations used to mark space
    * @param marking_observations A reference to a vector that will be populated with the observations
@@ -142,6 +151,13 @@ protected:
    */
   bool getClearingObservations(std::vector<robot_costmap_2d::Observation>& clearing_observations) const;
 
+  /**
+   * @brief Collect fresh depth frames from every configured frustum-clearing source.
+   * @return True when every configured depth source is current.
+   */
+  bool getFrustumClearingObservations(
+      std::vector<robot_costmap_2d::DepthCameraObservation>& frustum_clearing_observations) const;
+
   /**
    * @brief  Clear freespace based on one observation
    * @param clearing_observation The observation used to raytrace
@@ -170,6 +186,9 @@ protected:
   std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > marking_buffers_;  ///< @brief Used to store observation buffers used for marking obstacles
   std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > clearing_buffers_;  ///< @brief Used to store observation buffers used for clearing obstacles
 
+  std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > depth_observation_buffers_;
+  std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > depth_clearing_buffers_;
+
   // Used only for testing purposes
   std::vector<robot_costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
 
@@ -178,6 +197,10 @@ protected:
 
   int combination_method_;
   std::vector<CallBackInfo> callback_infos_;
+  std::vector<CallBackInfo> callback_depth_infos_;
+  std::string depth_camera_data_topic_;
+  mutable std::mutex depth_camera_data_mutex_;
+  robot_sensor_msgs::DepthCameraData::ConstPtr pending_depth_camera_data_;
 
 private:
   bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);