hiep update

CMakeLists.txt

@@ -1,6 +1,6 @@
 # --- CMake version và project name ---
 cmake_minimum_required(VERSION 3.10)
-project(costmap_2d)
+project(robot_costmap_2d)
 
 # --- C++ standard và position independent code ---
 set(CMAKE_CXX_STANDARD 17)            # Sử dụng C++17
@@ -10,8 +10,8 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON)  # Thư viện có thể build thành sh
 # Dùng để runtime linker tìm thư viện đã build trước khi install
 set(CMAKE_SKIP_BUILD_RPATH FALSE)
 set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
-set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/costmap_2d")
+set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/robot_costmap_2d")
-set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/costmap_2d")
+set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/robot_costmap_2d")
 set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
 
 # --- Dependencies ---
@@ -23,7 +23,7 @@ find_package(GTest REQUIRED)           # Google Test cho unit test
 find_package(PCL REQUIRED COMPONENTS common io)   # Point Cloud Library
 
 # --- Define macro để dùng trong code ---
-add_definitions(-DCOSTMAP_2D_DIR="${CMAKE_CURRENT_SOURCE_DIR}")
+add_definitions(-DROBOT_COSTMAP_2D_DIR="${CMAKE_CURRENT_SOURCE_DIR}")
 
 # --- Include directories ---
 # Thêm các folder chứa header files
@@ -39,9 +39,9 @@ link_directories(${PCL_LIBRARY_DIRS})  # Thêm thư viện PCL vào linker path
 # --- Eigen và PCL definitions ---
 add_definitions(${EIGEN3_DEFINITIONS} ${PCL_DEFINITIONS})
 
-# --- Core library: costmap_2d ---
+# --- Core library: robot_costmap_2d ---
 # Tạo thư viện chính
-add_library(costmap_2d
+add_library(robot_costmap_2d
     src/costmap_2d_robot.cpp
     src/array_parser.cpp
     src/costmap_2d.cpp
@@ -54,28 +54,28 @@ add_library(costmap_2d
 )
 
 # --- Link các thư viện phụ thuộc ---
-target_link_libraries(costmap_2d
+target_link_libraries(robot_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
+    robot_laser_geometry
-    visualization_msgs
+    robot_visualization_msgs
-    voxel_grid
+    robot_voxel_grid
     tf3
-    tf3_geometry_msgs
+    robot_tf3_geometry_msgs
-    tf3_sensor_msgs
+    robot_tf3_sensor_msgs
     data_convert
-    xmlrpcpp                  # XMLRPC
+    robot_xmlrpcpp                  # XMLRPC
     yaml-cpp
     dl
     robot_cpp 
 )
 
 # --- Include directories cho target ---
-target_include_directories(costmap_2d 
+target_include_directories(robot_costmap_2d 
         PUBLIC 
             ${Boost_INCLUDE_DIRS}  # Boost headers
             $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>   # Khi build từ source
@@ -83,23 +83,23 @@ target_include_directories(costmap_2d
         )
 
 # --- Cài đặt thư viện vào hệ thống khi chạy make install ---
-install(TARGETS costmap_2d
+install(TARGETS robot_costmap_2d
-  EXPORT costmap_2d-targets
+  EXPORT robot_costmap_2d-targets
   ARCHIVE DESTINATION lib      # Thư viện tĩnh .a
   LIBRARY DESTINATION lib      # Thư viện động .so
   RUNTIME DESTINATION bin      # File thực thi (nếu có)
   INCLUDES DESTINATION include # Cài đặt include
 )
 
-# --- Xuất export set costmap_2dTargets thành file CMake module ---
+# --- Xuất export set robot_costmap_2dTargets thành file CMake module ---
-# --- Tạo file lib/cmake/costmap_2d/costmap_2dTargets.cmake ---
+# --- Tạo file lib/cmake/robot_costmap_2d/robot_costmap_2dTargets.cmake ---
 # --- File này chứa cấu hình giúp project khác có thể dùng ---
-# --- Find_package(costmap_2d REQUIRED) ---
+# --- Find_package(robot_costmap_2d REQUIRED) ---
-# --- Target_link_libraries(my_app PRIVATE costmap_2d::costmap_2d) ---
+# --- Target_link_libraries(my_app PRIVATE robot_costmap_2d::robot_costmap_2d) ---
-install(EXPORT costmap_2d-targets 
+install(EXPORT robot_costmap_2d-targets 
-  FILE costmap_2d-targets.cmake
+  FILE robot_costmap_2d-targets.cmake
-  NAMESPACE costmap_2d::
+  NAMESPACE robot_costmap_2d::
-  DESTINATION lib/cmake/costmap_2d
+  DESTINATION lib/cmake/robot_costmap_2d
 )
 
 # --- Cài đặt headers ---
@@ -123,7 +123,7 @@ add_library(plugins
 
 target_link_libraries(plugins 
     PRIVATE
-        costmap_2d 
+        robot_costmap_2d 
         ${Boost_LIBRARIES} 
         yaml-cpp
         robot_time
@@ -144,13 +144,13 @@ install(TARGETS plugins
 
 install(EXPORT plugins-targets 
   FILE plugins-targets.cmake
-  NAMESPACE costmap_2d::
+  NAMESPACE robot_costmap_2d::
   DESTINATION lib/cmake/plugins
 )
 
 
 # --- Option để bật/tắt test ---
-option(BUILD_COSTMAP_TESTS "Build costmap_2d test executables" ON)
+option(BUILD_COSTMAP_TESTS "Build robot_costmap_2d test executables" ON)
 
 if(BUILD_COSTMAP_TESTS)
     # --- Test executables ---
@@ -159,8 +159,8 @@ if(BUILD_COSTMAP_TESTS)
     add_executable(test_plugin test/static_layer_test.cpp)
 
     # --- Link thư viện cho test ---
-    target_link_libraries(test_array_parser PRIVATE costmap_2d GTest::GTest GTest::Main pthread)
+    target_link_libraries(test_array_parser PRIVATE robot_costmap_2d GTest::GTest GTest::Main pthread)
-    target_link_libraries(test_costmap PRIVATE costmap_2d GTest::GTest GTest::Main pthread)
+    target_link_libraries(test_costmap PRIVATE robot_costmap_2d GTest::GTest GTest::Main pthread)
     target_link_libraries(test_plugin PRIVATE 
     
         ${Boost_LIBRARIES}
@@ -171,24 +171,24 @@ if(BUILD_COSTMAP_TESTS)
         yaml-cpp
         tf3
         robot_time
-        costmap_2d
+        robot_costmap_2d
         GTest::GTest GTest::Main
     )
     
     # --- Set RPATH để tìm thư viện của project này thay vì system ROS libraries ---
     set_target_properties(test_array_parser PROPERTIES
-        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
+        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
-        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d"
+        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
         LINK_FLAGS "-Wl,--disable-new-dtags"
     )
     set_target_properties(test_costmap PROPERTIES
-        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
+        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
-        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d"
+        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
         LINK_FLAGS "-Wl,--disable-new-dtags"
     )
     set_target_properties(test_plugin PROPERTIES
-        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
+        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
-        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d"
+        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
         LINK_FLAGS "-Wl,--disable-new-dtags"
     )
 endif()

config/costmap_params.yaml

@@ -1,4 +1,4 @@
-costmap_2d:
+robot_costmap_2d:
   global_frame: map
   robot_base_frame: base_link
   rolling_window: false

include/costmap_2d/critical_layer.h

@@ -1,19 +0,0 @@
-#ifndef COSTMAP_2D_CRITICAL_LAYER_H_
-#define COSTMAP_2D_CRITICAL_LAYER_H_
-
-#include <costmap_2d/static_layer.h>
-
-namespace costmap_2d
-{
-class CriticalLayer : public StaticLayer
-{
-public:
-  CriticalLayer();
-  virtual ~CriticalLayer();
-private:
-  unsigned char interpretValue(unsigned char value) override;
-  void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override;
-};
-}
-
-#endif // COSTMAP_2D_CRITICAL_LAYER_H_

include/costmap_2d/preferred_layer.h

@@ -1,18 +0,0 @@
-#ifndef COSTMAP_2D_PREFERRED_LAYER_H_
-#define COSTMAP_2D_PREFERRED_LAYER_H_
-
-#include <costmap_2d/static_layer.h>
-
-namespace costmap_2d
-{
-class PreferredLayer : public StaticLayer
-{
-public:
-  PreferredLayer();
-  virtual ~PreferredLayer();
-private:
-  unsigned char interpretValue(unsigned char value);
-};
-}
-
-#endif // COSTMAP_2D_PREFERRED_LAYER_

include/costmap_2d/testing_helper.h

@@ -1,99 +0,0 @@
-#ifndef COSTMAP_2D_TESTING_HELPER_H
-#define COSTMAP_2D_TESTING_HELPER_H
-
-#include<costmap_2d/cost_values.h>
-#include<costmap_2d/costmap_2d.h>
-#include <costmap_2d/static_layer.h>
-#include <costmap_2d/obstacle_layer.h>
-#include <costmap_2d/inflation_layer.h>
-
-#include <sensor_msgs/point_cloud2_iterator.h>
-
-const double MAX_Z(1.0);
-
-char printableCost(unsigned char cost)
-{
-  switch (cost)
-  {
-  case costmap_2d::NO_INFORMATION: return '?';
-  case costmap_2d::LETHAL_OBSTACLE: return 'L';
-  case costmap_2d::INSCRIBED_INFLATED_OBSTACLE: return 'I';
-  case costmap_2d::FREE_SPACE: return '.';
-  default: return '0' + (unsigned char) (10 * cost / 255);
-  }
-}
-
-void printMap(costmap_2d::Costmap2D& costmap)
-{
-  printf("map:\n");
-  for (int i = 0; i < costmap.getSizeInCellsY(); i++){
-    for (int j = 0; j < costmap.getSizeInCellsX(); j++){
-      printf("%4d", int(costmap.getCost(j, i)));
-    }
-    printf("\n\n");
-  }
-}
-
-unsigned int countValues(costmap_2d::Costmap2D& costmap, unsigned char value, bool equal = true)
-{
-  unsigned int count = 0;
-  for (int i = 0; i < costmap.getSizeInCellsY(); i++){
-    for (int j = 0; j < costmap.getSizeInCellsX(); j++){
-      unsigned char c = costmap.getCost(j, i);
-      if ((equal && c == value) || (!equal && c != value))
-      {
-        count+=1;
-      }
-    }
-  }
-  return count;
-}
-
-void addStaticLayer(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
-{
-  costmap_2d::StaticLayer* slayer = new costmap_2d::StaticLayer();
-  layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(slayer));
-  slayer->initialize(&layers, "static", &tf);
-}
-
-costmap_2d::ObstacleLayer* addObstacleLayer(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
-{
-  costmap_2d::ObstacleLayer* olayer = new costmap_2d::ObstacleLayer();
-  olayer->initialize(&layers, "obstacles", &tf);
-  layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(olayer));
-  return olayer;
-}
-
-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;
-  sensor_msgs::PointCloud2Modifier modifier(cloud);
-  modifier.setPointCloud2FieldsByString(1, "xyz");
-  modifier.resize(1);
-  sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
-  sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
-  sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
-  *iter_x = x;
-  *iter_y = y;
-  *iter_z = z;
-
-  geometry_msgs::Point p;
-  p.x = ox;
-  p.y = oy;
-  p.z = oz;
-
-  costmap_2d::Observation obs(p, cloud, 100.0, 100.0);  // obstacle range = raytrace range = 100.0
-  olayer->addStaticObservation(obs, true, true);
-}
-
-costmap_2d::InflationLayer* addInflationLayer(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
-{
-  costmap_2d::InflationLayer* ilayer = new costmap_2d::InflationLayer();
-  ilayer->initialize(&layers, "inflation", &tf);
-  boost::shared_ptr<costmap_2d::Layer> ipointer(ilayer);
-  layers.addPlugin(ipointer);
-  return ilayer;
-}
-
-
-#endif  // COSTMAP_2D_TESTING_HELPER_H

include/costmap_2d/unpreferred_layer.h

@@ -1,21 +0,0 @@
-#ifndef COSTMAP_2D_UNPREFERRED_LAYER_H_
-#define COSTMAP_2D_UNPREFERRED_LAYER_H_
-
-#include <costmap_2d/static_layer.h>
-
-namespace costmap_2d
-{
-class UnPreferredLayer : public StaticLayer
-{
-public:
-  UnPreferredLayer();
-  virtual ~UnPreferredLayer();
-
-private:
-  unsigned char interpretValue(unsigned char value);
-
-};
-
-}
-
-#endif // COSTMAP_2D_UNPREFERRED_LAYER_

include/costmap_2d/voxel_grid.h

@@ -1,32 +0,0 @@
-// Header header
-// uint32[] data
-// geometry_msgs/Point32 origin
-// geometry_msgs/Vector3 resolutions
-// uint32 size_x
-// uint32 size_y
-// uint32 size_z
-
-#ifndef VOXEL_GRID_COSTMAP_2D_H
-#define VOXEL_GRID_COSTMAP_2D_H
-
-#include <vector>
-#include <std_msgs/Header.h>
-#include <geometry_msgs/Point32.h>
-#include <geometry_msgs/Vector3.h>
-
-namespace costmap_2d
-{
-    struct VoxelGrid
-    {
-        std_msgs::Header header;
-        std::vector<uint32_t> data;
-        geometry_msgs::Point32 origin;
-        geometry_msgs::Vector3 resolutions;
-        uint32_t size_x;
-        uint32_t size_y;
-        uint32_t size_z;
-    };
-    
-}
-
-#endif // VOXEL_GRID_COSTMAP_2D_H

include/robot_costmap_2d/array_parser.h

@@ -28,13 +28,13 @@
  *
  * author: Dave Hershberger
  */
-#ifndef COSTMAP_2D_ARRAY_PARSER_H
+#ifndef ROBOT_COSTMAP_2D_ARRAY_PARSER_H
-#define COSTMAP_2D_ARRAY_PARSER_H
+#define ROBOT_COSTMAP_2D_ARRAY_PARSER_H
 
 #include <vector>
 #include <string>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 /** @brief Parse a vector of vectors of floats from a string.
@@ -46,6 +46,6 @@ namespace costmap_2d
  * anything, like part of a successful parse. */
 std::vector<std::vector<float> > parseVVF(const std::string& input, std::string& error_return);
 
-}  // end namespace costmap_2d
+}  // end namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_ARRAY_PARSER_H
+#endif  // ROBOT_COSTMAP_2D_ARRAY_PARSER_H

include/robot_costmap_2d/cost_values.h

@@ -34,10 +34,10 @@
  *
  * Author: Eitan Marder-Eppstein
  *********************************************************************/
-#ifndef COSTMAP_2D_COST_VALUES_H_
+#ifndef ROBOT_COSTMAP_2D_COST_VALUES_H_
-#define COSTMAP_2D_COST_VALUES_H_
+#define ROBOT_COSTMAP_2D_COST_VALUES_H_
 /** Provides a mapping for often used cost values */
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 static const unsigned char NO_INFORMATION = 255;
 static const unsigned char LETHAL_OBSTACLE = 254;
@@ -47,4 +47,4 @@ static const unsigned char FREE_SPACE = 60;
 static const unsigned char PREFERRED_SPACE = 20;
 static const unsigned char UNPREFERRED_SPACE = 100;
 }
-#endif  // COSTMAP_2D_COST_VALUES_H_
+#endif  // ROBOT_COSTMAP_2D_COST_VALUES_H_

include/robot_costmap_2d/costmap_2d.h

@@ -35,15 +35,15 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_COSTMAP_2D_H_
+#ifndef ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_H_
-#define COSTMAP_2D_COSTMAP_2D_H_
+#define ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_H_
 
 #include <vector>
 #include <queue>
-#include <geometry_msgs/Point.h>
+#include <robot_geometry_msgs/Point.h>
 #include <boost/thread.hpp>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 // convenient for storing x/y point pairs
@@ -249,7 +249,7 @@ public:
    * @param cost_value The value to set costs to
    * @return True if the polygon was filled... false if it could not be filled
    */
-  bool setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value);
+  bool setConvexPolygonCost(const std::vector<robot_geometry_msgs::Point>& polygon, unsigned char cost_value);
 
   /**
    * @brief  Get the map cells that make up the outline of a polygon
@@ -464,6 +464,6 @@ protected:
     std::vector<MapLocation>& cells_;
   };
 };
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_COSTMAP_2D_H
+#endif  // ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_H

include/robot_costmap_2d/costmap_2d_robot.h

@@ -35,42 +35,42 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_COSTMAP_2D_ROBOT_H_
+#ifndef ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H_
-#define COSTMAP_2D_COSTMAP_2D_ROBOT_H_
+#define ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H_
 
-#include <costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/layered_costmap.h>
-#include <costmap_2d/layer.h>
+#include <robot_costmap_2d/layer.h>
-#include <costmap_2d/footprint.h>
+#include <robot_costmap_2d/footprint.h>
 
-#include <geometry_msgs/Polygon.h>
+#include <robot_geometry_msgs/Polygon.h>
-#include <geometry_msgs/PolygonStamped.h>
+#include <robot_geometry_msgs/PolygonStamped.h>
-#include <geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/PoseStamped.h>
 
 #include <tf3/LinearMath/Transform.h>
 #include <robot/rate.h>
 #include <data_convert/data_convert.h>
-#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 
-#include <xmlrpcpp/XmlRpcValue.h>
+#include <robot_xmlrpcpp/XmlRpcValue.h>
 
 #include <robot/node_handle.h>
 #include <robot/plugin_loader_helper.h>
-class SuperValue : public XmlRpc::XmlRpcValue
+class SuperValue : public robot::XmlRpc::XmlRpcValue
 {
 public:
-  void setStruct(XmlRpc::XmlRpcValue::ValueStruct* a)
+  void setStruct(robot::XmlRpc::XmlRpcValue::ValueStruct* a)
   {
     _type = TypeStruct;
-    _value.asStruct = new XmlRpc::XmlRpcValue::ValueStruct(*a);
+    _value.asStruct = new robot::XmlRpc::XmlRpcValue::ValueStruct(*a);
   }
-  void setArray(XmlRpc::XmlRpcValue::ValueArray* a)
+  void setArray(robot::XmlRpc::XmlRpcValue::ValueArray* a)
   {
     _type = TypeArray;
-    _value.asArray = new std::vector<XmlRpc::XmlRpcValue>(*a);
+    _value.asArray = new std::vector<robot::XmlRpc::XmlRpcValue>(*a);
   }
 };
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 /** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to
@@ -135,7 +135,7 @@ public:
    * @param global_pose Will be set to the pose of the robot in the global frame of the costmap
    * @return True if the pose was set successfully, false otherwise
    */
-  bool getRobotPose(geometry_msgs::PoseStamped& global_pose) const;
+  bool getRobotPose(robot_geometry_msgs::PoseStamped& global_pose) const;
 
   /** @brief Returns costmap name */
   inline const std::string& getName() const noexcept
@@ -179,10 +179,10 @@ public:
       return layered_costmap_;
     }
 
-  /** @brief Returns the current padded footprint as a geometry_msgs::Polygon. */
+  /** @brief Returns the current padded footprint as a robot_geometry_msgs::Polygon. */
-  geometry_msgs::Polygon getRobotFootprintPolygon() const
+  robot_geometry_msgs::Polygon getRobotFootprintPolygon() const
   {
-    return costmap_2d::toPolygon(padded_footprint_);
+    return robot_costmap_2d::toPolygon(padded_footprint_);
   }
 
   /** @brief Return the current footprint of the robot as a vector of points.
@@ -193,7 +193,7 @@ public:
    * The footprint initially comes from the rosparam "footprint" but
    * can be overwritten by dynamic reconfigure or by messages received
    * on the "footprint" topic. */
-  inline const std::vector<geometry_msgs::Point>& getRobotFootprint() const noexcept
+  inline const std::vector<robot_geometry_msgs::Point>& getRobotFootprint() const noexcept
   {
     return padded_footprint_;
   }
@@ -205,7 +205,7 @@ public:
    * The footprint initially comes from the rosparam "footprint" but
    * can be overwritten by dynamic reconfigure or by messages received
    * on the "footprint" topic. */
-  inline const std::vector<geometry_msgs::Point>& getUnpaddedRobotFootprint() const noexcept
+  inline const std::vector<robot_geometry_msgs::Point>& getUnpaddedRobotFootprint() const noexcept
   {
     return unpadded_footprint_;
   }
@@ -214,7 +214,7 @@ public:
    * @brief  Build the oriented footprint of the robot at the robot's current pose
    * @param  oriented_footprint Will be filled with the points in the oriented footprint of the robot
    */
-  void getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const;
+  void getOrientedFootprint(std::vector<robot_geometry_msgs::Point>& oriented_footprint) const;
 
   /** @brief Set the footprint of the robot to be the given set of
    * points, padded by footprint_padding.
@@ -226,7 +226,7 @@ public:
    * layered_costmap_->setFootprint().  Also saves the unpadded
    * footprint, which is available from
    * getUnpaddedRobotFootprint(). */
-  void setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points);
+  void setUnpaddedRobotFootprint(const std::vector<robot_geometry_msgs::Point>& points);
 
   /** @brief Set the footprint of the robot to be the given polygon,
    * padded by footprint_padding.
@@ -238,7 +238,7 @@ public:
    * layered_costmap_->setFootprint().  Also saves the unpadded
    * footprint, which is available from
    * getUnpaddedRobotFootprint(). */
-  void setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint);
+  void setUnpaddedRobotFootprintPolygon(const robot_geometry_msgs::Polygon& footprint);
 
 protected:
   LayeredCostmap* layered_costmap_;
@@ -253,8 +253,8 @@ private:
    *
    * If the values of footprint and robot_radius are the same in
    * new_config and old_config, nothing is changed. */
-  void readFootprintFromConfig(const std::vector<geometry_msgs::Point> &new_footprint,
+  void readFootprintFromConfig(const std::vector<robot_geometry_msgs::Point> &new_footprint,
-                              const std::vector<geometry_msgs::Point> &old_footprint,
+                              const std::vector<robot_geometry_msgs::Point> &old_footprint,
                               const double &robot_radius);
   std::vector<boost::function<PluginLayerPtr()>> creators_;
   void checkMovement();
@@ -267,14 +267,14 @@ private:
 
   boost::recursive_mutex configuration_mutex_;
 
-  std::vector<geometry_msgs::Point> unpadded_footprint_;
+  std::vector<robot_geometry_msgs::Point> unpadded_footprint_;
-  std::vector<geometry_msgs::Point> padded_footprint_;
+  std::vector<robot_geometry_msgs::Point> padded_footprint_;
   float footprint_padding_;
 
 private:
   void getParams(const std::string& config_file_name, robot::NodeHandle& nh);
 };
 // class Costmap2DROBOT
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_COSTMAP_2D_ROBOT_H
+#endif  // ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H

include/robot_costmap_2d/costmap_layer.h

@@ -35,13 +35,13 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_COSTMAP_LAYER_H_
+#ifndef ROBOT_COSTMAP_2D_COSTMAP_LAYER_H_
-#define COSTMAP_2D_COSTMAP_LAYER_H_
+#define ROBOT_COSTMAP_2D_COSTMAP_LAYER_H_
 
-#include <costmap_2d/layer.h>
+#include <robot_costmap_2d/layer.h>
-#include <costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/layered_costmap.h>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 class CostmapLayer : public Layer, public Costmap2D
@@ -79,7 +79,7 @@ protected:
    * TrueOverwrite means every value from this layer
    * is written into the master grid.
    */
-  void updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+  void updateWithTrueOverwrite(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
   /*
    * Updates the master_grid within the specified
@@ -88,7 +88,7 @@ protected:
    * Overwrite means every valid value from this layer
    * is written into the master grid (does not copy NO_INFORMATION)
    */
-  void updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+  void updateWithOverwrite(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
   /*
    * Updates the master_grid within the specified
@@ -99,7 +99,7 @@ protected:
    * it is overwritten. If the layer's value is NO_INFORMATION,
    * the master value does not change.
    */
-  void updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+  void updateWithMax(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
   /*
    * Updates the master_grid within the specified
@@ -113,7 +113,7 @@ protected:
    * If the sum value is larger than INSCRIBED_INFLATED_OBSTACLE,
    * the master value is set to (INSCRIBED_INFLATED_OBSTACLE - 1).
    */
-  void updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+  void updateWithAddition(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
   /**
    * Updates the bounding box specified in the parameters to include
@@ -147,5 +147,5 @@ private:
   double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_;
 };
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
-#endif  // COSTMAP_2D_COSTMAP_LAYER_H_
+#endif  // ROBOT_COSTMAP_2D_COSTMAP_LAYER_H_

include/robot_costmap_2d/costmap_math.h

@@ -35,13 +35,13 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_COSTMAP_MATH_H_
+#ifndef ROBOT_COSTMAP_2D_COSTMAP_MATH_H_
-#define COSTMAP_2D_COSTMAP_MATH_H_
+#define ROBOT_COSTMAP_2D_COSTMAP_MATH_H_
 
 #include <math.h>
 #include <algorithm>
 #include <vector>
-#include <geometry_msgs/Point.h>
+#include <robot_geometry_msgs/Point.h>
 
 /** @brief Return -1 if x < 0, +1 otherwise. */
 inline double sign(double x)
@@ -62,8 +62,8 @@ inline double distance(double x0, double y0, double x1, double y1)
 
 double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1);
 
-bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float testy);
+bool intersects(std::vector<robot_geometry_msgs::Point>& polygon, float testx, float testy);
 
-bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2);
+bool intersects(std::vector<robot_geometry_msgs::Point>& polygon1, std::vector<robot_geometry_msgs::Point>& polygon2);
 
-#endif  // COSTMAP_2D_COSTMAP_MATH_H_
+#endif  // ROBOT_COSTMAP_2D_COSTMAP_MATH_H_

include/robot_costmap_2d/critical_layer.h

@@ -0,0 +1,19 @@
+#ifndef ROBOT_COSTMAP_2D_CRITICAL_LAYER_H_
+#define ROBOT_COSTMAP_2D_CRITICAL_LAYER_H_
+
+#include <robot_costmap_2d/static_layer.h>
+
+namespace robot_costmap_2d
+{
+class CriticalLayer : public StaticLayer
+{
+public:
+  CriticalLayer();
+  virtual ~CriticalLayer();
+private:
+  unsigned char interpretValue(unsigned char value) override;
+  void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override;
+};
+}
+
+#endif // ROBOT_COSTMAP_2D_CRITICAL_LAYER_H_

include/robot_costmap_2d/directional_layer.h

@@ -1,35 +1,35 @@
-#ifndef COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
+#ifndef ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
-#define COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
+#define ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
 
-#include <costmap_2d/static_layer.h>
+#include <robot_costmap_2d/static_layer.h>
-#include <nav_msgs/Path.h>
+#include <robot_nav_msgs/Path.h>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
   class DirectionalLayer : public StaticLayer
   {
   public:
     DirectionalLayer();
     virtual ~DirectionalLayer();
-    bool laneFilter(const std::vector<geometry_msgs::PoseStamped> plan);
+    bool laneFilter(const std::vector<robot_geometry_msgs::PoseStamped> plan);
     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_;
 	};
 }
 
-#endif // COSTMAP_2D_DIRECTIONAL_LAYER_H_
+#endif // ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_H_

include/robot_costmap_2d/footprint.h

@@ -35,18 +35,18 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_FOOTPRINT_H
+#ifndef ROBOT_COSTMAP_2D_FOOTPRINT_H
-#define COSTMAP_2D_FOOTPRINT_H
+#define ROBOT_COSTMAP_2D_FOOTPRINT_H
 
-#include <geometry_msgs/Polygon.h>
+#include <robot_geometry_msgs/Polygon.h>
-#include <geometry_msgs/PolygonStamped.h>
+#include <robot_geometry_msgs/PolygonStamped.h>
-#include <geometry_msgs/Point.h>
+#include <robot_geometry_msgs/Point.h>
-#include <geometry_msgs/Point32.h>
+#include <robot_geometry_msgs/Point32.h>
 
 #include <robot/node_handle.h>
-#include <xmlrpcpp/XmlRpcValue.h> 
+#include <robot_xmlrpcpp/XmlRpcValue.h> 
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 /**
@@ -56,28 +56,28 @@ namespace costmap_2d
  * @param min_dist Output parameter of the minimum distance
  * @param max_dist Output parameter of the maximum distance
  */
-void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint,
+void calculateMinAndMaxDistances(const std::vector<robot_geometry_msgs::Point>& footprint,
                                  double& min_dist, double& max_dist);
 
 /**
  * @brief Convert Point32 to Point
  */
-geometry_msgs::Point              toPoint(geometry_msgs::Point32 pt);
+robot_geometry_msgs::Point              toPoint(robot_geometry_msgs::Point32 pt);
 
 /**
  * @brief Convert Point to Point32
  */
-geometry_msgs::Point32            toPoint32(geometry_msgs::Point   pt);
+robot_geometry_msgs::Point32            toPoint32(robot_geometry_msgs::Point   pt);
 
 /**
  * @brief Convert vector of Points to Polygon msg
  */
-geometry_msgs::Polygon            toPolygon(std::vector<geometry_msgs::Point> pts);
+robot_geometry_msgs::Polygon            toPolygon(std::vector<robot_geometry_msgs::Point> pts);
 
 /**
  * @brief Convert Polygon msg to vector of Points.
  */
-std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon);
+std::vector<robot_geometry_msgs::Point> toPointVector(robot_geometry_msgs::Polygon polygon);
 
 /**
  * @brief  Given a pose and base footprint, build the oriented footprint of the robot (list of Points)
@@ -87,8 +87,8 @@ std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon);
  * @param  footprint_spec Basic shape of the footprint
  * @param  oriented_footprint Will be filled with the points in the oriented footprint of the robot
 */
-void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
+void transformFootprint(double x, double y, double theta, const std::vector<robot_geometry_msgs::Point>& footprint_spec,
-                        std::vector<geometry_msgs::Point>& oriented_footprint);
+                        std::vector<robot_geometry_msgs::Point>& oriented_footprint);
 
 /**
  * @brief  Given a pose and base footprint, build the oriented footprint of the robot (PolygonStamped)
@@ -98,18 +98,18 @@ void transformFootprint(double x, double y, double theta, const std::vector<geom
  * @param  footprint_spec Basic shape of the footprint
  * @param  oriented_footprint Will be filled with the points in the oriented footprint of the robot
 */
-void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
+void transformFootprint(double x, double y, double theta, const std::vector<robot_geometry_msgs::Point>& footprint_spec,
-                        geometry_msgs::PolygonStamped & oriented_footprint);
+                        robot_geometry_msgs::PolygonStamped & oriented_footprint);
 
 /**
  * @brief Adds the specified amount of padding to the footprint (in place)
  */
-void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding);
+void padFootprint(std::vector<robot_geometry_msgs::Point>& footprint, double padding);
 
 /**
  * @brief Create a circular footprint from a given radius
  */
-std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius);
+std::vector<robot_geometry_msgs::Point> makeFootprintFromRadius(double radius);
 
 /**
  * @brief Make the footprint from the given string.
@@ -117,13 +117,13 @@ std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius);
  * Format should be bracketed array of arrays of floats, like so: [[1.0, 2.2], [3.3, 4.2], ...]
  *
  */
-bool makeFootprintFromString(const std::string& footprint_string, std::vector<geometry_msgs::Point>& footprint);
+bool makeFootprintFromString(const std::string& footprint_string, std::vector<robot_geometry_msgs::Point>& footprint);
 
 /**
  * @brief Read the ros-params "footprint" and/or "robot_radius" from
  * the given NodeHandle using searchParam() to go up the tree.
  */
-std::vector<geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh);
+std::vector<robot_geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh);
 
 /**
  * @brief Create the footprint from the given XmlRpcValue.
@@ -136,14 +136,14 @@ std::vector<geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh)
  * @param full_param_name this is the full name of the rosparam from
  * which the footprint_xmlrpc value came.  It is used only for
  * reporting errors. */
-std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc,
+std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::XmlRpcValue& footprint_xmlrpc,
                                 const std::string& full_param_name);
 
 // /** @brief Write the current unpadded_footprint_ to the "footprint"
 //  * parameter of the given NodeHandle so that dynamic_reconfigure
 //  * will see the new value. */
-// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<geometry_msgs::Point>& footprint);
+// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<robot_geometry_msgs::Point>& footprint);
 
-}  // end namespace costmap_2d
+}  // end namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_FOOTPRINT_H
+#endif  // ROBOT_COSTMAP_2D_FOOTPRINT_H

include/robot_costmap_2d/inflation_layer.h

@@ -35,14 +35,14 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_INFLATION_LAYER_H_
+#ifndef ROBOT_COSTMAP_2D_INFLATION_LAYER_H_
-#define COSTMAP_2D_INFLATION_LAYER_H_
+#define ROBOT_COSTMAP_2D_INFLATION_LAYER_H_
 
-#include <costmap_2d/layer.h>
+#include <robot_costmap_2d/layer.h>
-#include <costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/layered_costmap.h>
 #include <boost/thread.hpp>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 /**
  * @class CellData
@@ -84,7 +84,7 @@ public:
   virtual void onInitialize();
   virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                             double* max_x, double* max_y);
-  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+  virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
   virtual bool isDiscretized()
   {
     return true;
@@ -192,6 +192,6 @@ private:
   bool need_reinflation_;  ///< Indicates that the entire costmap should be reinflated next time around.
 };
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_INFLATION_LAYER_H_
+#endif  // ROBOT_COSTMAP_2D_INFLATION_LAYER_H_

include/robot_costmap_2d/layer.h

@@ -34,16 +34,16 @@
  *
  * Author: David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_LAYER_H_
+#ifndef ROBOT_COSTMAP_2D_LAYER_H_
-#define COSTMAP_2D_LAYER_H_
+#define ROBOT_COSTMAP_2D_LAYER_H_
 
-#include <costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/costmap_2d.h>
-#include <costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/layered_costmap.h>
-#include <costmap_2d/utils.h>
+#include <robot_costmap_2d/utils.h>
 #include <string>
 #include <tf3/buffer_core.h>
 #include <robot/node_handle.h>
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 class LayeredCostmap;
 
@@ -123,7 +123,7 @@ public:
   }
 
   /** @brief Convenience function for layered_costmap_->getFootprint(). */
-  const std::vector<geometry_msgs::Point>& getFootprint() const;
+  const std::vector<robot_geometry_msgs::Point>& getFootprint() const;
 
   /** @brief LayeredCostmap calls this whenever the footprint there
    * changes (via LayeredCostmap::setFootprint()).  Override to be
@@ -159,11 +159,11 @@ protected:
   tf3::BufferCore *tf_;
 
 private:
-  std::vector<geometry_msgs::Point> footprint_spec_;
+  std::vector<robot_geometry_msgs::Point> footprint_spec_;
 };
 
 using PluginLayerPtr = boost::shared_ptr<Layer>;
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_LAYER_H_
+#endif  // ROBOT_COSTMAP_2D_LAYER_H_

include/robot_costmap_2d/layered_costmap.h

@@ -35,16 +35,16 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_LAYERED_COSTMAP_H_
+#ifndef ROBOT_COSTMAP_2D_LAYERED_COSTMAP_H_
-#define COSTMAP_2D_LAYERED_COSTMAP_H_
+#define ROBOT_COSTMAP_2D_LAYERED_COSTMAP_H_
 
-#include <costmap_2d/cost_values.h>
+#include <robot_costmap_2d/cost_values.h>
-#include <costmap_2d/layer.h>
+#include <robot_costmap_2d/layer.h>
-#include <costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/costmap_2d.h>
 #include <vector>
 #include <string>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 class Layer;
 
@@ -101,7 +101,7 @@ public:
 
   bool isTrackingUnknown()
   {
-    return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION;
+    return costmap_.getDefaultValue() == robot_costmap_2d::NO_INFORMATION;
   }
 
   std::vector<boost::shared_ptr<Layer> >* getPlugins()
@@ -135,10 +135,10 @@ public:
   /** @brief Updates the stored footprint, updates the circumscribed
    * and inscribed radii, and calls onFootprintChanged() in all
    * layers. */
-  void setFootprint(const std::vector<geometry_msgs::Point>& footprint_spec);
+  void setFootprint(const std::vector<robot_geometry_msgs::Point>& footprint_spec);
 
   /** @brief Returns the latest footprint stored with setFootprint(). */
-  const std::vector<geometry_msgs::Point>& getFootprint() { return footprint_; }
+  const std::vector<robot_geometry_msgs::Point>& getFootprint() { return footprint_; }
 
   /** @brief The radius of a circle centered at the origin of the
    * robot which just surrounds all points on the robot's
@@ -169,9 +169,9 @@ private:
   bool initialized_;
   bool size_locked_;
   double circumscribed_radius_, inscribed_radius_;
-  std::vector<geometry_msgs::Point> footprint_;
+  std::vector<robot_geometry_msgs::Point> footprint_;
 };
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_LAYERED_COSTMAP_H_
+#endif  // ROBOT_COSTMAP_2D_LAYERED_COSTMAP_H_

include/robot_costmap_2d/observation.h

@@ -29,13 +29,13 @@
  * Authors: Conor McGann
  */
 
-#ifndef COSTMAP_2D_OBSERVATION_H_
+#ifndef ROBOT_COSTMAP_2D_OBSERVATION_H_
-#define COSTMAP_2D_OBSERVATION_H_
+#define ROBOT_COSTMAP_2D_OBSERVATION_H_
 
-#include <geometry_msgs/Point.h>
+#include <robot_geometry_msgs/Point.h>
-#include <sensor_msgs/PointCloud2.h>
+#include <robot_sensor_msgs/PointCloud2.h>
 
-namespace costmap_2d
+namespace robot_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(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,15 +88,15 @@ 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)
   {
   }
 
-  geometry_msgs::Point origin_;
+  robot_geometry_msgs::Point origin_;
-  sensor_msgs::PointCloud2* cloud_;
+  robot_sensor_msgs::PointCloud2* cloud_;
   double obstacle_range_, raytrace_range_;
 };
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
-#endif  // COSTMAP_2D_OBSERVATION_H_
+#endif  // ROBOT_COSTMAP_2D_OBSERVATION_H_

include/robot_costmap_2d/observation_buffer.h

@@ -34,22 +34,22 @@
  *
  * Author: Eitan Marder-Eppstein
  *********************************************************************/
-#ifndef COSTMAP_2D_OBSERVATION_BUFFER_H_
+#ifndef ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
-#define COSTMAP_2D_OBSERVATION_BUFFER_H_
+#define ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
 
 #include <vector>
 #include <list>
 #include <string>
 #include <robot/time.h>
-#include <costmap_2d/observation.h>
+#include <robot_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>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 /**
  * @class ObservationBuffer
@@ -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
@@ -150,5 +150,5 @@ private:
   double obstacle_range_, raytrace_range_;
   double tf_tolerance_;
 };
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
-#endif  // COSTMAP_2D_OBSERVATION_BUFFER_H_
+#endif  // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_

include/robot_costmap_2d/obstacle_layer.h

@@ -35,23 +35,23 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_OBSTACLE_LAYER_H_
+#ifndef ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
-#define COSTMAP_2D_OBSTACLE_LAYER_H_
+#define ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
 
-#include <costmap_2d/costmap_layer.h>
+#include <robot_costmap_2d/costmap_layer.h>
-#include <costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/layered_costmap.h>
-#include <costmap_2d/observation_buffer.h>
+#include <robot_costmap_2d/observation_buffer.h>
-#include <costmap_2d/footprint.h>
+#include <robot_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 <robot_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
+namespace robot_costmap_2d
 {
 
 class ObstacleLayer : public CostmapLayer
@@ -66,14 +66,14 @@ public:
   virtual void onInitialize();
   virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                             double* max_x, double* max_y);
-  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+  virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
   virtual void activate();
   virtual void deactivate();
   virtual void reset();
 
   // for testing purposes
-  void addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing);
+  void addStaticObservation(robot_costmap_2d::Observation& obs, bool marking, bool clearing);
   void clearStaticObservations(bool marking, bool clearing);
 
 protected:
@@ -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,30 +102,30 @@ 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);
+                          const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);
 
   /**
    * @brief  A callback to handle buffering PointCloud2 messages
    * @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);
+                           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
    * @return True if all the observation buffers are current, false otherwise
    */
-  bool getMarkingObservations(std::vector<costmap_2d::Observation>& marking_observations) const;
+  bool getMarkingObservations(std::vector<robot_costmap_2d::Observation>& marking_observations) const;
 
   /**
    * @brief  Get the observations used to clear space
    * @param clearing_observations A reference to a vector that will be populated with the observations
    * @return True if all the observation buffers are current, false otherwise
    */
-  bool getClearingObservations(std::vector<costmap_2d::Observation>& clearing_observations) const;
+  bool getClearingObservations(std::vector<robot_costmap_2d::Observation>& clearing_observations) const;
 
   /**
    * @brief  Clear freespace based on one observation
@@ -135,13 +135,13 @@ protected:
    * @param max_x
    * @param max_y
    */
-  virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
+  virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
                                  double* max_x, double* max_y);
 
   void updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y,
                             double* max_x, double* max_y);
 
-  std::vector<geometry_msgs::Point> transformed_footprint_;
+  std::vector<robot_geometry_msgs::Point> transformed_footprint_;
   bool footprint_clearing_enabled_;
   void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, 
                        double* max_x, double* max_y);
@@ -149,14 +149,14 @@ protected:
   std::string global_frame_;  ///< @brief The global frame for the costmap
   double max_obstacle_height_;  ///< @brief Max Obstacle Height
 
-  laser_geometry::LaserProjection projector_;  ///< @brief Used to project laser scans into point clouds
+  robot_laser_geometry::LaserProjection projector_;  ///< @brief Used to project laser scans into point clouds
 
-  std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > observation_buffers_;  ///< @brief Used to store observations from various sensors
+  std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > observation_buffers_;  ///< @brief Used to store observations from various sensors
-  std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > marking_buffers_;  ///< @brief Used to store observation buffers used for marking obstacles
+  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<costmap_2d::ObservationBuffer> > clearing_buffers_;  ///< @brief Used to store observation buffers used for clearing obstacles
+  std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > clearing_buffers_;  ///< @brief Used to store observation buffers used for clearing obstacles
 
   // Used only for testing purposes
-  std::vector<costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
+  std::vector<robot_costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
 
   bool rolling_window_;
   bool stop_receiving_data_ = false;
@@ -167,6 +167,6 @@ private:
   bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
 };
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_OBSTACLE_LAYER_H_
+#endif  // ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_

include/robot_costmap_2d/preferred_layer.h

@@ -0,0 +1,18 @@
+#ifndef ROBOT_COSTMAP_2D_PREFERRED_LAYER_H_
+#define ROBOT_COSTMAP_2D_PREFERRED_LAYER_H_
+
+#include <robot_costmap_2d/static_layer.h>
+
+namespace robot_costmap_2d
+{
+class PreferredLayer : public StaticLayer
+{
+public:
+  PreferredLayer();
+  virtual ~PreferredLayer();
+private:
+  unsigned char interpretValue(unsigned char value);
+};
+}
+
+#endif // ROBOT_COSTMAP_2D_PREFERRED_LAYER_

include/robot_costmap_2d/static_layer.h

@@ -35,17 +35,17 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_STATIC_LAYER_H_
+#ifndef ROBOT_COSTMAP_2D_STATIC_LAYER_H_
-#define COSTMAP_2D_STATIC_LAYER_H_
+#define ROBOT_COSTMAP_2D_STATIC_LAYER_H_
 
-#include <costmap_2d/costmap_layer.h>
+#include <robot_costmap_2d/costmap_layer.h>
-#include <costmap_2d/layered_costmap.h>
+#include <robot_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
+namespace robot_costmap_2d
 {
 
 class StaticLayer : public CostmapLayer
@@ -60,7 +60,7 @@ public:
 
   virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                             double* max_x, double* max_y);
-  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+  virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
   virtual void matchSize();
 protected:
@@ -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
@@ -91,6 +91,6 @@ private:
   bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
 };
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_STATIC_LAYER_H_
+#endif  // ROBOT_COSTMAP_2D_STATIC_LAYER_H_

include/robot_costmap_2d/testing_helper.h

@@ -0,0 +1,99 @@
+#ifndef ROBOT_COSTMAP_2D_TESTING_HELPER_H
+#define ROBOT_COSTMAP_2D_TESTING_HELPER_H
+
+#include<robot_costmap_2d/cost_values.h>
+#include<robot_costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/static_layer.h>
+#include <robot_costmap_2d/obstacle_layer.h>
+#include <robot_costmap_2d/inflation_layer.h>
+
+#include <robot_sensor_msgs/point_cloud2_iterator.h>
+
+const double MAX_Z(1.0);
+
+char printableCost(unsigned char cost)
+{
+  switch (cost)
+  {
+  case robot_costmap_2d::NO_INFORMATION: return '?';
+  case robot_costmap_2d::LETHAL_OBSTACLE: return 'L';
+  case robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE: return 'I';
+  case robot_costmap_2d::FREE_SPACE: return '.';
+  default: return '0' + (unsigned char) (10 * cost / 255);
+  }
+}
+
+void printMap(robot_costmap_2d::Costmap2D& costmap)
+{
+  printf("map:\n");
+  for (int i = 0; i < costmap.getSizeInCellsY(); i++){
+    for (int j = 0; j < costmap.getSizeInCellsX(); j++){
+      printf("%4d", int(costmap.getCost(j, i)));
+    }
+    printf("\n\n");
+  }
+}
+
+unsigned int countValues(robot_costmap_2d::Costmap2D& costmap, unsigned char value, bool equal = true)
+{
+  unsigned int count = 0;
+  for (int i = 0; i < costmap.getSizeInCellsY(); i++){
+    for (int j = 0; j < costmap.getSizeInCellsX(); j++){
+      unsigned char c = costmap.getCost(j, i);
+      if ((equal && c == value) || (!equal && c != value))
+      {
+        count+=1;
+      }
+    }
+  }
+  return count;
+}
+
+void addStaticLayer(robot_costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
+{
+  robot_costmap_2d::StaticLayer* slayer = new robot_costmap_2d::StaticLayer();
+  layers.addPlugin(boost::shared_ptr<robot_costmap_2d::Layer>(slayer));
+  slayer->initialize(&layers, "static", &tf);
+}
+
+robot_costmap_2d::ObstacleLayer* addObstacleLayer(robot_costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
+{
+  robot_costmap_2d::ObstacleLayer* olayer = new robot_costmap_2d::ObstacleLayer();
+  olayer->initialize(&layers, "obstacles", &tf);
+  layers.addPlugin(boost::shared_ptr<robot_costmap_2d::Layer>(olayer));
+  return olayer;
+}
+
+void addObservation(robot_costmap_2d::ObstacleLayer* olayer, double x, double y, double z = 0.0,
+                    double ox = 0.0, double oy = 0.0, double oz = MAX_Z){
+  robot_sensor_msgs::PointCloud2 cloud;
+  robot_sensor_msgs::PointCloud2Modifier modifier(cloud);
+  modifier.setPointCloud2FieldsByString(1, "xyz");
+  modifier.resize(1);
+  robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
+  robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
+  robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
+  *iter_x = x;
+  *iter_y = y;
+  *iter_z = z;
+
+  robot_geometry_msgs::Point p;
+  p.x = ox;
+  p.y = oy;
+  p.z = oz;
+
+  robot_costmap_2d::Observation obs(p, cloud, 100.0, 100.0);  // obstacle range = raytrace range = 100.0
+  olayer->addStaticObservation(obs, true, true);
+}
+
+robot_costmap_2d::InflationLayer* addInflationLayer(robot_costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
+{
+  robot_costmap_2d::InflationLayer* ilayer = new robot_costmap_2d::InflationLayer();
+  ilayer->initialize(&layers, "inflation", &tf);
+  boost::shared_ptr<robot_costmap_2d::Layer> ipointer(ilayer);
+  layers.addPlugin(ipointer);
+  return ilayer;
+}
+
+
+#endif  // ROBOT_COSTMAP_2D_TESTING_HELPER_H

include/robot_costmap_2d/unpreferred_layer.h

@@ -0,0 +1,21 @@
+#ifndef ROBOT_COSTMAP_2D_UNPREFERRED_LAYER_H_
+#define ROBOT_COSTMAP_2D_UNPREFERRED_LAYER_H_
+
+#include <robot_costmap_2d/static_layer.h>
+
+namespace robot_costmap_2d
+{
+class UnPreferredLayer : public StaticLayer
+{
+public:
+  UnPreferredLayer();
+  virtual ~UnPreferredLayer();
+
+private:
+  unsigned char interpretValue(unsigned char value);
+
+};
+
+}
+
+#endif // ROBOT_COSTMAP_2D_UNPREFERRED_LAYER_

include/robot_costmap_2d/utils.h

@@ -1,11 +1,11 @@
-#ifndef COSTMAP_2D_UTILS_H
+#ifndef ROBOT_COSTMAP_2D_UTILS_H
-#define COSTMAP_2D_UTILS_H
+#define ROBOT_COSTMAP_2D_UTILS_H
 #include <yaml-cpp/yaml.h>
 #include <filesystem>
 #include <string>
 #include <iostream>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
     template<typename T>
     T loadParam(const YAML::Node& node, const std::string& key, const T& default_value)
@@ -15,17 +15,17 @@ namespace costmap_2d
         return default_value;
     }
 
-    inline std::vector<geometry_msgs::Point> loadFootprint(const YAML::Node& node, const std::vector<geometry_msgs::Point>& default_value)
+    inline std::vector<robot_geometry_msgs::Point> loadFootprint(const YAML::Node& node, const std::vector<robot_geometry_msgs::Point>& default_value)
     {
         if( !node || !node.IsDefined())
             return default_value;
 
-        std::vector<geometry_msgs::Point> fp;
+        std::vector<robot_geometry_msgs::Point> fp;
 
         for (const auto& p : node) {
             if (p.size() != 2)
                 throw std::runtime_error("Footprint point must be [x, y]");
-            geometry_msgs::Point point;
+            robot_geometry_msgs::Point point;
             point.x = p[0].as<double>();
             point.y = p[1].as<double>();
             point.z = 0.0;
@@ -64,4 +64,4 @@ namespace costmap_2d
 
 }
 
-#endif  // COSTMAP_2D_UTILS_H
+#endif  // ROBOT_COSTMAP_2D_UTILS_H

include/robot_costmap_2d/voxel_grid.h

@@ -0,0 +1,32 @@
+// Header header
+// uint32[] data
+// geometry_msgs/Point32 origin
+// geometry_msgs/Vector3 resolutions
+// uint32 size_x
+// uint32 size_y
+// uint32 size_z
+
+#ifndef VOXEL_GRID_ROBOT_COSTMAP_2D_H
+#define VOXEL_GRID_ROBOT_COSTMAP_2D_H
+
+#include <vector>
+#include <robot_std_msgs/Header.h>
+#include <robot_geometry_msgs/Point32.h>
+#include <robot_geometry_msgs/Vector3.h>
+
+namespace robot_costmap_2d
+{
+    struct VoxelGrid
+    {
+        robot_std_msgs::Header header;
+        std::vector<uint32_t> data;
+        robot_geometry_msgs::Point32 origin;
+        robot_geometry_msgs::Vector3 resolutions;
+        uint32_t size_x;
+        uint32_t size_y;
+        uint32_t size_z;
+    };
+    
+}
+
+#endif // VOXEL_GRID_ROBOT_COSTMAP_2D_H

include/robot_costmap_2d/voxel_layer.h

@@ -35,30 +35,30 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_VOXEL_LAYER_H_
+#ifndef ROBOT_COSTMAP_2D_VOXEL_LAYER_H_
-#define COSTMAP_2D_VOXEL_LAYER_H_
+#define ROBOT_COSTMAP_2D_VOXEL_LAYER_H_
 
-#include <costmap_2d/layer.h>
+#include <robot_costmap_2d/layer.h>
-#include <costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/layered_costmap.h>
-#include <costmap_2d/observation_buffer.h>
+#include <robot_costmap_2d/observation_buffer.h>
-#include <costmap_2d/voxel_grid.h>
+#include <robot_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 <robot_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 <robot_costmap_2d/obstacle_layer.h>
-#include <voxel_grid/voxel_grid.h>
+#include <robot_voxel_grid/voxel_grid.h>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 class VoxelLayer : public ObstacleLayer
 {
 public:
   VoxelLayer() :
-      voxel_grid_(0, 0, 0)
+      robot_voxel_grid_(0, 0, 0)
   {
     costmap_ = NULL;  // this is the unsigned char* member of parent class's parent class Costmap2D.
   }
@@ -85,15 +85,15 @@ protected:
 private:
   bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
   void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
-  virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
+  virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
                                  double* max_x, double* max_y);
 
 
   bool publish_voxel_;
-  voxel_grid::VoxelGrid voxel_grid_;
+  robot_voxel_grid::VoxelGrid robot_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)
   {
@@ -137,6 +137,6 @@ private:
   }
 };
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_VOXEL_LAYER_H_
+#endif  // ROBOT_COSTMAP_2D_VOXEL_LAYER_H_

package.xml

@@ -0,0 +1,26 @@
+<package>
+  <name>robot_costmap_2d</name>
+  <version>0.7.10</version>
+  <description>
+    robot_costmap_2d is the second generation of the transform library, which lets
+    the user keep track of multiple coordinate frames over time. robot_costmap_2d
+    maintains the relationship between coordinate frames in a tree
+    structure buffered in time, and lets the user transform points,
+    vectors, etc between any two coordinate frames at any desired
+    point in time.
+  </description>
+  <author>Tully Foote</author>
+  <author>Eitan Marder-Eppstein</author>
+  <author>Wim Meeussen</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/robot_costmap_2d</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>libconsole-bridge-dev</build_depend>
+
+  <run_depend>libconsole-bridge-dev</run_depend>
+
+</package>

plugins/critical_layer.cpp

@@ -1,11 +1,11 @@
-#include <costmap_2d/critical_layer.h>
+#include <robot_costmap_2d/critical_layer.h>
 #include <boost/dll/alias.hpp>
 
-using costmap_2d::NO_INFORMATION;
+using robot_costmap_2d::NO_INFORMATION;
-using costmap_2d::LETHAL_OBSTACLE;
+using robot_costmap_2d::LETHAL_OBSTACLE;
-using costmap_2d::CRITICAL_SPACE;
+using robot_costmap_2d::CRITICAL_SPACE;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 CriticalLayer::CriticalLayer(){}
@@ -21,7 +21,7 @@ unsigned char CriticalLayer::interpretValue(unsigned char value)
     return NO_INFORMATION;
 }
 
-void CriticalLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+void CriticalLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 {
   // printf("TEST PLUGIN CRITICAL\n");
   if (!map_received_)

plugins/directional_layer.cpp

@@ -1,36 +1,36 @@
-#include <costmap_2d/directional_layer.h>
+#include <robot_costmap_2d/directional_layer.h>
 #include <data_convert/data_convert.h>
 #include <tf3/convert.h>
 #include <tf3/utils.h>
-#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 
 #include <boost/dll/alias.hpp>
 
 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
 
-using costmap_2d::CRITICAL_SPACE;
+using robot_costmap_2d::CRITICAL_SPACE;
-using costmap_2d::LETHAL_OBSTACLE;
+using robot_costmap_2d::LETHAL_OBSTACLE;
-using costmap_2d::NO_INFORMATION;
+using robot_costmap_2d::NO_INFORMATION;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
   DirectionalLayer::DirectionalLayer()
   {}
   
   DirectionalLayer::~DirectionalLayer() {}
 
-  bool DirectionalLayer::laneFilter(const std::vector<geometry_msgs::PoseStamped> plan)
+  bool DirectionalLayer::laneFilter(const std::vector<robot_geometry_msgs::PoseStamped> plan)
   {
     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_)
     {
@@ -79,7 +79,7 @@ namespace costmap_2d
           int index = getIndex(ix / 2, iy / 2);
           directional_areas_[index][0] = (uint16_t)(color_avg & 0xfff);
           directional_areas_[index][1] = (uint16_t)(color_avg >> 12u);
-          costmap_[index] = costmap_2d::NO_INFORMATION;
+          costmap_[index] = robot_costmap_2d::NO_INFORMATION;
         }
       }
       map_frame_ = new_map.header.frame_id;
@@ -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())
@@ -119,13 +119,13 @@ namespace costmap_2d
     std::vector<std::pair<unsigned int, double>> X_List, Y_List;
     std::vector<double> Yaw_list;
 
-    const geometry_msgs::PoseStamped &e = path.poses.back();
+    const robot_geometry_msgs::PoseStamped &e = path.poses.back();
     
     bool get_success = getRobotPose(pose_x_, pose_y_, pose_yaw_);
     if(!get_success) return false;
     for (auto it = path.poses.begin(); it != path.poses.end(); ++it)
     {
-      const geometry_msgs::PoseStamped &p = *it; // Get the pose from the iterator
+      const robot_geometry_msgs::PoseStamped &p = *it; // Get the pose from the iterator
       unsigned int mx, my;
       if (!worldToMap(p.pose.position.x, p.pose.position.y, mx, my))
       {
@@ -144,7 +144,7 @@ namespace costmap_2d
             || p == path.poses.back()
            )
         {
-          if (value == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || value == costmap_2d::LETHAL_OBSTACLE)
+          if (value == robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE || value == robot_costmap_2d::LETHAL_OBSTACLE)
           {
             std::pair<unsigned int, double> x_val(mx, p.pose.position.x);
             std::pair<unsigned int, double> y_val(my, p.pose.position.y);
@@ -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();
@@ -187,7 +187,7 @@ namespace costmap_2d
     for (int i = 0; i < size_costmap; i++)
     {
       int8_t value;
-      if (costmap[i] == costmap_2d::NO_INFORMATION)
+      if (costmap[i] == robot_costmap_2d::NO_INFORMATION)
       {
         value = -1;
       }
@@ -211,24 +211,24 @@ namespace costmap_2d
   unsigned char DirectionalLayer::laneFilter(unsigned int x, unsigned int y, double yaw_robot)
   {
     double yaw_lane;
-    unsigned char cost = costmap_2d::NO_INFORMATION;
+    unsigned char cost = robot_costmap_2d::NO_INFORMATION;
 
     int index = getIndex(x, y);
     for (auto &lane : directional_areas_[index])
     {
       if (lane > 359)
       {
-        cost = std::min(cost, costmap_2d::NO_INFORMATION);
+        cost = std::min(cost, robot_costmap_2d::NO_INFORMATION);
       }
       else
       {
         double yaw_lane = (double)lane / 180 * M_PI;
         if (0.4 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= 1.0)
-          cost = std::min(cost, costmap_2d::FREE_SPACE);
+          cost = std::min(cost, robot_costmap_2d::FREE_SPACE);
         else if (-1.0 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= -0.4)
-          cost = std::min(cost, costmap_2d::LETHAL_OBSTACLE);
+          cost = std::min(cost, robot_costmap_2d::LETHAL_OBSTACLE);
         else
-          cost = std::min(cost, costmap_2d::NO_INFORMATION);
+          cost = std::min(cost, robot_costmap_2d::NO_INFORMATION);
       }
     }
     return cost;
@@ -278,10 +278,10 @@ namespace costmap_2d
         for (int j = y[i].first; j <= y_max; j++)
         {
           int y_ = j;
-          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+          if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
             continue;
           unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
-          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+          if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
             costmap_[getIndex(x_, y_)] = value;
           else
             break;
@@ -290,10 +290,10 @@ namespace costmap_2d
         for (int k = y[i].first; k >= y_min; k--)
         {
           int y_ = k;
-          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+          if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
             continue;
           unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
-          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+          if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
             costmap_[getIndex(x_, y_)] = value;
           else
             break;
@@ -304,10 +304,10 @@ namespace costmap_2d
         for (int j = x[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsX(); j++)
         {
           int x_ = j;
-          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+          if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
             continue;
           unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
-          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+          if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
             costmap_[getIndex(x_, y_)] = value;
           else
             break;
@@ -316,10 +316,10 @@ namespace costmap_2d
         for (int k = x[i].first; k >= 0; k--)
         {
           int x_ = k;
-          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+          if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
             continue;
           unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
-          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+          if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
             costmap_[getIndex(x_, y_)] = value;
           else
             break;
@@ -332,10 +332,10 @@ namespace costmap_2d
         for (int j = x[i].first; j <= x_max; j++)
         {
           int x_ = j;
-          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+          if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
             continue;
           unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
-          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+          if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
             costmap_[getIndex(x_, y_)] = value;
           else
             break;
@@ -344,10 +344,10 @@ namespace costmap_2d
         for (int k = x[i].first; k >= x_min; k--)
         {
           int x_ = k;
-          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+          if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
             continue;
           unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
-          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+          if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
             costmap_[getIndex(x_, y_)] = value;
           else
             break;
@@ -358,10 +358,10 @@ namespace costmap_2d
         for (int j = y[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsY(); j++)
         {
           int y_ = j;
-          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+          if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
             continue;
           unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
-          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+          if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
             costmap_[getIndex(x_, y_)] = value;
           else
             break;
@@ -370,10 +370,10 @@ namespace costmap_2d
         for (int k = y[i].first; k >= 0; k--)
         {
           int y_ = k;
-          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+          if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
             continue;
           unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
-          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+          if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
             costmap_[getIndex(x_, y_)] = value;
           else
             break;
@@ -391,7 +391,7 @@ namespace costmap_2d
       for (unsigned int ix = 0; ix < size_x; ix++)
       {
         int index = getIndex(ix, iy);
-        costmap_[index] = costmap_2d::NO_INFORMATION;
+        costmap_[index] = robot_costmap_2d::NO_INFORMATION;
       }
     }
   }

plugins/inflation_layer.cpp

@@ -36,18 +36,18 @@
  *         David V. Lu!!
  *********************************************************************/
 #include <algorithm>
-#include <costmap_2d/inflation_layer.h>
+#include <robot_costmap_2d/inflation_layer.h>
-#include <costmap_2d/costmap_math.h>
+#include <robot_costmap_2d/costmap_math.h>
-#include <costmap_2d/footprint.h>
+#include <robot_costmap_2d/footprint.h>
 #include <boost/thread.hpp>
 #include <boost/dll/alias.hpp>
 
 
-using costmap_2d::LETHAL_OBSTACLE;
+using robot_costmap_2d::LETHAL_OBSTACLE;
-using costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
+using robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
-using costmap_2d::NO_INFORMATION;
+using robot_costmap_2d::NO_INFORMATION;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 InflationLayer::InflationLayer()
@@ -91,7 +91,7 @@ void InflationLayer::onInitialize()
 bool InflationLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
 {
     try {
-        std::string folder = COSTMAP_2D_DIR;
+        std::string folder = ROBOT_COSTMAP_2D_DIR;
         std::string path_source = getSourceFile(folder,config_file_name);
 
         YAML::Node config = YAML::LoadFile(path_source);
@@ -132,7 +132,7 @@ bool InflationLayer::getParams(const std::string& config_file_name, robot::NodeH
 void InflationLayer::matchSize()
 {
   boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
-  costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
+  robot_costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
   resolution_ = costmap->getResolution();
   cell_inflation_radius_ = cellDistance(inflation_radius_);
   computeCaches();
@@ -191,7 +191,7 @@ void InflationLayer::onFootprintChanged()
             layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_);
 }
 
-void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 {
   boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
   if (cell_inflation_radius_ == 0)
@@ -416,4 +416,4 @@ static boost::shared_ptr<Layer> create_inflation_plugin() {
 // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
 BOOST_DLL_ALIAS(create_inflation_plugin, InflationLayer)
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d

plugins/obstacle_layer.cpp

@@ -35,23 +35,23 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#include <costmap_2d/obstacle_layer.h>
+#include <robot_costmap_2d/obstacle_layer.h>
-#include <costmap_2d/costmap_math.h>
+#include <robot_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>
 
 
-using costmap_2d::NO_INFORMATION;
+using robot_costmap_2d::NO_INFORMATION;
-using costmap_2d::LETHAL_OBSTACLE;
+using robot_costmap_2d::LETHAL_OBSTACLE;
-using costmap_2d::FREE_SPACE;
+using robot_costmap_2d::FREE_SPACE;
 
-using costmap_2d::ObservationBuffer;
+using robot_costmap_2d::ObservationBuffer;
-using costmap_2d::Observation;
+using robot_costmap_2d::Observation;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 void ObstacleLayer::onInitialize()
@@ -75,7 +75,7 @@ ObstacleLayer::~ObstacleLayer()
 bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
 {
   try {
-      std::string folder = COSTMAP_2D_DIR;
+      std::string folder = ROBOT_COSTMAP_2D_DIR;
       std::string path_source = getSourceFile(folder,config_file_name);
 
       
@@ -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)
     {
@@ -405,11 +405,11 @@ void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot
     }
 }
 
-void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+void ObstacleLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 {
   if (footprint_clearing_enabled_)
   {
-    setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
+    setConvexPolygonCost(transformed_footprint_, robot_costmap_2d::FREE_SPACE);
   }
   
   switch (combination_method_)
@@ -426,7 +426,7 @@ void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, i
 
 }
 
-void ObstacleLayer::addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing)
+void ObstacleLayer::addStaticObservation(robot_costmap_2d::Observation& obs, bool marking, bool clearing)
 {
   if (marking)
     static_marking_observations_.push_back(obs);
@@ -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)
   {
@@ -597,4 +597,4 @@ static boost::shared_ptr<Layer> create_obstacle_plugin() {
 // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
 BOOST_DLL_ALIAS(create_obstacle_plugin, ObstacleLayer)
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d

plugins/preferred_layer.cpp

@@ -1,12 +1,12 @@
-#include <costmap_2d/preferred_layer.h>
+#include <robot_costmap_2d/preferred_layer.h>
 #include <boost/dll/alias.hpp>
 
-using costmap_2d::NO_INFORMATION;
+using robot_costmap_2d::NO_INFORMATION;
-using costmap_2d::FREE_SPACE;
+using robot_costmap_2d::FREE_SPACE;
-using costmap_2d::LETHAL_OBSTACLE;
+using robot_costmap_2d::LETHAL_OBSTACLE;
-using costmap_2d::PREFERRED_SPACE;
+using robot_costmap_2d::PREFERRED_SPACE;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 PreferredLayer::PreferredLayer(){}

plugins/static_layer.cpp

@@ -36,20 +36,20 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#include <costmap_2d/static_layer.h>
+#include <robot_costmap_2d/static_layer.h>
-#include <costmap_2d/costmap_math.h>
+#include <robot_costmap_2d/costmap_math.h>
 
 #include <tf3/convert.h>
-#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 
 #include <boost/dll/alias.hpp>
 
 
-using costmap_2d::NO_INFORMATION;
+using robot_costmap_2d::NO_INFORMATION;
-using costmap_2d::LETHAL_OBSTACLE;
+using robot_costmap_2d::LETHAL_OBSTACLE;
-using costmap_2d::FREE_SPACE;
+using robot_costmap_2d::FREE_SPACE;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 StaticLayer::StaticLayer()
@@ -75,7 +75,7 @@ void StaticLayer::onInitialize()
 bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
 {
     try {
-        std::string folder = COSTMAP_2D_DIR;
+        std::string folder = ROBOT_COSTMAP_2D_DIR;
         std::string path_source = getSourceFile(folder,config_file_name);
         
         YAML::Node config = YAML::LoadFile(path_source);
@@ -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_)
   {
@@ -324,7 +324,7 @@ void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
   has_updated_data_ = false;
 }
 
-void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+void StaticLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 {
   if (!map_received_)
     return;
@@ -343,7 +343,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
     unsigned int mx, my;
     double wx, wy;
     // Might even be in a different frame
-    // geometry_msgs::TransformStamped transform;
+    // robot_geometry_msgs::TransformStamped transform;
     tf3::TransformStampedMsg transformMsg;
     try
     {
@@ -389,4 +389,4 @@ static boost::shared_ptr<Layer> create_static_plugin() {
 // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
 BOOST_DLL_ALIAS(create_static_plugin, StaticLayer)
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d

plugins/unpreferred_layer.cpp

@@ -1,11 +1,11 @@
-#include <costmap_2d/unpreferred_layer.h>
+#include <robot_costmap_2d/unpreferred_layer.h>
 #include <boost/dll/alias.hpp>
 
-using costmap_2d::NO_INFORMATION;
+using robot_costmap_2d::NO_INFORMATION;
-using costmap_2d::LETHAL_OBSTACLE;
+using robot_costmap_2d::LETHAL_OBSTACLE;
-using costmap_2d::PREFERRED_SPACE;
+using robot_costmap_2d::PREFERRED_SPACE;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 UnPreferredLayer::UnPreferredLayer(){}

plugins/voxel_layer.cpp

@@ -35,20 +35,20 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#include <costmap_2d/voxel_layer.h>
+#include <robot_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
 
-using costmap_2d::NO_INFORMATION;
+using robot_costmap_2d::NO_INFORMATION;
-using costmap_2d::LETHAL_OBSTACLE;
+using robot_costmap_2d::LETHAL_OBSTACLE;
-using costmap_2d::FREE_SPACE;
+using robot_costmap_2d::FREE_SPACE;
 
-using costmap_2d::ObservationBuffer;
+using robot_costmap_2d::ObservationBuffer;
-using costmap_2d::Observation;
+using robot_costmap_2d::Observation;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 void VoxelLayer::onInitialize()
@@ -68,7 +68,7 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
 {
   try 
   {
-    std::string folder = COSTMAP_2D_DIR;
+    std::string folder = ROBOT_COSTMAP_2D_DIR;
     std::string path_source = getSourceFile(folder,config_file_name);
     
     YAML::Node config = YAML::LoadFile(path_source);
@@ -126,11 +126,11 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
 void VoxelLayer::matchSize()
 {
   ObstacleLayer::matchSize();
-  voxel_grid_.resize(size_x_, size_y_, size_z_);
+  robot_voxel_grid_.resize(size_x_, size_y_, size_z_);
-  if (!(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_)) 
+  if (!(robot_voxel_grid_.sizeX() == size_x_ && robot_voxel_grid_.sizeY() == size_y_)) 
   {
     std::cerr << "[FATAL] Voxel grid size mismatch: "
-              << "voxel(" << voxel_grid_.sizeX() << ", " << voxel_grid_.sizeY()
+              << "voxel(" << robot_voxel_grid_.sizeX() << ", " << robot_voxel_grid_.sizeY()
               << ") but costmap(" << size_x_ << ", " << size_y_ << ")\n";
     std::abort();  // dừng chương trình
   }
@@ -140,14 +140,14 @@ void VoxelLayer::reset()
 {
   deactivate();
   resetMaps();
-  voxel_grid_.reset();
+  robot_voxel_grid_.reset();
   activate();
 }
 
 void VoxelLayer::resetMaps()
 {
   Costmap2D::resetMaps();
-  voxel_grid_.reset();
+  robot_voxel_grid_.reset();
 }
 
 void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
@@ -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)
     {
@@ -216,7 +216,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
       }
 
       // mark the cell in the voxel grid and check if we should also mark it in the costmap
-      if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
+      if (robot_voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
       {
         unsigned int index = getIndex(mx, my);
 
@@ -228,13 +228,13 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
 
   // if (publish_voxel_)
   // {
-  //   costmap_2d::VoxelGrid grid_msg;
+  //   robot_costmap_2d::VoxelGrid grid_msg;
-  //   unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
+  //   unsigned int size = robot_voxel_grid_.sizeX() * robot_voxel_grid_.sizeY();
-  //   grid_msg.size_x = voxel_grid_.sizeX();
+  //   grid_msg.size_x = robot_voxel_grid_.sizeX();
-  //   grid_msg.size_y = voxel_grid_.sizeY();
+  //   grid_msg.size_y = robot_voxel_grid_.sizeY();
-  //   grid_msg.size_z = voxel_grid_.sizeZ();
+  //   grid_msg.size_z = robot_voxel_grid_.sizeZ();
   //   grid_msg.data.resize(size);
-  //   memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
+  //   memcpy(&grid_msg.data[0], robot_voxel_grid_.getData(), size * sizeof(unsigned int));
 
   //   grid_msg.origin.x = origin_x_;
   //   grid_msg.origin.y = origin_y_;
@@ -291,7 +291,7 @@ void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_
         if (clear_no_info || *current != NO_INFORMATION)
         {
           *current = FREE_SPACE;
-          voxel_grid_.clearVoxelColumn(index);
+          robot_voxel_grid_.clearVoxelColumn(index);
         }
       }
       current++;
@@ -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)
   {
@@ -397,8 +397,8 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
     {
       unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
 
-      // voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
+      // robot_voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
-      voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
+      robot_voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
                                       unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
                                       cell_raytrace_range);
 
@@ -406,7 +406,7 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
 
       // if (publish_clearing_points)
       // {
-        geometry_msgs::Point32 point;
+        robot_geometry_msgs::Point32 point;
         point.x = wpx;
         point.y = wpy;
         point.z = wpz;
@@ -455,7 +455,7 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
   // we need a map to store the obstacles in the window temporarily
   unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
   unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
-  unsigned int* voxel_map = voxel_grid_.getData();
+  unsigned int* voxel_map = robot_voxel_grid_.getData();
 
   // copy the local window in the costmap to the local map
   copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
@@ -490,4 +490,4 @@ static boost::shared_ptr<Layer> create_voxel_plugin() {
 // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
 BOOST_DLL_ALIAS(create_voxel_plugin, VoxelLayer)
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d

src/array_parser.cpp

@@ -34,7 +34,7 @@
 #include <sstream>
 #include <vector>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 /** @brief Parse a vector of vector of floats from a string.
@@ -112,4 +112,4 @@ std::vector<std::vector<float> > parseVVF(const std::string& input, std::string&
   return result;
 }
 
-}  // end namespace costmap_2d
+}  // end namespace robot_costmap_2d

src/costmap_2d.cpp

@@ -35,12 +35,12 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#include <costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/costmap_2d.h>
 #include <cstdio>
 
 using namespace std;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
                      double origin_x, double origin_y, unsigned char default_value) :
@@ -312,7 +312,7 @@ void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
   delete[] local_map;
 }
 
-bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)
+bool Costmap2D::setConvexPolygonCost(const std::vector<robot_geometry_msgs::Point>& polygon, unsigned char cost_value)
 {
   // we assume the polygon is given in the global_frame... we need to transform it to map coordinates
   std::vector<MapLocation> map_polygon;
@@ -485,4 +485,4 @@ bool Costmap2D::saveMap(std::string file_name)
   return true;
 }
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d

src/costmap_2d_robot.cpp

@@ -41,9 +41,9 @@
 #include <algorithm>
 #include <vector>
 
-#include <costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/layered_costmap.h>
-#include <costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
-#include <costmap_2d/utils.h>
+#include <robot_costmap_2d/utils.h>
 
 #include <tf3/convert.h>
 #include <tf3/utils.h>
@@ -53,7 +53,7 @@
 
 using namespace std;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
     layered_costmap_(NULL),
@@ -83,11 +83,11 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
 {
   try 
   {
-    std::string folder = COSTMAP_2D_DIR;
+    std::string folder = ROBOT_COSTMAP_2D_DIR;
     std::string path_source = getSourceFile(folder,config_file_name);
 
     YAML::Node config = YAML::LoadFile(path_source);
-    YAML::Node layer = config["costmap_2d"];
+    YAML::Node layer = config["robot_costmap_2d"];
 
     std::string global_frame =
     loadParam(layer, "global_frame", std::string("map"));
@@ -227,7 +227,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
       }
     }
 
-    std::vector<geometry_msgs::Point> new_footprint;// = loadParam(layer, "footprint", 0.0);
+    std::vector<robot_geometry_msgs::Point> new_footprint;// = loadParam(layer, "footprint", 0.0);
     new_footprint = loadFootprint(layer["footprint"], new_footprint);
     transform_tolerance_ = loadParam(layer, "transform_tolerance", 0.0);
 
@@ -306,7 +306,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
   }
 }
 
-void Costmap2DROBOT::setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint)
+void Costmap2DROBOT::setUnpaddedRobotFootprintPolygon(const robot_geometry_msgs::Polygon& footprint)
 { 
   setUnpaddedRobotFootprint(toPointVector(footprint));
 }
@@ -324,8 +324,8 @@ Costmap2DROBOT::~Costmap2DROBOT()
 }
 
 
-void Costmap2DROBOT::readFootprintFromConfig(const std::vector<geometry_msgs::Point> &new_footprint,
+void Costmap2DROBOT::readFootprintFromConfig(const std::vector<robot_geometry_msgs::Point> &new_footprint,
-                                           const std::vector<geometry_msgs::Point> &old_footprint,
+                                           const std::vector<robot_geometry_msgs::Point> &old_footprint,
                                            const double &robot_radius)
 {
   // Only change the footprint if footprint or robot_radius has
@@ -354,7 +354,7 @@ void Costmap2DROBOT::readFootprintFromConfig(const std::vector<geometry_msgs::Po
   }
 }
 
-void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points)
+void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vector<robot_geometry_msgs::Point>& points)
 {
   unpadded_footprint_ = points;
   padded_footprint_ = points;
@@ -365,7 +365,7 @@ void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::
 
 void Costmap2DROBOT::checkMovement()
 {
-    geometry_msgs::PoseStamped new_pose;
+    robot_geometry_msgs::PoseStamped new_pose;
     if (!getRobotPose(new_pose))
         std::cout << "Cannot get robot pose\n";
 }
@@ -390,14 +390,14 @@ void Costmap2DROBOT::updateMap()
   if (!stop_updates_)
   {
     // get global pose
-    geometry_msgs::PoseStamped pose;
+    robot_geometry_msgs::PoseStamped pose;
     if (getRobotPose (pose))
     {
       double x = pose.pose.position.x,
              y = pose.pose.position.y,
              yaw = data_convert::getYaw(pose.pose.orientation);
       layered_costmap_->updateMap(x, y, yaw);
-      geometry_msgs::PolygonStamped footprint;
+      robot_geometry_msgs::PolygonStamped footprint;
       footprint.header.frame_id = global_frame_;
       footprint.header.stamp = robot::Time::now();
       transformFootprint(x, y, yaw, padded_footprint_, footprint);
@@ -495,10 +495,10 @@ void Costmap2DROBOT::resetLayers()
   }
 }
 
-bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
+bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose) const
 {
-  geometry_msgs::PoseStamped robot_pose;
+  robot_geometry_msgs::PoseStamped robot_pose;
-  geometry_msgs::Pose pose_default;
+  robot_geometry_msgs::Pose pose_default;
   global_pose.pose = pose_default;
   robot_pose.pose = pose_default;
 
@@ -555,10 +555,10 @@ bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
   return true;
 }
 
-void Costmap2DROBOT::getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const
+void Costmap2DROBOT::getOrientedFootprint(std::vector<robot_geometry_msgs::Point>& oriented_footprint) const
 {
   // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::getOrientedFootprint");
-  geometry_msgs::PoseStamped global_pose;
+  robot_geometry_msgs::PoseStamped global_pose;
   if (!getRobotPose(global_pose))
     return;
 
@@ -567,4 +567,4 @@ void Costmap2DROBOT::getOrientedFootprint(std::vector<geometry_msgs::Point>& ori
                      padded_footprint_, oriented_footprint);
 }
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d

src/costmap_layer.cpp

@@ -1,6 +1,6 @@
-#include<costmap_2d/costmap_layer.h>
+#include<robot_costmap_2d/costmap_layer.h>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 void CostmapLayer::touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y)
@@ -60,7 +60,7 @@ void CostmapLayer::useExtraBounds(double* min_x, double* min_y, double* max_x, d
     has_extra_bounds_ = false;
 }
 
-void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+void CostmapLayer::updateWithMax(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 {
   if (!enabled_)
     return;
@@ -100,7 +100,7 @@ void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i,
   }
 }
 
-void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
+void CostmapLayer::updateWithTrueOverwrite(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
                                            int max_i, int max_j)
 {
   if (!enabled_)
@@ -119,7 +119,7 @@ void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, i
   }
 }
 
-void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+void CostmapLayer::updateWithOverwrite(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 {
   if (!enabled_)
     return;
@@ -138,7 +138,7 @@ void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int m
   }
 }
 
-void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+void CostmapLayer::updateWithAddition(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 {
   if (!enabled_)
     return;
@@ -161,8 +161,8 @@ void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int mi
       else
       {
         int sum = old_cost + costmap_[it];
-        if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+        if (sum >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
-            master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
+            master_array[it] = robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
         else
             master_array[it] = sum;
       }
@@ -170,4 +170,4 @@ void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int mi
     }
   }
 }
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d

src/costmap_math.cpp

@@ -27,7 +27,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <costmap_2d/costmap_math.h>
+#include <robot_costmap_2d/costmap_math.h>
 
 double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)
 {
@@ -61,7 +61,7 @@ double distanceToLine(double pX, double pY, double x0, double y0, double x1, dou
   return distance(pX, pY, xx, yy);
 }
 
-bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float testy)
+bool intersects(std::vector<robot_geometry_msgs::Point>& polygon, float testx, float testy)
 {
   bool c = false;
   int i, j, nvert = polygon.size();
@@ -75,7 +75,7 @@ bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float t
   return c;
 }
 
-bool intersects_helper(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2)
+bool intersects_helper(std::vector<robot_geometry_msgs::Point>& polygon1, std::vector<robot_geometry_msgs::Point>& polygon2)
 {
   for (unsigned int i = 0; i < polygon1.size(); i++)
     if (intersects(polygon2, polygon1[i].x, polygon1[i].y))
@@ -83,7 +83,7 @@ bool intersects_helper(std::vector<geometry_msgs::Point>& polygon1, std::vector<
   return false;
 }
 
-bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2)
+bool intersects(std::vector<robot_geometry_msgs::Point>& polygon1, std::vector<robot_geometry_msgs::Point>& polygon2)
 {
   return intersects_helper(polygon1, polygon2) || intersects_helper(polygon2, polygon1);
 }

src/footprint.cpp

@@ -27,18 +27,18 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <costmap_2d/costmap_math.h>
+#include <robot_costmap_2d/costmap_math.h>
 #include <boost/tokenizer.hpp>
 #include <boost/foreach.hpp>
 #include <boost/algorithm/string.hpp>
-#include <costmap_2d/footprint.h>
+#include <robot_costmap_2d/footprint.h>
-#include <costmap_2d/array_parser.h>
+#include <robot_costmap_2d/array_parser.h>
-#include <geometry_msgs/Point32.h>
+#include <robot_geometry_msgs/Point32.h>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
-void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
+void calculateMinAndMaxDistances(const std::vector<robot_geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
 {
   min_dist = std::numeric_limits<double>::max();
   max_dist = 0.0;
@@ -66,36 +66,36 @@ void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footpr
   max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
 }
 
-geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt)
+robot_geometry_msgs::Point32 toPoint32(robot_geometry_msgs::Point pt)
 {
-  geometry_msgs::Point32 point32;
+  robot_geometry_msgs::Point32 point32;
   point32.x = pt.x;
   point32.y = pt.y;
   point32.z = pt.z;
   return point32;
 }
 
-geometry_msgs::Point toPoint(geometry_msgs::Point32 pt)
+robot_geometry_msgs::Point toPoint(robot_geometry_msgs::Point32 pt)
 {
-  geometry_msgs::Point point;
+  robot_geometry_msgs::Point point;
   point.x = pt.x;
   point.y = pt.y;
   point.z = pt.z;
   return point;
 }
 
-geometry_msgs::Polygon toPolygon(std::vector<geometry_msgs::Point> pts)
+robot_geometry_msgs::Polygon toPolygon(std::vector<robot_geometry_msgs::Point> pts)
 {
-  geometry_msgs::Polygon polygon;
+  robot_geometry_msgs::Polygon polygon;
   for (int i = 0; i < pts.size(); i++){
     polygon.points.push_back(toPoint32(pts[i]));
   }
   return polygon;
 }
 
-std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon)
+std::vector<robot_geometry_msgs::Point> toPointVector(robot_geometry_msgs::Polygon polygon)
 {
-  std::vector<geometry_msgs::Point> pts;
+  std::vector<robot_geometry_msgs::Point> pts;
   for (int i = 0; i < polygon.points.size(); i++)
   {
     pts.push_back(toPoint(polygon.points[i]));
@@ -103,8 +103,8 @@ std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon)
   return pts;
 }
 
-void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
+void transformFootprint(double x, double y, double theta, const std::vector<robot_geometry_msgs::Point>& footprint_spec,
-                        std::vector<geometry_msgs::Point>& oriented_footprint)
+                        std::vector<robot_geometry_msgs::Point>& oriented_footprint)
 {
   // build the oriented footprint at a given location
   oriented_footprint.clear();
@@ -112,15 +112,15 @@ void transformFootprint(double x, double y, double theta, const std::vector<geom
   double sin_th = sin(theta);
   for (unsigned int i = 0; i < footprint_spec.size(); ++i)
   {
-    geometry_msgs::Point new_pt;
+    robot_geometry_msgs::Point new_pt;
     new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
     new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
     oriented_footprint.push_back(new_pt);
   }
 }
 
-void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
+void transformFootprint(double x, double y, double theta, const std::vector<robot_geometry_msgs::Point>& footprint_spec,
-                        geometry_msgs::PolygonStamped& oriented_footprint)
+                        robot_geometry_msgs::PolygonStamped& oriented_footprint)
 {
   // build the oriented footprint at a given location
   oriented_footprint.polygon.points.clear();
@@ -128,32 +128,32 @@ void transformFootprint(double x, double y, double theta, const std::vector<geom
   double sin_th = sin(theta);
   for (unsigned int i = 0; i < footprint_spec.size(); ++i)
   {
-    geometry_msgs::Point32 new_pt;
+    robot_geometry_msgs::Point32 new_pt;
     new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
     new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
     oriented_footprint.polygon.points.push_back(new_pt);
   }
 }
 
-void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding)
+void padFootprint(std::vector<robot_geometry_msgs::Point>& footprint, double padding)
 {
   // pad footprint in place
   for (unsigned int i = 0; i < footprint.size(); i++)
   {
-    geometry_msgs::Point& pt = footprint[ i ];
+    robot_geometry_msgs::Point& pt = footprint[ i ];
     pt.x += sign0(pt.x) * padding;
     pt.y += sign0(pt.y) * padding;
   }
 }
 
 
-std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius)
+std::vector<robot_geometry_msgs::Point> makeFootprintFromRadius(double radius)
 {
-  std::vector<geometry_msgs::Point> points;
+  std::vector<robot_geometry_msgs::Point> points;
 
   // Loop over 16 angles around a circle making a point each time
   int N = 16;
-  geometry_msgs::Point pt;
+  robot_geometry_msgs::Point pt;
   for (int i = 0; i < N; ++i)
   {
     double angle = i * 2 * M_PI / N;
@@ -167,7 +167,7 @@ std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius)
 }
 
 
-bool makeFootprintFromString(const std::string& footprint_string, std::vector<geometry_msgs::Point>& footprint)
+bool makeFootprintFromString(const std::string& footprint_string, std::vector<robot_geometry_msgs::Point>& footprint)
 {
   std::string error;
   std::vector<std::vector<float> > vvf = parseVVF(footprint_string, error);
@@ -190,7 +190,7 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
   {
     if (vvf[ i ].size() == 2)
     {
-      geometry_msgs::Point point;
+      robot_geometry_msgs::Point point;
       point.x = vvf[ i ][ 0 ];
       point.y = vvf[ i ][ 1 ];
       point.z = 0;
@@ -207,17 +207,17 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
   return true;
 }
 
-std::vector<geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh)
+std::vector<robot_geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh)
 {
   YAML::Node ft = nh.getParamValue("footprint");
-  std::vector<geometry_msgs::Point> footprint;
+  std::vector<robot_geometry_msgs::Point> footprint;
 
   if (ft && ft.IsSequence())
   {
     for (size_t i = 0; i < ft.size(); ++i)
     {
         auto pt = ft[i];  
-        geometry_msgs::Point p;
+        robot_geometry_msgs::Point p;
         p.x = pt[0].as<double>();
         p.y = pt[1].as<double>();
         p.z = 0.0;
@@ -228,13 +228,13 @@ std::vector<geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh)
   return footprint;
 }
 
-// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<geometry_msgs::Point>& footprint)
+// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<robot_geometry_msgs::Point>& footprint)
 // {
 //   std::ostringstream oss;
 //   bool first = true;
 //   for (unsigned int i = 0; i < footprint.size(); i++)
 //   {
-//     geometry_msgs::Point p = footprint[ i ];
+//     robot_geometry_msgs::Point p = footprint[ i ];
 //     if (first)
 //     {
 //       oss << "[[" << p.x << "," << p.y << "]";
@@ -249,25 +249,25 @@ std::vector<geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh)
 //   nh.setParam("footprint", oss.str().c_str());
 // }
 
-double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name)
+double getNumberFromXMLRPC(robot::XmlRpc::XmlRpcValue& value, const std::string& full_param_name)
 {
   // Make sure that the value we're looking at is either a double or an int.
-  if (value.getType() != XmlRpc::XmlRpcValue::TypeInt &&
+  if (value.getType() != robot::XmlRpc::XmlRpcValue::TypeInt &&
-      value.getType() != XmlRpc::XmlRpcValue::TypeDouble)
+      value.getType() != robot::XmlRpc::XmlRpcValue::TypeDouble)
   {
     std::string& value_string = value;
     printf("Values in the footprint specification (param %s) must be numbers. Found value %s.\n",
                full_param_name.c_str(), value_string.c_str());
     throw std::runtime_error("Values in the footprint specification must be numbers");
   }
-  return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
+  return value.getType() == robot::XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
 }
 
-std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc,
+std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::XmlRpcValue& footprint_xmlrpc,
                                 const std::string& full_param_name)
 {
   // Make sure we have an array of at least 3 elements.
-  if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray ||
+  if (footprint_xmlrpc.getType() != robot::XmlRpc::XmlRpcValue::TypeArray ||
       footprint_xmlrpc.size() < 3)
   {
     printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s\n",
@@ -276,14 +276,14 @@ std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& f
                              "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
   }
 
-  std::vector<geometry_msgs::Point> footprint;
+  std::vector<robot_geometry_msgs::Point> footprint;
-  geometry_msgs::Point pt;
+  robot_geometry_msgs::Point pt;
 
   for (int i = 0; i < footprint_xmlrpc.size(); ++i)
   {
     // Make sure each element of the list is an array of size 2. (x and y coordinates)
-    XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ];
+    robot::XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ];
-    if (point.getType() != XmlRpc::XmlRpcValue::TypeArray ||
+    if (point.getType() != robot::XmlRpc::XmlRpcValue::TypeArray ||
         point.size() != 2)
     {
       printf("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "
@@ -301,4 +301,4 @@ std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& f
   return footprint;
 }
 
-}  // end namespace costmap_2d
+}  // end namespace robot_costmap_2d

src/layer.cpp

@@ -27,9 +27,9 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include "costmap_2d/layer.h"
+#include "robot_costmap_2d/layer.h"
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 Layer::Layer()
@@ -48,9 +48,9 @@ void Layer::initialize(LayeredCostmap* parent, std::string name, tf3::BufferCore
   onInitialize();
 }
 
-const std::vector<geometry_msgs::Point>& Layer::getFootprint() const
+const std::vector<robot_geometry_msgs::Point>& Layer::getFootprint() const
 {
   return layered_costmap_->getFootprint();
 }
 
-}  // end namespace costmap_2d
+}  // end namespace robot_costmap_2d

src/layered_costmap.cpp

@@ -35,8 +35,8 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#include <costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/layered_costmap.h>
-#include <costmap_2d/footprint.h>
+#include <robot_costmap_2d/footprint.h>
 #include <cstdio>
 #include <string>
 #include <algorithm>
@@ -44,7 +44,7 @@
 
 using std::vector;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
   LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown)
@@ -175,10 +175,10 @@ namespace costmap_2d
     return current_;
   }
 
-  void LayeredCostmap::setFootprint(const std::vector<geometry_msgs::Point> &footprint_spec)
+  void LayeredCostmap::setFootprint(const std::vector<robot_geometry_msgs::Point> &footprint_spec)
   {
     footprint_ = footprint_spec;
-    costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
+    robot_costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
 
     for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
          ++plugin)
@@ -187,4 +187,4 @@ namespace costmap_2d
     }
   }
 
-} // namespace costmap_2d
+} // namespace robot_costmap_2d

src/observation_buffer.cpp

@@ -34,16 +34,16 @@
  *
  * Author: Eitan Marder-Eppstein
  *********************************************************************/
-#include <costmap_2d/observation_buffer.h>
+#include <robot_costmap_2d/observation_buffer.h>
 
-#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
-#include <tf3_sensor_msgs/tf3_sensor_msgs.h>
+#include <robot_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;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
                                      double min_obstacle_height, double max_obstacle_height, double obstacle_range,
@@ -65,7 +65,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
   tf3::Time transform_time = tf3::Time::now();
   std::string tf_error;
 
-  geometry_msgs::TransformStamped transformStamped;
+  robot_geometry_msgs::TransformStamped transformStamped;
   if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, transform_time, &tf_error))
   {
     printf("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
@@ -80,7 +80,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
     {
       Observation& obs = *obs_it;
 
-      geometry_msgs::PointStamped origin;
+      robot_geometry_msgs::PointStamped origin;
       origin.header.frame_id = global_frame_;
       origin.header.stamp = data_convert::convertTime(transform_time);
       origin.point = obs.origin_;
@@ -117,9 +117,9 @@ 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)
 {
-  geometry_msgs::PointStamped global_origin;
+  robot_geometry_msgs::PointStamped global_origin;
 
   // create a new observation on the list to be populated
   observation_list_.push_front(Observation());
@@ -130,7 +130,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
   try
   {
     // given these observations come from sensors... we'll need to store the origin pt of the sensor
-    geometry_msgs::PointStamped local_origin;
+    robot_geometry_msgs::PointStamped local_origin;
     local_origin.header.stamp = cloud.header.stamp;
     local_origin.header.frame_id = origin_frame;
     local_origin.point.x = 0;
@@ -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)
@@ -276,5 +276,5 @@ void ObservationBuffer::resetLastUpdated()
 {
   last_updated_ = robot::Time::now();
 }
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 

test/array_parser_test.cpp

@@ -29,9 +29,9 @@
 
 #include <gtest/gtest.h>
 
-#include "costmap_2d/array_parser.h"
+#include "robot_costmap_2d/array_parser.h"
 
-using namespace costmap_2d;
+using namespace robot_costmap_2d;
 
 TEST(array_parser, basic_operation)
 {

test/coordinates_test.cpp

@@ -35,9 +35,9 @@
 // Tests ripped from https://github.com/locusrobotics/robot_navigation/blob/master/nav_grid/test/utest.cpp
 
 #include <gtest/gtest.h>
-#include <costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/costmap_2d.h>
 
-using namespace costmap_2d;
+using namespace robot_costmap_2d;
 
 TEST(CostmapCoordinates, easy_coordinates_test)
 {

test/static_layer_test.cpp

@@ -1,28 +1,28 @@
-#include <costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
-#include <costmap_2d/cost_values.h>
+#include <robot_costmap_2d/cost_values.h>
 #include <gtest/gtest.h>
 #include <tf3/buffer_core.h>
-#include <geometry_msgs/PoseWithCovariance.h>
+#include <robot_geometry_msgs/PoseWithCovariance.h>
 
-namespace costmap_2d {
+namespace robot_costmap_2d {
 
 class CostmapTester : public testing::Test {
   public:
     CostmapTester(tf3::BufferCore& tf);
     void checkConsistentCosts();
-    void compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y);
+    void compareCellToNeighbors(robot_costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y);
-    void compareCells(costmap_2d::Costmap2D& costmap, 
+    void compareCells(robot_costmap_2d::Costmap2D& costmap, 
         unsigned int x, unsigned int y, unsigned int nx, unsigned int ny);
     virtual void TestBody(){}
 
   private:
-    costmap_2d::Costmap2DROBOT costmap_ros_;
+    robot_costmap_2d::Costmap2DROBOT costmap_ros_;
 };
 
 CostmapTester::CostmapTester(tf3::BufferCore& tf): costmap_ros_("costmap_params", tf){}
 
 void CostmapTester::checkConsistentCosts(){
-  costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap();
+  robot_costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap();
 
   //get a copy of the costmap contained by our ros wrapper
   costmap->saveMap("costmap_test.pgm");
@@ -35,7 +35,7 @@ void CostmapTester::checkConsistentCosts(){
   }
 }
 
-void CostmapTester::compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y){
+void CostmapTester::compareCellToNeighbors(robot_costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y){
   //we'll compare the cost of this cell with that of its eight neighbors to see if they're reasonable
   for(int offset_x = -1; offset_x <= 1; ++offset_x){
     for(int offset_y = -1; offset_y <= 1; ++offset_y){
@@ -51,18 +51,18 @@ void CostmapTester::compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsig
 }
 
 //for all lethal and inscribed costs, we'll make sure that their neighbors have the cost values we'd expect
-void CostmapTester::compareCells(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y, unsigned int nx, unsigned int ny){
+void CostmapTester::compareCells(robot_costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y, unsigned int nx, unsigned int ny){
   double cell_distance = hypot(static_cast<int>(x-nx), static_cast<int>(y-ny));
 
   unsigned char cell_cost = costmap.getCost(x, y);
   unsigned char neighbor_cost = costmap.getCost(nx, ny);
 
-  if(cell_cost == costmap_2d::LETHAL_OBSTACLE){
+  if(cell_cost == robot_costmap_2d::LETHAL_OBSTACLE){
     //if the cell is a lethal obstacle, then we know that all its neighbors should have equal or slighlty less cost
     unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(cell_distance);
-    EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > 0 /*costmap.cell_inflation_radius_*/ && neighbor_cost == costmap_2d::FREE_SPACE));
+    EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > 0 /*costmap.cell_inflation_radius_*/ && neighbor_cost == robot_costmap_2d::FREE_SPACE));
   }
-  else if(cell_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
+  else if(cell_cost == robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
     //the furthest valid distance from an obstacle is the inscribed radius plus the cell distance away
     double furthest_valid_distance = 0; // ################costmap.cell_inscribed_radius_ + cell_distance + 1;
     unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(furthest_valid_distance);
@@ -72,19 +72,19 @@ void CostmapTester::compareCells(costmap_2d::Costmap2D& costmap, unsigned int x,
       printf("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
       costmap.saveMap("failing_costmap.pgm");
     }
-    EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == costmap_2d::FREE_SPACE));
+    EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == robot_costmap_2d::FREE_SPACE));
   }
 }
 };
 
-costmap_2d::CostmapTester* map_tester = NULL;
+robot_costmap_2d::CostmapTester* map_tester = NULL;
 
 
 int main(int argc, char** argv){
 
   tf3::BufferCore tf(tf3::Duration(10));
 
-  map_tester = new costmap_2d::CostmapTester(tf);
+  map_tester = new robot_costmap_2d::CostmapTester(tf);
 
 
   return(0);