hiep sua ten file

CMakeLists.txt

@@ -56,13 +56,13 @@ add_library(costmap_2d
 # --- Link các thư viện phụ thuộc ---
 target_link_libraries(costmap_2d
     ${Boost_LIBRARIES}        # Boost
-    std_msgs                   
+    robot_std_msgs                   
-    sensor_msgs
+    robot_sensor_msgs
     geometry_msgs
-    nav_msgs
+    robot_nav_msgs
-    map_msgs
+    robot_map_msgs
     laser_geometry
-    visualization_msgs
+    robot_visualization_msgs
     voxel_grid
     tf3
     tf3_geometry_msgs

include/costmap_2d/directional_layer.h

@@ -2,7 +2,7 @@
 #define COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
 
 #include <costmap_2d/static_layer.h>
-#include <nav_msgs/Path.h>
+#include <robot_nav_msgs/Path.h>
 
 namespace costmap_2d
 {
@@ -15,20 +15,20 @@ namespace costmap_2d
     void resetMap();
 
 	private:
-    void incomingMap(const nav_msgs::OccupancyGrid &new_map);
+    void incomingMap(const robot_nav_msgs::OccupancyGrid &new_map);
-		bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const nav_msgs::Path path);
+		bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const robot_nav_msgs::Path path);
     unsigned char laneFilter(unsigned int x, unsigned int y, double yaw_robot);
 		void laneFilter(std::vector<unsigned int> x, std::vector<unsigned int> y, std::vector<double> yaw_robot);
     void laneFilter(std::vector<std::pair<unsigned int, double>> x, std::vector<std::pair<unsigned int, double>> y, std::vector<double> yaw_robot);
     bool inSkipErea(double start_x, double start_y, double end_x, double end_y, double skip_distance);
-    void convertToMap(unsigned char *costmap, nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th);
+    void convertToMap(unsigned char *costmap, robot_nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th);
     bool getRobotPose(double &x, double &y, double &yaw);
 
     std::vector<std::array<uint16_t, 2>> directional_areas_;
     double pose_x_, pose_y_, pose_yaw_;
     double distance_skip_ = 1.0;
     // ros::Publisher lane_mask_pub_;
-    nav_msgs::OccupancyGrid new_map_;
+    robot_nav_msgs::OccupancyGrid new_map_;
 	};
 }
 

include/costmap_2d/observation.h

@@ -33,7 +33,7 @@
 #define COSTMAP_2D_OBSERVATION_H_
 
 #include <robot_geometry_msgs/Point.h>
-#include <sensor_msgs/PointCloud2.h>
+#include <robot_sensor_msgs/PointCloud2.h>
 
 namespace costmap_2d
 {
@@ -50,7 +50,7 @@ public:
    * @brief  Creates an empty observation
    */
   Observation() :
-    cloud_(new sensor_msgs::PointCloud2()), obstacle_range_(0.0), raytrace_range_(0.0)
+    cloud_(new robot_sensor_msgs::PointCloud2()), obstacle_range_(0.0), raytrace_range_(0.0)
   {
   }
 
@@ -66,9 +66,9 @@ public:
    * @param obstacle_range The range out to which an observation should be able to insert obstacles
    * @param raytrace_range The range out to which an observation should be able to clear via raytracing
    */
-  Observation(robot_geometry_msgs::Point& origin, const sensor_msgs::PointCloud2 &cloud,
+  Observation(robot_geometry_msgs::Point& origin, const robot_sensor_msgs::PointCloud2 &cloud,
               double obstacle_range, double raytrace_range) :
-      origin_(origin), cloud_(new sensor_msgs::PointCloud2(cloud)),
+      origin_(origin), cloud_(new robot_sensor_msgs::PointCloud2(cloud)),
       obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
   {
   }
@@ -78,7 +78,7 @@ public:
    * @param obs The observation to copy
    */
   Observation(const Observation& obs) :
-      origin_(obs.origin_), cloud_(new sensor_msgs::PointCloud2(*(obs.cloud_))),
+      origin_(obs.origin_), cloud_(new robot_sensor_msgs::PointCloud2(*(obs.cloud_))),
       obstacle_range_(obs.obstacle_range_), raytrace_range_(obs.raytrace_range_)
   {
   }
@@ -88,13 +88,13 @@ public:
    * @param cloud The point cloud of the observation
    * @param obstacle_range The range out to which an observation should be able to insert obstacles
    */
-  Observation(const sensor_msgs::PointCloud2 &cloud, double obstacle_range) :
+  Observation(const robot_sensor_msgs::PointCloud2 &cloud, double obstacle_range) :
-      cloud_(new sensor_msgs::PointCloud2(cloud)), obstacle_range_(obstacle_range), raytrace_range_(0.0)
+      cloud_(new robot_sensor_msgs::PointCloud2(cloud)), obstacle_range_(obstacle_range), raytrace_range_(0.0)
   {
   }
 
   robot_geometry_msgs::Point origin_;
-  sensor_msgs::PointCloud2* cloud_;
+  robot_sensor_msgs::PointCloud2* cloud_;
   double obstacle_range_, raytrace_range_;
 };
 

include/costmap_2d/observation_buffer.h

@@ -44,7 +44,7 @@
 #include <costmap_2d/observation.h>
 #include <tf3/buffer_core.h>
 
-#include <sensor_msgs/PointCloud2.h>
+#include <robot_sensor_msgs/PointCloud2.h>
 
 // Thread support
 #include <boost/thread.hpp>
@@ -96,7 +96,7 @@ public:
    * <b>Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier</b>
    * @param  cloud The cloud to be buffered
    */
-  void bufferCloud(const sensor_msgs::PointCloud2& cloud);
+  void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud);
 
   /**
    * @brief  Pushes copies of all current observations onto the end of the vector passed in

include/costmap_2d/obstacle_layer.h

@@ -43,13 +43,13 @@
 #include <costmap_2d/observation_buffer.h>
 #include <costmap_2d/footprint.h>
 
-#include <nav_msgs/OccupancyGrid.h>
+#include <robot_nav_msgs/OccupancyGrid.h>
 
-#include <sensor_msgs/LaserScan.h>
+#include <robot_sensor_msgs/LaserScan.h>
 #include <laser_geometry/laser_geometry.hpp>
-#include <sensor_msgs/PointCloud.h>
+#include <robot_sensor_msgs/PointCloud.h>
-#include <sensor_msgs/PointCloud2.h>
+#include <robot_sensor_msgs/PointCloud2.h>
-#include <sensor_msgs/point_cloud_conversion.h>
+#include <robot_sensor_msgs/point_cloud_conversion.h>
 
 namespace costmap_2d
 {
@@ -86,7 +86,7 @@ protected:
    * @param message The message returned from a message notifier
    * @param buffer A pointer to the observation buffer to update
    */
-  void laserScanCallback(const sensor_msgs::LaserScan& message,
+  void laserScanCallback(const robot_sensor_msgs::LaserScan& message,
                          const boost::shared_ptr<ObservationBuffer>& buffer);
 
    /**
@@ -94,7 +94,7 @@ protected:
     * @param message The message returned from a message notifier
     * @param buffer A pointer to the observation buffer to update
     */
-  void laserScanValidInfCallback(const sensor_msgs::LaserScan& message,
+  void laserScanValidInfCallback(const robot_sensor_msgs::LaserScan& message,
                                  const boost::shared_ptr<ObservationBuffer>& buffer);
 
   /**
@@ -102,7 +102,7 @@ protected:
    * @param message The message returned from a message notifier
    * @param buffer A pointer to the observation buffer to update
    */
-  void pointCloudCallback(const sensor_msgs::PointCloud& message,
+  void pointCloudCallback(const robot_sensor_msgs::PointCloud& message,
                           const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
 
   /**
@@ -110,7 +110,7 @@ protected:
    * @param message The message returned from a message notifier
    * @param buffer A pointer to the observation buffer to update
    */
-  void pointCloud2Callback(const sensor_msgs::PointCloud2& message,
+  void pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message,
                            const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
 
   /**

include/costmap_2d/static_layer.h

@@ -41,8 +41,8 @@
 #include <costmap_2d/costmap_layer.h>
 #include <costmap_2d/layered_costmap.h>
 
-#include <nav_msgs/OccupancyGrid.h>
+#include <robot_nav_msgs/OccupancyGrid.h>
-#include <map_msgs/OccupancyGridUpdate.h>
+#include <robot_map_msgs/OccupancyGridUpdate.h>
 #include <string>
 
 namespace costmap_2d
@@ -68,8 +68,8 @@ protected:
                   const std::type_info& type,
                   const std::string& topic) override;
   virtual unsigned char interpretValue(unsigned char value);
-  virtual void incomingMap(const nav_msgs::OccupancyGrid& new_map);
+  virtual void incomingMap(const robot_nav_msgs::OccupancyGrid& new_map);
-  virtual void incomingUpdate(const map_msgs::OccupancyGridUpdate& update);
+  virtual void incomingUpdate(const robot_map_msgs::OccupancyGridUpdate& update);
   unsigned char* threshold_;
   std::string base_frame_id_;
   std::string global_frame_;  ///< @brief The global frame for the costmap

include/costmap_2d/testing_helper.h

@@ -7,7 +7,7 @@
 #include <costmap_2d/obstacle_layer.h>
 #include <costmap_2d/inflation_layer.h>
 
-#include <sensor_msgs/point_cloud2_iterator.h>
+#include <robot_sensor_msgs/point_cloud2_iterator.h>
 
 const double MAX_Z(1.0);
 
@@ -66,13 +66,13 @@ costmap_2d::ObstacleLayer* addObstacleLayer(costmap_2d::LayeredCostmap& layers,
 
 void addObservation(costmap_2d::ObstacleLayer* olayer, double x, double y, double z = 0.0,
                     double ox = 0.0, double oy = 0.0, double oz = MAX_Z){
-  sensor_msgs::PointCloud2 cloud;
+  robot_sensor_msgs::PointCloud2 cloud;
-  sensor_msgs::PointCloud2Modifier modifier(cloud);
+  robot_sensor_msgs::PointCloud2Modifier modifier(cloud);
   modifier.setPointCloud2FieldsByString(1, "xyz");
   modifier.resize(1);
-  sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
+  robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
-  sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
+  robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
-  sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
+  robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
   *iter_x = x;
   *iter_y = y;
   *iter_z = z;

include/costmap_2d/voxel_grid.h

@@ -10,7 +10,7 @@
 #define VOXEL_GRID_COSTMAP_2D_H
 
 #include <vector>
-#include <std_msgs/Header.h>
+#include <robot_std_msgs/Header.h>
 #include <robot_geometry_msgs/Point32.h>
 #include <robot_geometry_msgs/Vector3.h>
 
@@ -18,7 +18,7 @@ namespace costmap_2d
 {
     struct VoxelGrid
     {
-        std_msgs::Header header;
+        robot_std_msgs::Header header;
         std::vector<uint32_t> data;
         robot_geometry_msgs::Point32 origin;
         robot_geometry_msgs::Vector3 resolutions;

include/costmap_2d/voxel_layer.h

@@ -42,12 +42,12 @@
 #include <costmap_2d/layered_costmap.h>
 #include <costmap_2d/observation_buffer.h>
 #include <costmap_2d/voxel_grid.h>
-#include <nav_msgs/OccupancyGrid.h>
+#include <robot_nav_msgs/OccupancyGrid.h>
-#include <sensor_msgs/LaserScan.h>
+#include <robot_sensor_msgs/LaserScan.h>
 #include <laser_geometry/laser_geometry.hpp>
-#include <sensor_msgs/PointCloud.h>
+#include <robot_sensor_msgs/PointCloud.h>
-#include <sensor_msgs/PointCloud2.h>
+#include <robot_sensor_msgs/PointCloud2.h>
-#include <sensor_msgs/point_cloud_conversion.h>
+#include <robot_sensor_msgs/point_cloud_conversion.h>
 #include <costmap_2d/obstacle_layer.h>
 #include <voxel_grid/voxel_grid.h>
 
@@ -93,7 +93,7 @@ private:
   voxel_grid::VoxelGrid voxel_grid_;
   double z_resolution_, origin_z_;
   unsigned int unknown_threshold_, mark_threshold_, size_z_;
-  sensor_msgs::PointCloud clearing_endpoints_;
+  robot_sensor_msgs::PointCloud clearing_endpoints_;
 
   inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
   {

plugins/directional_layer.cpp

@@ -23,14 +23,14 @@ namespace costmap_2d
   {
     if (directional_areas_.empty())
       throw "Has no map data";
-    nav_msgs::Path path;
+    robot_nav_msgs::Path path;
     path.header.stamp = robot::Time::now();
     path.header.frame_id = map_frame_;
     path.poses = plan;
     return this->laneFilter(directional_areas_, path);
   }
 
-  void DirectionalLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
+  void DirectionalLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
   {
     if(!map_shutdown_)
     {
@@ -110,7 +110,7 @@ namespace costmap_2d
     }
   }
 
-  bool DirectionalLayer::laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const nav_msgs::Path path)
+  bool DirectionalLayer::laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const robot_nav_msgs::Path path)
   {
     boost::unique_lock<mutex_t> lock(*layered_costmap_->getCostmap()->getMutex());
     if (new_map.empty())
@@ -159,7 +159,7 @@ namespace costmap_2d
     if (!Yaw_list.empty())
     {
       laneFilter(X_List, Y_List, Yaw_list);
-      nav_msgs::OccupancyGrid lanes;
+      robot_nav_msgs::OccupancyGrid lanes;
       convertToMap(costmap_, lanes, 0.65, 0.196);
 
       //////////////////////////////////
@@ -177,7 +177,7 @@ namespace costmap_2d
     
   }
 
-  void DirectionalLayer::convertToMap(unsigned char *costmap, nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th)
+  void DirectionalLayer::convertToMap(unsigned char *costmap, robot_nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th)
   {
     dir_map.header = new_map_.header;
     dir_map.header.stamp = robot::Time::now();

plugins/obstacle_layer.cpp

@@ -38,7 +38,7 @@
 #include <costmap_2d/obstacle_layer.h>
 #include <costmap_2d/costmap_math.h>
 
-#include <sensor_msgs/point_cloud2_iterator.h>
+#include <robot_sensor_msgs/point_cloud2_iterator.h>
 
 #include <tf3/exceptions.h>
 #include <boost/dll/alias.hpp>
@@ -199,14 +199,14 @@ void ObstacleLayer::handleImpl(const void* data,
   {
     if(observation_buffers_.empty()) return;
     boost::shared_ptr<ObservationBuffer> buffer = observation_buffers_.back();
-    if (type == typeid(sensor_msgs::LaserScan) && topic == "laser") {
+    if (type == typeid(robot_sensor_msgs::LaserScan) && topic == "laser") {
-        laserScanCallback(*static_cast<const sensor_msgs::LaserScan*>(data), buffer);
+        laserScanCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
-    } else if (type == typeid(sensor_msgs::LaserScan) && topic == "laser_valid") {
+    } else if (type == typeid(robot_sensor_msgs::LaserScan) && topic == "laser_valid") {
-        laserScanValidInfCallback(*static_cast<const sensor_msgs::LaserScan*>(data), buffer);
+        laserScanValidInfCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
-    } else if (type == typeid(sensor_msgs::PointCloud) && topic == "pcl_cb") {
+    } else if (type == typeid(robot_sensor_msgs::PointCloud) && topic == "pcl_cb") {
-        pointCloudCallback(*static_cast<const sensor_msgs::PointCloud*>(data), buffer);
+        pointCloudCallback(*static_cast<const robot_sensor_msgs::PointCloud*>(data), buffer);
-    } else if (type == typeid(sensor_msgs::PointCloud2) && topic == "pcl2_cb") {
+    } else if (type == typeid(robot_sensor_msgs::PointCloud2) && topic == "pcl2_cb") {
-        pointCloud2Callback(*static_cast<const sensor_msgs::PointCloud2*>(data), buffer);
+        pointCloud2Callback(*static_cast<const robot_sensor_msgs::PointCloud2*>(data), buffer);
     } else {
         std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
     }
@@ -218,11 +218,11 @@ void ObstacleLayer::handleImpl(const void* data,
   }
 }
 
-void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message,
+void ObstacleLayer::laserScanCallback(const robot_sensor_msgs::LaserScan& message,
                                       const boost::shared_ptr<ObservationBuffer>& buffer)
 {
   // project the laser into a point cloud
-  sensor_msgs::PointCloud2 cloud;
+  robot_sensor_msgs::PointCloud2 cloud;
   cloud.header = message.header;
 
   // project the scan into a point cloud
@@ -248,12 +248,12 @@ void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message,
   buffer->unlock();
 }
 
-void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_message,
+void ObstacleLayer::laserScanValidInfCallback(const robot_sensor_msgs::LaserScan& raw_message,
                                               const boost::shared_ptr<ObservationBuffer>& buffer)
 {
   // Filter positive infinities ("Inf"s) to max_range.
   float epsilon = 0.0001;  // a tenth of a millimeter
-  sensor_msgs::LaserScan message = raw_message;
+  robot_sensor_msgs::LaserScan message = raw_message;
   for (size_t i = 0; i < message.ranges.size(); i++)
   {
     float range = message.ranges[ i ];
@@ -264,7 +264,7 @@ void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_
   }
 
   // project the laser into a point cloud
-  sensor_msgs::PointCloud2 cloud;
+  robot_sensor_msgs::PointCloud2 cloud;
   cloud.header = message.header;
 
   // project the scan into a point cloud
@@ -290,12 +290,12 @@ void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_
   buffer->unlock();
 }
 
-void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloud& message,
+void ObstacleLayer::pointCloudCallback(const robot_sensor_msgs::PointCloud& message,
                                       const boost::shared_ptr<ObservationBuffer>& buffer)
 {
-  sensor_msgs::PointCloud2 cloud2;
+  robot_sensor_msgs::PointCloud2 cloud2;
 
-  if (!sensor_msgs::convertPointCloudToPointCloud2(message, cloud2))
+  if (!robot_sensor_msgs::convertPointCloudToPointCloud2(message, cloud2))
   {
     printf("Failed to convert a PointCloud to a PointCloud2, dropping message\n");
     return;
@@ -307,7 +307,7 @@ void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloud& message,
   buffer->unlock();
 }
 
-void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2& message,
+void ObstacleLayer::pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message,
                                         const boost::shared_ptr<ObservationBuffer>& buffer)
 {
   // buffer the point cloud
@@ -346,13 +346,13 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
   {
     const Observation& obs = *it;
 
-    const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
+    const robot_sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
 
     double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
 
-    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
+    robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
-    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
+    robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
-    sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
+    robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
 
     for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
     {
@@ -479,7 +479,7 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
 {
   double ox = clearing_observation.origin_.x;
   double oy = clearing_observation.origin_.y;
-  const sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);
+  const robot_sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);
 
   // get the map coordinates of the origin of the sensor
   unsigned int x0, y0;
@@ -500,8 +500,8 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
   touch(ox, oy, min_x, min_y, max_x, max_y);
 
   // for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
-  sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
+  robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
-  sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
+  robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
 
   for (; iter_x != iter_x.end(); ++iter_x, ++iter_y)
   {

plugins/static_layer.cpp

@@ -170,16 +170,16 @@ void StaticLayer::handleImpl(const void* data,
                             const std::type_info& type,
                             const std::string& topic)
 {
-    if (type == typeid(nav_msgs::OccupancyGrid) && topic == "map") {
+    if (type == typeid(robot_nav_msgs::OccupancyGrid) && topic == "map") {
-        incomingMap(*static_cast<const nav_msgs::OccupancyGrid*>(data));
+        incomingMap(*static_cast<const robot_nav_msgs::OccupancyGrid*>(data));
-    } else if (type == typeid(map_msgs::OccupancyGridUpdate) && topic == "map_update") {
+    } else if (type == typeid(robot_map_msgs::OccupancyGridUpdate) && topic == "map_update") {
-        incomingUpdate(*static_cast<const map_msgs::OccupancyGridUpdate*>(data));
+        incomingUpdate(*static_cast<const robot_map_msgs::OccupancyGridUpdate*>(data));
     } else {
         std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
     }
 }
 
-void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
+void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
 {
   if(!map_shutdown_)
   {
@@ -248,7 +248,7 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
   }
 }
 
-void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdate& update)
+void StaticLayer::incomingUpdate(const robot_map_msgs::OccupancyGridUpdate& update)
 {
   if(!map_update_shutdown_)
   {

plugins/voxel_layer.cpp

@@ -36,7 +36,7 @@
  *         David V. Lu!!
  *********************************************************************/
 #include <costmap_2d/voxel_layer.h>
-#include <sensor_msgs/point_cloud2_iterator.h>
+#include <robot_sensor_msgs/point_cloud2_iterator.h>
 #include <boost/dll/alias.hpp>
 
 #define VOXEL_BITS 16
@@ -180,13 +180,13 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
   {
     const Observation& obs = *it;
 
-    const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
+    const robot_sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
 
     double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
 
-    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
+    robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
-    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
+    robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
-    sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
+    robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
 
     for (unsigned int i = 0; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
     {
@@ -333,9 +333,9 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
   double map_end_x = origin_x_ + getSizeInMetersX();
   double map_end_y = origin_y_ + getSizeInMetersY();
 
-  sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
+  robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
-  sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
+  robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
-  sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
+  robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
 
   for (;iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
   {

src/observation_buffer.cpp

@@ -38,7 +38,7 @@
 
 #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
 #include <tf3_sensor_msgs/tf3_sensor_msgs.h>
-#include <sensor_msgs/point_cloud2_iterator.h>
+#include <robot_sensor_msgs/point_cloud2_iterator.h>
 
 using namespace std;
 using namespace tf3;
@@ -117,7 +117,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
   return true;
 }
 
-void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
+void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
 {
   robot_geometry_msgs::PointStamped global_origin;
 
@@ -154,7 +154,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
     observation_list_.front().raytrace_range_ = raytrace_range_;
     observation_list_.front().obstacle_range_ = obstacle_range_;
 
-    sensor_msgs::PointCloud2 global_frame_cloud;
+    robot_sensor_msgs::PointCloud2 global_frame_cloud;
 
     // transform the point cloud
     // tf3_buffer_.transform(cloud, global_frame_cloud, global_frame_);
@@ -167,7 +167,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
     global_frame_cloud.header.stamp = cloud.header.stamp;
 
     // now we need to remove observations from the cloud that are below or above our height thresholds
-    sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
+    robot_sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
     observation_cloud.height = global_frame_cloud.height;
     observation_cloud.width = global_frame_cloud.width;
     observation_cloud.fields = global_frame_cloud.fields;
@@ -177,12 +177,12 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
     observation_cloud.is_dense = global_frame_cloud.is_dense;
 
     unsigned int cloud_size = global_frame_cloud.height*global_frame_cloud.width;
-    sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
+    robot_sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
     modifier.resize(cloud_size);
     unsigned int point_count = 0;
 
     // copy over the points that are within our height bounds
-    sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");
+    robot_sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");
     std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end();
     std::vector<unsigned char>::iterator iter_obs = observation_cloud.data.begin();
     for (; iter_global != iter_global_end; ++iter_z, iter_global += global_frame_cloud.point_step)