first commit

CMakeLists.txt

@@ -4,6 +4,14 @@ project(costmap_2d)
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
+if (NOT TARGET sensor_msgs)
+    add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../sensor_msgs ${CMAKE_BINARY_DIR}/sensor_msgs_build)
+endif()
+
+if (NOT TARGET geometry_msgs)
+    add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../geometry_msgs ${CMAKE_BINARY_DIR}/geometry_msgs_build)
+endif()
+
 # ---- Dependencies ----
 find_package(Eigen3 REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system thread)
@@ -24,13 +32,15 @@ add_library(costmap_2d
     src/observation_buffer.cpp
     src/layer.cpp
     src/layered_costmap.cpp
-    # src/costmap_math.cpp
+    src/costmap_math.cpp
     src/footprint.cpp
-    # src/costmap_layer.cpp
+    src/costmap_layer.cpp
 )
 
 target_link_libraries(costmap_2d
     ${Boost_LIBRARIES}
+    sensor_msgs
+    geometry_msgs
 )
 
 # # ---- Layer plugins ----

build/CMakeCache.txt

@@ -243,9 +243,27 @@ costmap_2d_LIB_DEPENDS:STATIC=general;Boost::system;general;Boost::thread;
 //Value Computed by CMake
 costmap_2d_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/costmap_2d
 
+//Value Computed by CMake
+geometry_msgs_BINARY_DIR:STATIC=/home/duongtd/robotics_core/costmap_2d/build/geometry_msgs_build
+
+//Value Computed by CMake
+geometry_msgs_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/geometry_msgs
+
 //Dependencies for the target
 layers_LIB_DEPENDS:STATIC=general;costmap_2d;general;Boost::system;general;Boost::thread;
 
+//Value Computed by CMake
+sensor_msgs_BINARY_DIR:STATIC=/home/duongtd/robotics_core/costmap_2d/build/sensor_msgs_build
+
+//Value Computed by CMake
+sensor_msgs_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/sensor_msgs
+
+//Value Computed by CMake
+std_msgs_BINARY_DIR:STATIC=/home/duongtd/robotics_core/costmap_2d/build/std_msgs_build
+
+//Value Computed by CMake
+std_msgs_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/std_msgs
+
 
 ########################
 # INTERNAL cache entries
@@ -361,7 +379,7 @@ CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
 //ADVANCED property for variable: CMAKE_NM
 CMAKE_NM-ADVANCED:INTERNAL=1
 //number of local generators
-CMAKE_NUMBER_OF_MAKEFILES:INTERNAL=1
+CMAKE_NUMBER_OF_MAKEFILES:INTERNAL=4
 //ADVANCED property for variable: CMAKE_OBJCOPY
 CMAKE_OBJCOPY-ADVANCED:INTERNAL=1
 //ADVANCED property for variable: CMAKE_OBJDUMP

build/CMakeFiles/Makefile.cmake

@@ -11,6 +11,9 @@ set(CMAKE_MAKEFILE_DEPENDS
   "CMakeFiles/3.16.3/CMakeCCompiler.cmake"
   "CMakeFiles/3.16.3/CMakeCXXCompiler.cmake"
   "CMakeFiles/3.16.3/CMakeSystem.cmake"
+  "/home/duongtd/robotics_core/geometry_msgs/CMakeLists.txt"
+  "/home/duongtd/robotics_core/sensor_msgs/CMakeLists.txt"
+  "/home/duongtd/robotics_core/std_msgs/CMakeLists.txt"
   "/usr/lib/cmake/eigen3/Eigen3Config.cmake"
   "/usr/lib/cmake/eigen3/Eigen3ConfigVersion.cmake"
   "/usr/lib/cmake/eigen3/Eigen3Targets.cmake"
@@ -69,10 +72,14 @@ set(CMAKE_MAKEFILE_OUTPUTS
 # Byproducts of CMake generate step:
 set(CMAKE_MAKEFILE_PRODUCTS
   "CMakeFiles/CMakeDirectoryInformation.cmake"
+  "sensor_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake"
+  "std_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake"
+  "geometry_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake"
   )
 
 # Dependency information for all targets:
 set(CMAKE_DEPEND_INFO_FILES
   "CMakeFiles/layers.dir/DependInfo.cmake"
   "CMakeFiles/costmap_2d.dir/DependInfo.cmake"
+  "sensor_msgs_build/CMakeFiles/test_battery_state.dir/DependInfo.cmake"
   )

build/CMakeFiles/Makefile2

@@ -55,20 +55,81 @@ CMAKE_BINARY_DIR = /home/duongtd/robotics_core/costmap_2d/build
 # The main recursive "all" target.
 all: CMakeFiles/layers.dir/all
 all: CMakeFiles/costmap_2d.dir/all
+all: sensor_msgs_build/all
+all: geometry_msgs_build/all
 
 .PHONY : all
 
 # The main recursive "preinstall" target.
-preinstall:
+preinstall: sensor_msgs_build/preinstall
+preinstall: geometry_msgs_build/preinstall
 
 .PHONY : preinstall
 
 # The main recursive "clean" target.
 clean: CMakeFiles/layers.dir/clean
 clean: CMakeFiles/costmap_2d.dir/clean
+clean: sensor_msgs_build/clean
+clean: geometry_msgs_build/clean
 
 .PHONY : clean
 
+#=============================================================================
+# Directory level rules for directory geometry_msgs_build
+
+# Recursive "all" directory target.
+geometry_msgs_build/all:
+
+.PHONY : geometry_msgs_build/all
+
+# Recursive "preinstall" directory target.
+geometry_msgs_build/preinstall:
+
+.PHONY : geometry_msgs_build/preinstall
+
+# Recursive "clean" directory target.
+geometry_msgs_build/clean:
+
+.PHONY : geometry_msgs_build/clean
+
+#=============================================================================
+# Directory level rules for directory sensor_msgs_build
+
+# Recursive "all" directory target.
+sensor_msgs_build/all: sensor_msgs_build/CMakeFiles/test_battery_state.dir/all
+sensor_msgs_build/all: std_msgs_build/all
+
+.PHONY : sensor_msgs_build/all
+
+# Recursive "preinstall" directory target.
+sensor_msgs_build/preinstall: std_msgs_build/preinstall
+
+.PHONY : sensor_msgs_build/preinstall
+
+# Recursive "clean" directory target.
+sensor_msgs_build/clean: sensor_msgs_build/CMakeFiles/test_battery_state.dir/clean
+sensor_msgs_build/clean: std_msgs_build/clean
+
+.PHONY : sensor_msgs_build/clean
+
+#=============================================================================
+# Directory level rules for directory std_msgs_build
+
+# Recursive "all" directory target.
+std_msgs_build/all:
+
+.PHONY : std_msgs_build/all
+
+# Recursive "preinstall" directory target.
+std_msgs_build/preinstall:
+
+.PHONY : std_msgs_build/preinstall
+
+# Recursive "clean" directory target.
+std_msgs_build/clean:
+
+.PHONY : std_msgs_build/clean
+
 #=============================================================================
 # Target rules for target CMakeFiles/layers.dir
 
@@ -76,12 +137,12 @@ clean: CMakeFiles/costmap_2d.dir/clean
 CMakeFiles/layers.dir/all: CMakeFiles/costmap_2d.dir/all
 	$(MAKE) -f CMakeFiles/layers.dir/build.make CMakeFiles/layers.dir/depend
 	$(MAKE) -f CMakeFiles/layers.dir/build.make CMakeFiles/layers.dir/build
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --progress-dir=/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles --progress-num=8,9 "Built target layers"
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --progress-dir=/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles --progress-num=10,11 "Built target layers"
 .PHONY : CMakeFiles/layers.dir/all
 
 # Build rule for subdir invocation for target.
 CMakeFiles/layers.dir/rule: cmake_check_build_system
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles 9
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles 11
 	$(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/layers.dir/all
 	$(CMAKE_COMMAND) -E cmake_progress_start /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles 0
 .PHONY : CMakeFiles/layers.dir/rule
@@ -103,12 +164,12 @@ CMakeFiles/layers.dir/clean:
 CMakeFiles/costmap_2d.dir/all:
 	$(MAKE) -f CMakeFiles/costmap_2d.dir/build.make CMakeFiles/costmap_2d.dir/depend
 	$(MAKE) -f CMakeFiles/costmap_2d.dir/build.make CMakeFiles/costmap_2d.dir/build
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --progress-dir=/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles --progress-num=1,2,3,4,5,6,7 "Built target costmap_2d"
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --progress-dir=/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles --progress-num=1,2,3,4,5,6,7,8,9 "Built target costmap_2d"
 .PHONY : CMakeFiles/costmap_2d.dir/all
 
 # Build rule for subdir invocation for target.
 CMakeFiles/costmap_2d.dir/rule: cmake_check_build_system
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles 7
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles 9
 	$(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/costmap_2d.dir/all
 	$(CMAKE_COMMAND) -E cmake_progress_start /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles 0
 .PHONY : CMakeFiles/costmap_2d.dir/rule
@@ -123,6 +184,33 @@ CMakeFiles/costmap_2d.dir/clean:
 	$(MAKE) -f CMakeFiles/costmap_2d.dir/build.make CMakeFiles/costmap_2d.dir/clean
 .PHONY : CMakeFiles/costmap_2d.dir/clean
 
+#=============================================================================
+# Target rules for target sensor_msgs_build/CMakeFiles/test_battery_state.dir
+
+# All Build rule for target.
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/all:
+	$(MAKE) -f sensor_msgs_build/CMakeFiles/test_battery_state.dir/build.make sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend
+	$(MAKE) -f sensor_msgs_build/CMakeFiles/test_battery_state.dir/build.make sensor_msgs_build/CMakeFiles/test_battery_state.dir/build
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --progress-dir=/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles --progress-num=12,13 "Built target test_battery_state"
+.PHONY : sensor_msgs_build/CMakeFiles/test_battery_state.dir/all
+
+# Build rule for subdir invocation for target.
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/rule: cmake_check_build_system
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles 2
+	$(MAKE) -f CMakeFiles/Makefile2 sensor_msgs_build/CMakeFiles/test_battery_state.dir/all
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles 0
+.PHONY : sensor_msgs_build/CMakeFiles/test_battery_state.dir/rule
+
+# Convenience name for target.
+test_battery_state: sensor_msgs_build/CMakeFiles/test_battery_state.dir/rule
+
+.PHONY : test_battery_state
+
+# clean rule for target.
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/clean:
+	$(MAKE) -f sensor_msgs_build/CMakeFiles/test_battery_state.dir/build.make sensor_msgs_build/CMakeFiles/test_battery_state.dir/clean
+.PHONY : sensor_msgs_build/CMakeFiles/test_battery_state.dir/clean
+
 #=============================================================================
 # Special targets to cleanup operation of make.
 

build/CMakeFiles/Progress/3

@@ -1 +0,0 @@
-empty

build/CMakeFiles/Progress/count.txt

@@ -1 +0,0 @@
-9

build/CMakeFiles/TargetDirectories.txt

@@ -2,3 +2,10 @@
 /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/layers.dir
 /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/edit_cache.dir
 /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/costmap_2d.dir
+/home/duongtd/robotics_core/costmap_2d/build/sensor_msgs_build/CMakeFiles/rebuild_cache.dir
+/home/duongtd/robotics_core/costmap_2d/build/sensor_msgs_build/CMakeFiles/edit_cache.dir
+/home/duongtd/robotics_core/costmap_2d/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir
+/home/duongtd/robotics_core/costmap_2d/build/std_msgs_build/CMakeFiles/rebuild_cache.dir
+/home/duongtd/robotics_core/costmap_2d/build/std_msgs_build/CMakeFiles/edit_cache.dir
+/home/duongtd/robotics_core/costmap_2d/build/geometry_msgs_build/CMakeFiles/rebuild_cache.dir
+/home/duongtd/robotics_core/costmap_2d/build/geometry_msgs_build/CMakeFiles/edit_cache.dir

build/CMakeFiles/costmap_2d.dir/CXX.includecache

@@ -6,31 +6,55 @@
 
 #IncludeRegexTransform: 
 
-../include/costmap_2d/observation_buffer.h
+../include/costmap_2d/cost_values.h
+
+../include/costmap_2d/costmap_2d.h
 vector
 -
-list
+queue
--
-string
--
-ros/time.h
--
-costmap_2d/observation.h
--
-tf2_ros/buffer.h
--
-sensor_msgs/PointCloud2.h
 -
 boost/thread.hpp
 -
-
+costmap_2d/msg.h
-/home/duongtd/robotics_core/costmap_2d/src/observation_buffer.cpp
+-
-costmap_2d/observation_buffer.h
+
--
+../include/costmap_2d/costmap_layer.h
-tf2_geometry_msgs/tf2_geometry_msgs.h
+costmap_2d/layer.h
--
+-
-tf2_sensor_msgs/tf2_sensor_msgs.h
+costmap_2d/layered_costmap.h
--
+-
-sensor_msgs/point_cloud2_iterator.h
+
+../include/costmap_2d/layer.h
+costmap_2d/costmap_2d.h
+-
+costmap_2d/layered_costmap.h
+-
+string
+-
+tf2/buffer_core.h
+../include/costmap_2d/tf2/buffer_core.h
+
+../include/costmap_2d/layered_costmap.h
+costmap_2d/cost_values.h
+-
+costmap_2d/layer.h
+-
+costmap_2d/costmap_2d.h
+-
+vector
+-
+string
+-
+
+../include/costmap_2d/msg.h
+vector
+-
+string
+-
+chrono
+-
+
+/home/duongtd/robotics_core/costmap_2d/src/costmap_layer.cpp
+costmap_2d/costmap_layer.h
 -
 

build/CMakeFiles/costmap_2d.dir/DependInfo.cmake

@@ -6,6 +6,8 @@ set(CMAKE_DEPENDS_LANGUAGES
 set(CMAKE_DEPENDS_CHECK_CXX
   "/home/duongtd/robotics_core/costmap_2d/src/array_parser.cpp" "/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/costmap_2d.dir/src/array_parser.cpp.o"
   "/home/duongtd/robotics_core/costmap_2d/src/costmap_2d.cpp" "/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o"
+  "/home/duongtd/robotics_core/costmap_2d/src/costmap_layer.cpp" "/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o"
+  "/home/duongtd/robotics_core/costmap_2d/src/costmap_math.cpp" "/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o"
   "/home/duongtd/robotics_core/costmap_2d/src/footprint.cpp" "/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/costmap_2d.dir/src/footprint.cpp.o"
   "/home/duongtd/robotics_core/costmap_2d/src/layer.cpp" "/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/costmap_2d.dir/src/layer.cpp.o"
   "/home/duongtd/robotics_core/costmap_2d/src/layered_costmap.cpp" "/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o"
@@ -25,6 +27,9 @@ set(CMAKE_TARGET_DEFINITIONS_CXX
 set(CMAKE_CXX_TARGET_INCLUDE_PATH
   "../include"
   "/usr/include/eigen3"
+  "/home/duongtd/robotics_core/sensor_msgs/include"
+  "/home/duongtd/robotics_core/std_msgs/include"
+  "/home/duongtd/robotics_core/geometry_msgs/include"
   )
 
 # Targets to which this target links.

build/CMakeFiles/costmap_2d.dir/build.make

@@ -122,9 +122,22 @@ CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.s: cmake_force
 	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.s"
 	/usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/duongtd/robotics_core/costmap_2d/src/layered_costmap.cpp -o CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.s
 
+CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o: CMakeFiles/costmap_2d.dir/flags.make
+CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o: ../src/costmap_math.cpp
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_6) "Building CXX object CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o"
+	/usr/bin/c++  $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -o CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o -c /home/duongtd/robotics_core/costmap_2d/src/costmap_math.cpp
+
+CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.i: cmake_force
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.i"
+	/usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /home/duongtd/robotics_core/costmap_2d/src/costmap_math.cpp > CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.i
+
+CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.s: cmake_force
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.s"
+	/usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/duongtd/robotics_core/costmap_2d/src/costmap_math.cpp -o CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.s
+
 CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: CMakeFiles/costmap_2d.dir/flags.make
 CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: ../src/footprint.cpp
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_6) "Building CXX object CMakeFiles/costmap_2d.dir/src/footprint.cpp.o"
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_7) "Building CXX object CMakeFiles/costmap_2d.dir/src/footprint.cpp.o"
 	/usr/bin/c++  $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -o CMakeFiles/costmap_2d.dir/src/footprint.cpp.o -c /home/duongtd/robotics_core/costmap_2d/src/footprint.cpp
 
 CMakeFiles/costmap_2d.dir/src/footprint.cpp.i: cmake_force
@@ -135,6 +148,19 @@ CMakeFiles/costmap_2d.dir/src/footprint.cpp.s: cmake_force
 	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/costmap_2d.dir/src/footprint.cpp.s"
 	/usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/duongtd/robotics_core/costmap_2d/src/footprint.cpp -o CMakeFiles/costmap_2d.dir/src/footprint.cpp.s
 
+CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: CMakeFiles/costmap_2d.dir/flags.make
+CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: ../src/costmap_layer.cpp
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_8) "Building CXX object CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o"
+	/usr/bin/c++  $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -o CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o -c /home/duongtd/robotics_core/costmap_2d/src/costmap_layer.cpp
+
+CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.i: cmake_force
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.i"
+	/usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /home/duongtd/robotics_core/costmap_2d/src/costmap_layer.cpp > CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.i
+
+CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.s: cmake_force
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.s"
+	/usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/duongtd/robotics_core/costmap_2d/src/costmap_layer.cpp -o CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.s
+
 # Object files for target costmap_2d
 costmap_2d_OBJECTS = \
 "CMakeFiles/costmap_2d.dir/src/array_parser.cpp.o" \
@@ -142,7 +168,9 @@ costmap_2d_OBJECTS = \
 "CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o" \
 "CMakeFiles/costmap_2d.dir/src/layer.cpp.o" \
 "CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o" \
-"CMakeFiles/costmap_2d.dir/src/footprint.cpp.o"
+"CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o" \
+"CMakeFiles/costmap_2d.dir/src/footprint.cpp.o" \
+"CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o"
 
 # External object files for target costmap_2d
 costmap_2d_EXTERNAL_OBJECTS =
@@ -152,10 +180,12 @@ libcostmap_2d.a: CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o
 libcostmap_2d.a: CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o
 libcostmap_2d.a: CMakeFiles/costmap_2d.dir/src/layer.cpp.o
 libcostmap_2d.a: CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o
+libcostmap_2d.a: CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o
 libcostmap_2d.a: CMakeFiles/costmap_2d.dir/src/footprint.cpp.o
+libcostmap_2d.a: CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o
 libcostmap_2d.a: CMakeFiles/costmap_2d.dir/build.make
 libcostmap_2d.a: CMakeFiles/costmap_2d.dir/link.txt
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --bold --progress-dir=/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_7) "Linking CXX static library libcostmap_2d.a"
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --bold --progress-dir=/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_9) "Linking CXX static library libcostmap_2d.a"
 	$(CMAKE_COMMAND) -P CMakeFiles/costmap_2d.dir/cmake_clean_target.cmake
 	$(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/costmap_2d.dir/link.txt --verbose=$(VERBOSE)
 

build/CMakeFiles/costmap_2d.dir/cmake_clean.cmake

@@ -1,6 +1,8 @@
 file(REMOVE_RECURSE
   "CMakeFiles/costmap_2d.dir/src/array_parser.cpp.o"
   "CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o"
+  "CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o"
+  "CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o"
   "CMakeFiles/costmap_2d.dir/src/footprint.cpp.o"
   "CMakeFiles/costmap_2d.dir/src/layer.cpp.o"
   "CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o"

build/CMakeFiles/costmap_2d.dir/depend.internal

@@ -7,6 +7,19 @@ CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o
  ../include/costmap_2d/costmap_2d.h
  ../include/costmap_2d/msg.h
  /home/duongtd/robotics_core/costmap_2d/src/costmap_2d.cpp
+CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o
+ ../include/costmap_2d/cost_values.h
+ ../include/costmap_2d/costmap_2d.h
+ ../include/costmap_2d/costmap_layer.h
+ ../include/costmap_2d/layer.h
+ ../include/costmap_2d/layered_costmap.h
+ ../include/costmap_2d/msg.h
+ /home/duongtd/robotics_core/costmap_2d/src/costmap_layer.cpp
+CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o
+ ../include/costmap_2d/costmap_math.h
+ ../include/costmap_2d/msg.h
+ /home/duongtd/robotics_core/costmap_2d/src/costmap_math.cpp
+ /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h
 CMakeFiles/costmap_2d.dir/src/footprint.cpp.o
  ../include/costmap_2d/array_parser.h
  ../include/costmap_2d/costmap_math.h
@@ -29,5 +42,13 @@ CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o
  ../include/costmap_2d/msg.h
  /home/duongtd/robotics_core/costmap_2d/src/layered_costmap.cpp
 CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o
+ ../include/costmap_2d/observation.h
  ../include/costmap_2d/observation_buffer.h
  /home/duongtd/robotics_core/costmap_2d/src/observation_buffer.cpp
+ /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h
+ /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/PointStamped.h
+ /home/duongtd/robotics_core/sensor_msgs/include/msg/PointCloud2.h
+ /home/duongtd/robotics_core/sensor_msgs/include/msg/PointField.h
+ /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h
+ /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h
+ /home/duongtd/robotics_core/std_msgs/include/msg/Header.h

build/CMakeFiles/costmap_2d.dir/depend.make

@@ -7,6 +7,19 @@ CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o: ../include/costmap_2d/costmap_2d
 CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o: ../include/costmap_2d/msg.h
 CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o: ../src/costmap_2d.cpp
 
+CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: ../include/costmap_2d/cost_values.h
+CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: ../include/costmap_2d/costmap_2d.h
+CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: ../include/costmap_2d/costmap_layer.h
+CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: ../include/costmap_2d/layer.h
+CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: ../include/costmap_2d/layered_costmap.h
+CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: ../include/costmap_2d/msg.h
+CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: ../src/costmap_layer.cpp
+
+CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o: ../include/costmap_2d/costmap_math.h
+CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o: ../include/costmap_2d/msg.h
+CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o: ../src/costmap_math.cpp
+CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h
+
 CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: ../include/costmap_2d/array_parser.h
 CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: ../include/costmap_2d/costmap_math.h
 CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: ../include/costmap_2d/footprint.h
@@ -28,6 +41,14 @@ CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: ../include/costmap_2d/layer
 CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: ../include/costmap_2d/msg.h
 CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: ../src/layered_costmap.cpp
 
+CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: ../include/costmap_2d/observation.h
 CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: ../include/costmap_2d/observation_buffer.h
 CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: ../src/observation_buffer.cpp
+CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h
+CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/PointStamped.h
+CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/msg/PointCloud2.h
+CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/msg/PointField.h
+CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h
+CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h
+CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/std_msgs/include/msg/Header.h
 

build/CMakeFiles/costmap_2d.dir/flags.make

@@ -6,5 +6,5 @@ CXX_FLAGS = -fPIC   -std=gnu++17
 
 CXX_DEFINES = -DBOOST_ALL_NO_LIB -DBOOST_ATOMIC_DYN_LINK -DBOOST_SYSTEM_DYN_LINK -DBOOST_THREAD_DYN_LINK
 
-CXX_INCLUDES = -I/home/duongtd/robotics_core/costmap_2d/include -I/usr/include/eigen3 
+CXX_INCLUDES = -I/home/duongtd/robotics_core/costmap_2d/include -I/usr/include/eigen3 -I/home/duongtd/robotics_core/sensor_msgs/include -I/home/duongtd/robotics_core/std_msgs/include -I/home/duongtd/robotics_core/geometry_msgs/include 
 

build/CMakeFiles/costmap_2d.dir/link.txt

@@ -1,2 +1,2 @@
-/usr/bin/ar qc libcostmap_2d.a  CMakeFiles/costmap_2d.dir/src/array_parser.cpp.o CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o CMakeFiles/costmap_2d.dir/src/layer.cpp.o CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o CMakeFiles/costmap_2d.dir/src/footprint.cpp.o
+/usr/bin/ar qc libcostmap_2d.a  CMakeFiles/costmap_2d.dir/src/array_parser.cpp.o CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o CMakeFiles/costmap_2d.dir/src/layer.cpp.o CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o CMakeFiles/costmap_2d.dir/src/footprint.cpp.o CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o
 /usr/bin/ranlib libcostmap_2d.a

build/CMakeFiles/costmap_2d.dir/progress.make

@@ -5,4 +5,6 @@ CMAKE_PROGRESS_4 = 4
 CMAKE_PROGRESS_5 = 5
 CMAKE_PROGRESS_6 = 6
 CMAKE_PROGRESS_7 = 7
+CMAKE_PROGRESS_8 = 8
+CMAKE_PROGRESS_9 = 9
 

build/CMakeFiles/layers.dir/DependInfo.cmake

@@ -20,6 +20,9 @@ set(CMAKE_TARGET_DEFINITIONS_CXX
 set(CMAKE_CXX_TARGET_INCLUDE_PATH
   "../include"
   "/usr/include/eigen3"
+  "/home/duongtd/robotics_core/sensor_msgs/include"
+  "/home/duongtd/robotics_core/std_msgs/include"
+  "/home/duongtd/robotics_core/geometry_msgs/include"
   )
 
 # Targets to which this target links.

build/CMakeFiles/layers.dir/flags.make

@@ -6,5 +6,5 @@ CXX_FLAGS = -fPIC   -std=gnu++17
 
 CXX_DEFINES = -DBOOST_ALL_NO_LIB -DBOOST_ATOMIC_DYN_LINK -DBOOST_SYSTEM_DYN_LINK -DBOOST_THREAD_DYN_LINK
 
-CXX_INCLUDES = -I/home/duongtd/robotics_core/costmap_2d/include -I/usr/include/eigen3 
+CXX_INCLUDES = -I/home/duongtd/robotics_core/costmap_2d/include -I/usr/include/eigen3 -I/home/duongtd/robotics_core/sensor_msgs/include -I/home/duongtd/robotics_core/std_msgs/include -I/home/duongtd/robotics_core/geometry_msgs/include 
 

build/CMakeFiles/layers.dir/progress.make

@@ -1,3 +1,3 @@
-CMAKE_PROGRESS_1 = 8
+CMAKE_PROGRESS_1 = 10
-CMAKE_PROGRESS_2 = 9
+CMAKE_PROGRESS_2 = 11
 

build/CMakeFiles/progress.marks

@@ -1 +1 @@
-9
+13

build/Makefile

@@ -136,6 +136,19 @@ costmap_2d/fast:
 	$(MAKE) -f CMakeFiles/costmap_2d.dir/build.make CMakeFiles/costmap_2d.dir/build
 .PHONY : costmap_2d/fast
 
+#=============================================================================
+# Target rules for targets named test_battery_state
+
+# Build rule for target.
+test_battery_state: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 test_battery_state
+.PHONY : test_battery_state
+
+# fast build rule for target.
+test_battery_state/fast:
+	$(MAKE) -f sensor_msgs_build/CMakeFiles/test_battery_state.dir/build.make sensor_msgs_build/CMakeFiles/test_battery_state.dir/build
+.PHONY : test_battery_state/fast
+
 plugins/inflation_layer.o: plugins/inflation_layer.cpp.o
 
 .PHONY : plugins/inflation_layer.o
@@ -217,6 +230,60 @@ src/costmap_2d.cpp.s:
 	$(MAKE) -f CMakeFiles/costmap_2d.dir/build.make CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.s
 .PHONY : src/costmap_2d.cpp.s
 
+src/costmap_layer.o: src/costmap_layer.cpp.o
+
+.PHONY : src/costmap_layer.o
+
+# target to build an object file
+src/costmap_layer.cpp.o:
+	$(MAKE) -f CMakeFiles/costmap_2d.dir/build.make CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o
+.PHONY : src/costmap_layer.cpp.o
+
+src/costmap_layer.i: src/costmap_layer.cpp.i
+
+.PHONY : src/costmap_layer.i
+
+# target to preprocess a source file
+src/costmap_layer.cpp.i:
+	$(MAKE) -f CMakeFiles/costmap_2d.dir/build.make CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.i
+.PHONY : src/costmap_layer.cpp.i
+
+src/costmap_layer.s: src/costmap_layer.cpp.s
+
+.PHONY : src/costmap_layer.s
+
+# target to generate assembly for a file
+src/costmap_layer.cpp.s:
+	$(MAKE) -f CMakeFiles/costmap_2d.dir/build.make CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.s
+.PHONY : src/costmap_layer.cpp.s
+
+src/costmap_math.o: src/costmap_math.cpp.o
+
+.PHONY : src/costmap_math.o
+
+# target to build an object file
+src/costmap_math.cpp.o:
+	$(MAKE) -f CMakeFiles/costmap_2d.dir/build.make CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o
+.PHONY : src/costmap_math.cpp.o
+
+src/costmap_math.i: src/costmap_math.cpp.i
+
+.PHONY : src/costmap_math.i
+
+# target to preprocess a source file
+src/costmap_math.cpp.i:
+	$(MAKE) -f CMakeFiles/costmap_2d.dir/build.make CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.i
+.PHONY : src/costmap_math.cpp.i
+
+src/costmap_math.s: src/costmap_math.cpp.s
+
+.PHONY : src/costmap_math.s
+
+# target to generate assembly for a file
+src/costmap_math.cpp.s:
+	$(MAKE) -f CMakeFiles/costmap_2d.dir/build.make CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.s
+.PHONY : src/costmap_math.cpp.s
+
 src/footprint.o: src/footprint.cpp.o
 
 .PHONY : src/footprint.o
@@ -335,6 +402,7 @@ help:
 	@echo "... layers"
 	@echo "... edit_cache"
 	@echo "... costmap_2d"
+	@echo "... test_battery_state"
 	@echo "... plugins/inflation_layer.o"
 	@echo "... plugins/inflation_layer.i"
 	@echo "... plugins/inflation_layer.s"
@@ -344,6 +412,12 @@ help:
 	@echo "... src/costmap_2d.o"
 	@echo "... src/costmap_2d.i"
 	@echo "... src/costmap_2d.s"
+	@echo "... src/costmap_layer.o"
+	@echo "... src/costmap_layer.i"
+	@echo "... src/costmap_layer.s"
+	@echo "... src/costmap_math.o"
+	@echo "... src/costmap_math.i"
+	@echo "... src/costmap_math.s"
 	@echo "... src/footprint.o"
 	@echo "... src/footprint.i"
 	@echo "... src/footprint.s"

build/cmake_install.cmake

@@ -37,6 +37,13 @@ if(NOT DEFINED CMAKE_CROSSCOMPILING)
   set(CMAKE_CROSSCOMPILING "FALSE")
 endif()
 
+if(NOT CMAKE_INSTALL_LOCAL_ONLY)
+  # Include the install script for each subdirectory.
+  include("/home/duongtd/robotics_core/costmap_2d/build/sensor_msgs_build/cmake_install.cmake")
+  include("/home/duongtd/robotics_core/costmap_2d/build/geometry_msgs_build/cmake_install.cmake")
+
+endif()
+
 if(CMAKE_INSTALL_COMPONENT)
   set(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt")
 else()

build/geometry_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake

@@ -0,0 +1,16 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 3.16
+
+# Relative path conversion top directories.
+set(CMAKE_RELATIVE_PATH_TOP_SOURCE "/home/duongtd/robotics_core/geometry_msgs")
+set(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/duongtd/robotics_core/costmap_2d/build")
+
+# Force unix paths in dependencies.
+set(CMAKE_FORCE_UNIX_PATHS 1)
+
+
+# The C and CXX include file regular expressions for this directory.
+set(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$")
+set(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$")
+set(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN})
+set(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN})

build/geometry_msgs_build/CMakeFiles/progress.marks

@@ -0,0 +1 @@
+0

build/geometry_msgs_build/Makefile

@@ -0,0 +1,134 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 3.16
+
+# Default target executed when no arguments are given to make.
+default_target: all
+
+.PHONY : default_target
+
+# Allow only one "make -f Makefile2" at a time, but pass parallelism.
+.NOTPARALLEL:
+
+
+#=============================================================================
+# Special targets provided by cmake.
+
+# Disable implicit rules so canonical targets will work.
+.SUFFIXES:
+
+
+# Remove some rules from gmake that .SUFFIXES does not remove.
+SUFFIXES =
+
+.SUFFIXES: .hpux_make_needs_suffix_list
+
+
+# Suppress display of executed commands.
+$(VERBOSE).SILENT:
+
+
+# A target that is always out of date.
+cmake_force:
+
+.PHONY : cmake_force
+
+#=============================================================================
+# Set environment variables for the build.
+
+# The shell in which to execute make rules.
+SHELL = /bin/sh
+
+# The CMake executable.
+CMAKE_COMMAND = /usr/bin/cmake
+
+# The command to remove a file.
+RM = /usr/bin/cmake -E remove -f
+
+# Escaping for special characters.
+EQUALS = =
+
+# The top-level source directory on which CMake was run.
+CMAKE_SOURCE_DIR = /home/duongtd/robotics_core/costmap_2d
+
+# The top-level build directory on which CMake was run.
+CMAKE_BINARY_DIR = /home/duongtd/robotics_core/costmap_2d/build
+
+#=============================================================================
+# Targets provided globally by CMake.
+
+# Special rule for the target rebuild_cache
+rebuild_cache:
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..."
+	/usr/bin/cmake -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR)
+.PHONY : rebuild_cache
+
+# Special rule for the target rebuild_cache
+rebuild_cache/fast: rebuild_cache
+
+.PHONY : rebuild_cache/fast
+
+# Special rule for the target edit_cache
+edit_cache:
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "No interactive CMake dialog available..."
+	/usr/bin/cmake -E echo No\ interactive\ CMake\ dialog\ available.
+.PHONY : edit_cache
+
+# Special rule for the target edit_cache
+edit_cache/fast: edit_cache
+
+.PHONY : edit_cache/fast
+
+# The main all target
+all: cmake_check_build_system
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(CMAKE_COMMAND) -E cmake_progress_start /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles /home/duongtd/robotics_core/costmap_2d/build/geometry_msgs_build/CMakeFiles/progress.marks
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f CMakeFiles/Makefile2 geometry_msgs_build/all
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles 0
+.PHONY : all
+
+# The main clean target
+clean:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f CMakeFiles/Makefile2 geometry_msgs_build/clean
+.PHONY : clean
+
+# The main clean target
+clean/fast: clean
+
+.PHONY : clean/fast
+
+# Prepare targets for installation.
+preinstall: all
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f CMakeFiles/Makefile2 geometry_msgs_build/preinstall
+.PHONY : preinstall
+
+# Prepare targets for installation.
+preinstall/fast:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f CMakeFiles/Makefile2 geometry_msgs_build/preinstall
+.PHONY : preinstall/fast
+
+# clear depends
+depend:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1
+.PHONY : depend
+
+# Help Target
+help:
+	@echo "The following are some of the valid targets for this Makefile:"
+	@echo "... all (the default if no target is provided)"
+	@echo "... clean"
+	@echo "... depend"
+	@echo "... rebuild_cache"
+	@echo "... edit_cache"
+.PHONY : help
+
+
+
+#=============================================================================
+# Special targets to cleanup operation of make.
+
+# Special rule to run CMake to check the build system integrity.
+# No rule that depends on this can have commands that come from listfiles
+# because they might be regenerated.
+cmake_check_build_system:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0
+.PHONY : cmake_check_build_system
+

build/geometry_msgs_build/cmake_install.cmake

@@ -0,0 +1,39 @@
+# Install script for directory: /home/duongtd/robotics_core/geometry_msgs
+
+# Set the install prefix
+if(NOT DEFINED CMAKE_INSTALL_PREFIX)
+  set(CMAKE_INSTALL_PREFIX "/usr/local")
+endif()
+string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}")
+
+# Set the install configuration name.
+if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
+  if(BUILD_TYPE)
+    string(REGEX REPLACE "^[^A-Za-z0-9_]+" ""
+           CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}")
+  else()
+    set(CMAKE_INSTALL_CONFIG_NAME "")
+  endif()
+  message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"")
+endif()
+
+# Set the component getting installed.
+if(NOT CMAKE_INSTALL_COMPONENT)
+  if(COMPONENT)
+    message(STATUS "Install component: \"${COMPONENT}\"")
+    set(CMAKE_INSTALL_COMPONENT "${COMPONENT}")
+  else()
+    set(CMAKE_INSTALL_COMPONENT)
+  endif()
+endif()
+
+# Install shared libraries without execute permission?
+if(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
+  set(CMAKE_INSTALL_SO_NO_EXE "1")
+endif()
+
+# Is this installation the result of a crosscompile?
+if(NOT DEFINED CMAKE_CROSSCOMPILING)
+  set(CMAKE_CROSSCOMPILING "FALSE")
+endif()
+

build/sensor_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake

@@ -0,0 +1,16 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 3.16
+
+# Relative path conversion top directories.
+set(CMAKE_RELATIVE_PATH_TOP_SOURCE "/home/duongtd/robotics_core/sensor_msgs")
+set(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/duongtd/robotics_core/costmap_2d/build")
+
+# Force unix paths in dependencies.
+set(CMAKE_FORCE_UNIX_PATHS 1)
+
+
+# The C and CXX include file regular expressions for this directory.
+set(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$")
+set(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$")
+set(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN})
+set(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN})

build/sensor_msgs_build/CMakeFiles/progress.marks

@@ -0,0 +1 @@
+2

build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/CXX.includecache

@@ -0,0 +1,66 @@
+#IncludeRegexLine: ^[ 	]*[#%][ 	]*(include|import)[ 	]*[<"]([^">]+)([">])
+
+#IncludeRegexScan: ^.*$
+
+#IncludeRegexComplain: ^$
+
+#IncludeRegexTransform: 
+
+/home/duongtd/robotics_core/sensor_msgs/include/msg/BatteryState.h
+msg/Header.h
+/home/duongtd/robotics_core/sensor_msgs/include/msg/msg/Header.h
+cstdint
+-
+string
+-
+vector
+-
+limits
+-
+
+/home/duongtd/robotics_core/sensor_msgs/include/msg/JoyFeedback.h
+cstdint
+-
+
+/home/duongtd/robotics_core/sensor_msgs/include/msg/JoyFeedbackArray.h
+vector
+-
+msg/JoyFeedback.h
+/home/duongtd/robotics_core/sensor_msgs/include/msg/msg/JoyFeedback.h
+
+/home/duongtd/robotics_core/sensor_msgs/include/msg/PointCloud2.h
+cstdint
+-
+string
+-
+vector
+-
+msg/Header.h
+/home/duongtd/robotics_core/sensor_msgs/include/msg/msg/Header.h
+msg/PointField.h
+/home/duongtd/robotics_core/sensor_msgs/include/msg/msg/PointField.h
+
+/home/duongtd/robotics_core/sensor_msgs/include/msg/PointField.h
+cstdint
+-
+string
+-
+
+/home/duongtd/robotics_core/sensor_msgs/test/main.cpp
+msg/BatteryState.h
+/home/duongtd/robotics_core/sensor_msgs/test/msg/BatteryState.h
+msg/JoyFeedbackArray.h
+/home/duongtd/robotics_core/sensor_msgs/test/msg/JoyFeedbackArray.h
+msg/PointCloud2.h
+/home/duongtd/robotics_core/sensor_msgs/test/msg/PointCloud2.h
+iostream
+-
+
+/home/duongtd/robotics_core/std_msgs/include/msg/Header.h
+string
+-
+chrono
+-
+cstdint
+-
+

build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/DependInfo.cmake

@@ -0,0 +1,22 @@
+# The set of languages for which implicit dependencies are needed:
+set(CMAKE_DEPENDS_LANGUAGES
+  "CXX"
+  )
+# The set of files for implicit dependencies of each language:
+set(CMAKE_DEPENDS_CHECK_CXX
+  "/home/duongtd/robotics_core/sensor_msgs/test/main.cpp" "/home/duongtd/robotics_core/costmap_2d/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o"
+  )
+set(CMAKE_CXX_COMPILER_ID "GNU")
+
+# The include file search paths:
+set(CMAKE_CXX_TARGET_INCLUDE_PATH
+  "/home/duongtd/robotics_core/sensor_msgs/include"
+  "/home/duongtd/robotics_core/std_msgs/include"
+  )
+
+# Targets to which this target links.
+set(CMAKE_TARGET_LINKED_INFO_FILES
+  )
+
+# Fortran module output directory.
+set(CMAKE_Fortran_TARGET_MODULE_DIR "")

build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/build.make

@@ -0,0 +1,98 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 3.16
+
+# Delete rule output on recipe failure.
+.DELETE_ON_ERROR:
+
+
+#=============================================================================
+# Special targets provided by cmake.
+
+# Disable implicit rules so canonical targets will work.
+.SUFFIXES:
+
+
+# Remove some rules from gmake that .SUFFIXES does not remove.
+SUFFIXES =
+
+.SUFFIXES: .hpux_make_needs_suffix_list
+
+
+# Suppress display of executed commands.
+$(VERBOSE).SILENT:
+
+
+# A target that is always out of date.
+cmake_force:
+
+.PHONY : cmake_force
+
+#=============================================================================
+# Set environment variables for the build.
+
+# The shell in which to execute make rules.
+SHELL = /bin/sh
+
+# The CMake executable.
+CMAKE_COMMAND = /usr/bin/cmake
+
+# The command to remove a file.
+RM = /usr/bin/cmake -E remove -f
+
+# Escaping for special characters.
+EQUALS = =
+
+# The top-level source directory on which CMake was run.
+CMAKE_SOURCE_DIR = /home/duongtd/robotics_core/costmap_2d
+
+# The top-level build directory on which CMake was run.
+CMAKE_BINARY_DIR = /home/duongtd/robotics_core/costmap_2d/build
+
+# Include any dependencies generated for this target.
+include sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.make
+
+# Include the progress variables for this target.
+include sensor_msgs_build/CMakeFiles/test_battery_state.dir/progress.make
+
+# Include the compile flags for this target's objects.
+include sensor_msgs_build/CMakeFiles/test_battery_state.dir/flags.make
+
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: sensor_msgs_build/CMakeFiles/test_battery_state.dir/flags.make
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/test/main.cpp
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_1) "Building CXX object sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o"
+	cd /home/duongtd/robotics_core/costmap_2d/build/sensor_msgs_build && /usr/bin/c++  $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -o CMakeFiles/test_battery_state.dir/test/main.cpp.o -c /home/duongtd/robotics_core/sensor_msgs/test/main.cpp
+
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.i: cmake_force
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/test_battery_state.dir/test/main.cpp.i"
+	cd /home/duongtd/robotics_core/costmap_2d/build/sensor_msgs_build && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /home/duongtd/robotics_core/sensor_msgs/test/main.cpp > CMakeFiles/test_battery_state.dir/test/main.cpp.i
+
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.s: cmake_force
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/test_battery_state.dir/test/main.cpp.s"
+	cd /home/duongtd/robotics_core/costmap_2d/build/sensor_msgs_build && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/duongtd/robotics_core/sensor_msgs/test/main.cpp -o CMakeFiles/test_battery_state.dir/test/main.cpp.s
+
+# Object files for target test_battery_state
+test_battery_state_OBJECTS = \
+"CMakeFiles/test_battery_state.dir/test/main.cpp.o"
+
+# External object files for target test_battery_state
+test_battery_state_EXTERNAL_OBJECTS =
+
+sensor_msgs_build/test_battery_state: sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o
+sensor_msgs_build/test_battery_state: sensor_msgs_build/CMakeFiles/test_battery_state.dir/build.make
+sensor_msgs_build/test_battery_state: sensor_msgs_build/CMakeFiles/test_battery_state.dir/link.txt
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --bold --progress-dir=/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_2) "Linking CXX executable test_battery_state"
+	cd /home/duongtd/robotics_core/costmap_2d/build/sensor_msgs_build && $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/test_battery_state.dir/link.txt --verbose=$(VERBOSE)
+
+# Rule to build all files generated by this target.
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/build: sensor_msgs_build/test_battery_state
+
+.PHONY : sensor_msgs_build/CMakeFiles/test_battery_state.dir/build
+
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/clean:
+	cd /home/duongtd/robotics_core/costmap_2d/build/sensor_msgs_build && $(CMAKE_COMMAND) -P CMakeFiles/test_battery_state.dir/cmake_clean.cmake
+.PHONY : sensor_msgs_build/CMakeFiles/test_battery_state.dir/clean
+
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/duongtd/robotics_core/costmap_2d /home/duongtd/robotics_core/sensor_msgs /home/duongtd/robotics_core/costmap_2d/build /home/duongtd/robotics_core/costmap_2d/build/sensor_msgs_build /home/duongtd/robotics_core/costmap_2d/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/DependInfo.cmake --color=$(COLOR)
+.PHONY : sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend
+

build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/cmake_clean.cmake

@@ -0,0 +1,10 @@
+file(REMOVE_RECURSE
+  "CMakeFiles/test_battery_state.dir/test/main.cpp.o"
+  "test_battery_state"
+  "test_battery_state.pdb"
+)
+
+# Per-language clean rules from dependency scanning.
+foreach(lang CXX)
+  include(CMakeFiles/test_battery_state.dir/cmake_clean_${lang}.cmake OPTIONAL)
+endforeach()

build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.internal

@@ -0,0 +1,11 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 3.16
+
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o
+ /home/duongtd/robotics_core/sensor_msgs/include/msg/BatteryState.h
+ /home/duongtd/robotics_core/sensor_msgs/include/msg/JoyFeedback.h
+ /home/duongtd/robotics_core/sensor_msgs/include/msg/JoyFeedbackArray.h
+ /home/duongtd/robotics_core/sensor_msgs/include/msg/PointCloud2.h
+ /home/duongtd/robotics_core/sensor_msgs/include/msg/PointField.h
+ /home/duongtd/robotics_core/sensor_msgs/test/main.cpp
+ /home/duongtd/robotics_core/std_msgs/include/msg/Header.h

build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.make

@@ -0,0 +1,11 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 3.16
+
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/msg/BatteryState.h
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/msg/JoyFeedback.h
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/msg/JoyFeedbackArray.h
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/msg/PointCloud2.h
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/msg/PointField.h
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/test/main.cpp
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/std_msgs/include/msg/Header.h
+

build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/flags.make

@@ -0,0 +1,10 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 3.16
+
+# compile CXX with /usr/bin/c++
+CXX_FLAGS = -fPIE   -std=gnu++17
+
+CXX_DEFINES = 
+
+CXX_INCLUDES = -I/home/duongtd/robotics_core/sensor_msgs/include -I/home/duongtd/robotics_core/std_msgs/include 
+

build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/link.txt

@@ -0,0 +1 @@
+/usr/bin/c++     CMakeFiles/test_battery_state.dir/test/main.cpp.o  -o test_battery_state 

build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/progress.make

@@ -0,0 +1,3 @@
+CMAKE_PROGRESS_1 = 12
+CMAKE_PROGRESS_2 = 13
+

build/sensor_msgs_build/Makefile

@@ -0,0 +1,180 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 3.16
+
+# Default target executed when no arguments are given to make.
+default_target: all
+
+.PHONY : default_target
+
+# Allow only one "make -f Makefile2" at a time, but pass parallelism.
+.NOTPARALLEL:
+
+
+#=============================================================================
+# Special targets provided by cmake.
+
+# Disable implicit rules so canonical targets will work.
+.SUFFIXES:
+
+
+# Remove some rules from gmake that .SUFFIXES does not remove.
+SUFFIXES =
+
+.SUFFIXES: .hpux_make_needs_suffix_list
+
+
+# Suppress display of executed commands.
+$(VERBOSE).SILENT:
+
+
+# A target that is always out of date.
+cmake_force:
+
+.PHONY : cmake_force
+
+#=============================================================================
+# Set environment variables for the build.
+
+# The shell in which to execute make rules.
+SHELL = /bin/sh
+
+# The CMake executable.
+CMAKE_COMMAND = /usr/bin/cmake
+
+# The command to remove a file.
+RM = /usr/bin/cmake -E remove -f
+
+# Escaping for special characters.
+EQUALS = =
+
+# The top-level source directory on which CMake was run.
+CMAKE_SOURCE_DIR = /home/duongtd/robotics_core/costmap_2d
+
+# The top-level build directory on which CMake was run.
+CMAKE_BINARY_DIR = /home/duongtd/robotics_core/costmap_2d/build
+
+#=============================================================================
+# Targets provided globally by CMake.
+
+# Special rule for the target rebuild_cache
+rebuild_cache:
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..."
+	/usr/bin/cmake -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR)
+.PHONY : rebuild_cache
+
+# Special rule for the target rebuild_cache
+rebuild_cache/fast: rebuild_cache
+
+.PHONY : rebuild_cache/fast
+
+# Special rule for the target edit_cache
+edit_cache:
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "No interactive CMake dialog available..."
+	/usr/bin/cmake -E echo No\ interactive\ CMake\ dialog\ available.
+.PHONY : edit_cache
+
+# Special rule for the target edit_cache
+edit_cache/fast: edit_cache
+
+.PHONY : edit_cache/fast
+
+# The main all target
+all: cmake_check_build_system
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(CMAKE_COMMAND) -E cmake_progress_start /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles /home/duongtd/robotics_core/costmap_2d/build/sensor_msgs_build/CMakeFiles/progress.marks
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f CMakeFiles/Makefile2 sensor_msgs_build/all
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles 0
+.PHONY : all
+
+# The main clean target
+clean:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f CMakeFiles/Makefile2 sensor_msgs_build/clean
+.PHONY : clean
+
+# The main clean target
+clean/fast: clean
+
+.PHONY : clean/fast
+
+# Prepare targets for installation.
+preinstall: all
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f CMakeFiles/Makefile2 sensor_msgs_build/preinstall
+.PHONY : preinstall
+
+# Prepare targets for installation.
+preinstall/fast:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f CMakeFiles/Makefile2 sensor_msgs_build/preinstall
+.PHONY : preinstall/fast
+
+# clear depends
+depend:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1
+.PHONY : depend
+
+# Convenience name for target.
+sensor_msgs_build/CMakeFiles/test_battery_state.dir/rule:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f CMakeFiles/Makefile2 sensor_msgs_build/CMakeFiles/test_battery_state.dir/rule
+.PHONY : sensor_msgs_build/CMakeFiles/test_battery_state.dir/rule
+
+# Convenience name for target.
+test_battery_state: sensor_msgs_build/CMakeFiles/test_battery_state.dir/rule
+
+.PHONY : test_battery_state
+
+# fast build rule for target.
+test_battery_state/fast:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f sensor_msgs_build/CMakeFiles/test_battery_state.dir/build.make sensor_msgs_build/CMakeFiles/test_battery_state.dir/build
+.PHONY : test_battery_state/fast
+
+test/main.o: test/main.cpp.o
+
+.PHONY : test/main.o
+
+# target to build an object file
+test/main.cpp.o:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f sensor_msgs_build/CMakeFiles/test_battery_state.dir/build.make sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o
+.PHONY : test/main.cpp.o
+
+test/main.i: test/main.cpp.i
+
+.PHONY : test/main.i
+
+# target to preprocess a source file
+test/main.cpp.i:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f sensor_msgs_build/CMakeFiles/test_battery_state.dir/build.make sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.i
+.PHONY : test/main.cpp.i
+
+test/main.s: test/main.cpp.s
+
+.PHONY : test/main.s
+
+# target to generate assembly for a file
+test/main.cpp.s:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f sensor_msgs_build/CMakeFiles/test_battery_state.dir/build.make sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.s
+.PHONY : test/main.cpp.s
+
+# Help Target
+help:
+	@echo "The following are some of the valid targets for this Makefile:"
+	@echo "... all (the default if no target is provided)"
+	@echo "... clean"
+	@echo "... depend"
+	@echo "... rebuild_cache"
+	@echo "... edit_cache"
+	@echo "... test_battery_state"
+	@echo "... test/main.o"
+	@echo "... test/main.i"
+	@echo "... test/main.s"
+.PHONY : help
+
+
+
+#=============================================================================
+# Special targets to cleanup operation of make.
+
+# Special rule to run CMake to check the build system integrity.
+# No rule that depends on this can have commands that come from listfiles
+# because they might be regenerated.
+cmake_check_build_system:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0
+.PHONY : cmake_check_build_system
+

build/sensor_msgs_build/cmake_install.cmake

@@ -0,0 +1,45 @@
+# Install script for directory: /home/duongtd/robotics_core/sensor_msgs
+
+# Set the install prefix
+if(NOT DEFINED CMAKE_INSTALL_PREFIX)
+  set(CMAKE_INSTALL_PREFIX "/usr/local")
+endif()
+string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}")
+
+# Set the install configuration name.
+if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
+  if(BUILD_TYPE)
+    string(REGEX REPLACE "^[^A-Za-z0-9_]+" ""
+           CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}")
+  else()
+    set(CMAKE_INSTALL_CONFIG_NAME "")
+  endif()
+  message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"")
+endif()
+
+# Set the component getting installed.
+if(NOT CMAKE_INSTALL_COMPONENT)
+  if(COMPONENT)
+    message(STATUS "Install component: \"${COMPONENT}\"")
+    set(CMAKE_INSTALL_COMPONENT "${COMPONENT}")
+  else()
+    set(CMAKE_INSTALL_COMPONENT)
+  endif()
+endif()
+
+# Install shared libraries without execute permission?
+if(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
+  set(CMAKE_INSTALL_SO_NO_EXE "1")
+endif()
+
+# Is this installation the result of a crosscompile?
+if(NOT DEFINED CMAKE_CROSSCOMPILING)
+  set(CMAKE_CROSSCOMPILING "FALSE")
+endif()
+
+if(NOT CMAKE_INSTALL_LOCAL_ONLY)
+  # Include the install script for each subdirectory.
+  include("/home/duongtd/robotics_core/costmap_2d/build/std_msgs_build/cmake_install.cmake")
+
+endif()
+

build/std_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake

@@ -0,0 +1,16 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 3.16
+
+# Relative path conversion top directories.
+set(CMAKE_RELATIVE_PATH_TOP_SOURCE "/home/duongtd/robotics_core/std_msgs")
+set(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/duongtd/robotics_core/costmap_2d/build")
+
+# Force unix paths in dependencies.
+set(CMAKE_FORCE_UNIX_PATHS 1)
+
+
+# The C and CXX include file regular expressions for this directory.
+set(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$")
+set(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$")
+set(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN})
+set(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN})

build/std_msgs_build/CMakeFiles/progress.marks

@@ -0,0 +1 @@
+0

build/std_msgs_build/Makefile

@@ -0,0 +1,134 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 3.16
+
+# Default target executed when no arguments are given to make.
+default_target: all
+
+.PHONY : default_target
+
+# Allow only one "make -f Makefile2" at a time, but pass parallelism.
+.NOTPARALLEL:
+
+
+#=============================================================================
+# Special targets provided by cmake.
+
+# Disable implicit rules so canonical targets will work.
+.SUFFIXES:
+
+
+# Remove some rules from gmake that .SUFFIXES does not remove.
+SUFFIXES =
+
+.SUFFIXES: .hpux_make_needs_suffix_list
+
+
+# Suppress display of executed commands.
+$(VERBOSE).SILENT:
+
+
+# A target that is always out of date.
+cmake_force:
+
+.PHONY : cmake_force
+
+#=============================================================================
+# Set environment variables for the build.
+
+# The shell in which to execute make rules.
+SHELL = /bin/sh
+
+# The CMake executable.
+CMAKE_COMMAND = /usr/bin/cmake
+
+# The command to remove a file.
+RM = /usr/bin/cmake -E remove -f
+
+# Escaping for special characters.
+EQUALS = =
+
+# The top-level source directory on which CMake was run.
+CMAKE_SOURCE_DIR = /home/duongtd/robotics_core/costmap_2d
+
+# The top-level build directory on which CMake was run.
+CMAKE_BINARY_DIR = /home/duongtd/robotics_core/costmap_2d/build
+
+#=============================================================================
+# Targets provided globally by CMake.
+
+# Special rule for the target rebuild_cache
+rebuild_cache:
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..."
+	/usr/bin/cmake -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR)
+.PHONY : rebuild_cache
+
+# Special rule for the target rebuild_cache
+rebuild_cache/fast: rebuild_cache
+
+.PHONY : rebuild_cache/fast
+
+# Special rule for the target edit_cache
+edit_cache:
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "No interactive CMake dialog available..."
+	/usr/bin/cmake -E echo No\ interactive\ CMake\ dialog\ available.
+.PHONY : edit_cache
+
+# Special rule for the target edit_cache
+edit_cache/fast: edit_cache
+
+.PHONY : edit_cache/fast
+
+# The main all target
+all: cmake_check_build_system
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(CMAKE_COMMAND) -E cmake_progress_start /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles /home/duongtd/robotics_core/costmap_2d/build/std_msgs_build/CMakeFiles/progress.marks
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f CMakeFiles/Makefile2 std_msgs_build/all
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles 0
+.PHONY : all
+
+# The main clean target
+clean:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f CMakeFiles/Makefile2 std_msgs_build/clean
+.PHONY : clean
+
+# The main clean target
+clean/fast: clean
+
+.PHONY : clean/fast
+
+# Prepare targets for installation.
+preinstall: all
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f CMakeFiles/Makefile2 std_msgs_build/preinstall
+.PHONY : preinstall
+
+# Prepare targets for installation.
+preinstall/fast:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f CMakeFiles/Makefile2 std_msgs_build/preinstall
+.PHONY : preinstall/fast
+
+# clear depends
+depend:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1
+.PHONY : depend
+
+# Help Target
+help:
+	@echo "The following are some of the valid targets for this Makefile:"
+	@echo "... all (the default if no target is provided)"
+	@echo "... clean"
+	@echo "... depend"
+	@echo "... rebuild_cache"
+	@echo "... edit_cache"
+.PHONY : help
+
+
+
+#=============================================================================
+# Special targets to cleanup operation of make.
+
+# Special rule to run CMake to check the build system integrity.
+# No rule that depends on this can have commands that come from listfiles
+# because they might be regenerated.
+cmake_check_build_system:
+	cd /home/duongtd/robotics_core/costmap_2d/build && $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0
+.PHONY : cmake_check_build_system
+

build/std_msgs_build/cmake_install.cmake

@@ -0,0 +1,39 @@
+# Install script for directory: /home/duongtd/robotics_core/std_msgs
+
+# Set the install prefix
+if(NOT DEFINED CMAKE_INSTALL_PREFIX)
+  set(CMAKE_INSTALL_PREFIX "/usr/local")
+endif()
+string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}")
+
+# Set the install configuration name.
+if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
+  if(BUILD_TYPE)
+    string(REGEX REPLACE "^[^A-Za-z0-9_]+" ""
+           CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}")
+  else()
+    set(CMAKE_INSTALL_CONFIG_NAME "")
+  endif()
+  message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"")
+endif()
+
+# Set the component getting installed.
+if(NOT CMAKE_INSTALL_COMPONENT)
+  if(COMPONENT)
+    message(STATUS "Install component: \"${COMPONENT}\"")
+    set(CMAKE_INSTALL_COMPONENT "${COMPONENT}")
+  else()
+    set(CMAKE_INSTALL_COMPONENT)
+  endif()
+endif()
+
+# Install shared libraries without execute permission?
+if(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
+  set(CMAKE_INSTALL_SO_NO_EXE "1")
+endif()
+
+# Is this installation the result of a crosscompile?
+if(NOT DEFINED CMAKE_CROSSCOMPILING)
+  set(CMAKE_CROSSCOMPILING "FALSE")
+endif()
+

include/costmap_2d/costmap_layer.h

@@ -37,7 +37,6 @@
  *********************************************************************/
 #ifndef COSTMAP_2D_COSTMAP_LAYER_H_
 #define COSTMAP_2D_COSTMAP_LAYER_H_
-#include <ros/ros.h>
 #include <costmap_2d/layer.h>
 #include <costmap_2d/layered_costmap.h>
 

include/costmap_2d/msg.h

@@ -2,8 +2,18 @@
 #define COSTMAP_2D_COST_VALUES_H_MSG
 /** Provides a mapping for often used cost values */
 #include<vector>
+#include<string>
+#include<chrono>
+
 namespace costmap_2d
 {
+ struct Header
+ {
+   uint32_t seq;
+   std::chrono::system_clock::time_point stamp;
+   std::string frame_id;
+ };
+
  struct Point
  {
     double x;
@@ -18,6 +28,12 @@ namespace costmap_2d
     float z;
  };
 
+ struct PointStamped
+ {
+      Header header;
+      Point point;
+ };
+
  struct Polygon
  {
     std::vector<Point32> points;
@@ -52,6 +68,7 @@ namespace costmap_2d
 
  struct PointCloud2
  {
+   Header header;
    uint32_t height;
    uint32_t width;
    std::vector<PointField> fields;
@@ -62,5 +79,7 @@ namespace costmap_2d
    bool is_dense;
  };
 
+ 
+
 }
 #endif  // COSTMAP_2D_COST_VALUES_H_MSG

include/costmap_2d/observation.h

@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2008, 2013, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Authors: Conor McGann
+ */
+
+#ifndef COSTMAP_2D_OBSERVATION_H_
+#define COSTMAP_2D_OBSERVATION_H_
+
+#include <msg/PointCloud2.h>
+#include <geometry_msgs/Point.h>
+
+namespace costmap_2d
+{
+
+/**
+ * @brief Stores an observation in terms of a point cloud and the origin of the source
+ * @note Tried to make members and constructor arguments const but the compiler would not accept the default
+ * assignment operator for vector insertion!
+ */
+class Observation
+{
+public:
+  /**
+   * @brief  Creates an empty observation
+   */
+  Observation() :
+    cloud_(new sensor_msgs::PointCloud2()), obstacle_range_(0.0), raytrace_range_(0.0)
+  {
+  }
+
+  virtual ~Observation()
+  {
+    delete cloud_;
+  }
+
+  /**
+   * @brief  Creates an observation from an origin point and a point cloud
+   * @param origin The origin point of the observation
+   * @param cloud The point cloud of the observation
+   * @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,
+              double obstacle_range, double raytrace_range) :
+      origin_(origin), cloud_(new sensor_msgs::PointCloud2(cloud)),
+      obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
+  {
+  }
+
+  /**
+   * @brief  Copy constructor
+   * @param obs The observation to copy
+   */
+  Observation(const Observation& obs) :
+      origin_(obs.origin_), cloud_(new sensor_msgs::PointCloud2(*(obs.cloud_))),
+      obstacle_range_(obs.obstacle_range_), raytrace_range_(obs.raytrace_range_)
+  {
+  }
+
+  /**
+   * @brief  Creates an observation from a point cloud
+   * @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) :
+      cloud_(new sensor_msgs::PointCloud2(cloud)), obstacle_range_(obstacle_range), raytrace_range_(0.0)
+  {
+  }
+
+  geometry_msgs::Point origin_;
+  sensor_msgs::PointCloud2* cloud_;
+  double obstacle_range_, raytrace_range_;
+};
+
+}  // namespace costmap_2d
+#endif  // COSTMAP_2D_OBSERVATION_H_

include/costmap_2d/observation_buffer.h

@@ -1,50 +1,20 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2008, 2013, Willow Garage, Inc.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of Willow Garage, Inc. nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Eitan Marder-Eppstein
- *********************************************************************/
 #ifndef COSTMAP_2D_OBSERVATION_BUFFER_H_
 #define COSTMAP_2D_OBSERVATION_BUFFER_H_
 
 #include <vector>
 #include <list>
 #include <string>
-#include <ros/time.h>
+#include <chrono>
-#include <costmap_2d/observation.h>
+// #include <ros/time.h>
-#include <tf2_ros/buffer.h>
+// #include <costmap_2d/observation.h>
+// #include <tf2/buffer_core.h>
+
+#include <costmap_2d/observation.h>
+#include <tf2/buffer_core.h>
+#include <msg/PointCloud2.h>
+#include <geometry_msgs/PointStamped.h>
+#include <geometry_msgs/Point.h>
 
-#include <sensor_msgs/PointCloud2.h>
 
 // Thread support
 #include <boost/thread.hpp>
@@ -58,6 +28,7 @@ namespace costmap_2d
 class ObservationBuffer
 {
 public:
+
   /**
    * @brief  Constructs an observation buffer
    * @param  topic_name The topic of the observations, used as an identifier for error and warning messages
@@ -74,7 +45,7 @@ public:
    */
   ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
                     double min_obstacle_height, double max_obstacle_height, double obstacle_range,
-                    double raytrace_range, tf2_ros::Buffer& tf2_buffer, std::string global_frame,
+                    double raytrace_range, tf2::BufferCore& tf2_buffer, std::string global_frame,
                     std::string sensor_frame, double tf_tolerance);
 
   /**
@@ -137,10 +108,13 @@ private:
    */
   void purgeStaleObservations();
 
-  tf2_ros::Buffer& tf2_buffer_;
+  tf2::BufferCore& tf2_buffer_;
-  const ros::Duration observation_keep_time_;
+  // const ros::Duration observation_keep_time_;
-  const ros::Duration expected_update_rate_;
+  // const ros::Duration expected_update_rate_;
-  ros::Time last_updated_;
+  // ros::Time last_updated_;
+  const std::chrono::duration<double> observation_keep_time_;
+  const std::chrono::duration<double> expected_update_rate_;
+  std::chrono::time_point<std::chrono::system_clock> last_updated_;
   std::string global_frame_;
   std::string sensor_frame_;
   std::list<Observation> observation_list_;

src/costmap_layer.cpp

@@ -0,0 +1,173 @@
+#include<costmap_2d/costmap_layer.h>
+
+namespace costmap_2d
+{
+
+void CostmapLayer::touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y)
+{
+    *min_x = std::min(x, *min_x);
+    *min_y = std::min(y, *min_y);
+    *max_x = std::max(x, *max_x);
+    *max_y = std::max(y, *max_y);
+}
+
+void CostmapLayer::matchSize()
+{
+    Costmap2D* master = layered_costmap_->getCostmap();
+    resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
+            master->getOriginX(), master->getOriginY());
+}
+
+void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y, bool invert_area)
+{
+  unsigned char* grid = getCharMap();
+  for(int x=0; x<(int)getSizeInCellsX(); x++){
+    bool xrange = x>start_x && x<end_x;
+
+    for(int y=0; y<(int)getSizeInCellsY(); y++){
+      if((xrange && y>start_y && y<end_y)!=invert_area)
+        continue;
+      int index = getIndex(x,y);
+      if(grid[index]!=NO_INFORMATION){
+        grid[index] = NO_INFORMATION;
+      }
+    }
+  }
+}
+
+void CostmapLayer::addExtraBounds(double mx0, double my0, double mx1, double my1)
+{
+    extra_min_x_ = std::min(mx0, extra_min_x_);
+    extra_max_x_ = std::max(mx1, extra_max_x_);
+    extra_min_y_ = std::min(my0, extra_min_y_);
+    extra_max_y_ = std::max(my1, extra_max_y_);
+    has_extra_bounds_ = true;
+}
+
+void CostmapLayer::useExtraBounds(double* min_x, double* min_y, double* max_x, double* max_y)
+{
+    if (!has_extra_bounds_)
+        return;
+
+    *min_x = std::min(extra_min_x_, *min_x);
+    *min_y = std::min(extra_min_y_, *min_y);
+    *max_x = std::max(extra_max_x_, *max_x);
+    *max_y = std::max(extra_max_y_, *max_y);
+    extra_min_x_ = 1e6;
+    extra_min_y_ = 1e6;
+    extra_max_x_ = -1e6;
+    extra_max_y_ = -1e6;
+    has_extra_bounds_ = false;
+}
+
+void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+{
+  if (!enabled_)
+    return;
+
+  unsigned char* master_array = master_grid.getCharMap();
+  unsigned int span = master_grid.getSizeInCellsX();
+ 
+  for (int j = min_j; j < max_j; j++)
+  { 
+    unsigned int it = j * span + min_i;
+    for (int i = min_i; i < max_i; i++)
+    { 
+      if (costmap_[it] == NO_INFORMATION){
+        it++;
+        continue;
+      }
+      if (costmap_[it] == CRITICAL_SPACE){
+        master_array[it] = costmap_[it];
+        it++;
+        continue;
+      }
+      if (costmap_[it] == PREFERRED_SPACE && master_array[it] < INSCRIBED_INFLATED_OBSTACLE ){
+        master_array[it] = costmap_[it];
+        it++;
+        continue;
+      }
+      if (costmap_[it] == UNPREFERRED_SPACE && master_array[it] < INSCRIBED_INFLATED_OBSTACLE ){
+        master_array[it] = costmap_[it];
+        it++;
+        continue;
+      }
+      unsigned char old_cost = master_array[it];
+      if (old_cost == NO_INFORMATION || old_cost < costmap_[it])
+        master_array[it] = costmap_[it];
+      it++;
+    }
+  }
+}
+
+void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
+                                           int max_i, int max_j)
+{
+  if (!enabled_)
+    return;
+  unsigned char* master = master_grid.getCharMap();
+  unsigned int span = master_grid.getSizeInCellsX();
+
+  for (int j = min_j; j < max_j; j++)
+  {
+    unsigned int it = span*j+min_i;
+    for (int i = min_i; i < max_i; i++)
+    {
+      master[it] = costmap_[it];
+      it++;
+    }
+  }
+}
+
+void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+{
+  if (!enabled_)
+    return;
+  unsigned char* master = master_grid.getCharMap();
+  unsigned int span = master_grid.getSizeInCellsX();
+
+  for (int j = min_j; j < max_j; j++)
+  {
+    unsigned int it = span*j+min_i;
+    for (int i = min_i; i < max_i; i++)
+    {
+      if (costmap_[it] != NO_INFORMATION)
+        master[it] = costmap_[it];
+      it++;
+    }
+  }
+}
+
+void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+{
+  if (!enabled_)
+    return;
+  unsigned char* master_array = master_grid.getCharMap();
+  unsigned int span = master_grid.getSizeInCellsX();
+
+  for (int j = min_j; j < max_j; j++)
+  {
+    unsigned int it = j * span + min_i;
+    for (int i = min_i; i < max_i; i++)
+    {
+      if (costmap_[it] == NO_INFORMATION){
+        it++;
+        continue;
+      }
+
+      unsigned char old_cost = master_array[it];
+      if (old_cost == NO_INFORMATION)
+        master_array[it] = costmap_[it];
+      else
+      {
+        int sum = old_cost + costmap_[it];
+        if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+            master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
+        else
+            master_array[it] = sum;
+      }
+      it++;
+    }
+  }
+}
+}  // namespace costmap_2d

src/costmap_math.cpp

@@ -0,0 +1,90 @@
+/*
+ * Copyright (c) 2013, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <costmap_2d/costmap_math.h>
+#include <geometry_msgs/Point.h>
+
+double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)
+{
+  double A = pX - x0;
+  double B = pY - y0;
+  double C = x1 - x0;
+  double D = y1 - y0;
+
+  double dot = A * C + B * D;
+  double len_sq = C * C + D * D;
+  double param = dot / len_sq;
+
+  double xx, yy;
+
+  if (param < 0)
+  {
+    xx = x0;
+    yy = y0;
+  }
+  else if (param > 1)
+  {
+    xx = x1;
+    yy = y1;
+  }
+  else
+  {
+    xx = x0 + param * C;
+    yy = y0 + param * D;
+  }
+
+  return distance(pX, pY, xx, yy);
+}
+
+bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float testy)
+{
+  bool c = false;
+  int i, j, nvert = polygon.size();
+  for (i = 0, j = nvert - 1; i < nvert; j = i++)
+  {
+    float yi = polygon[i].y, yj = polygon[j].y, xi = polygon[i].x, xj = polygon[j].x;
+
+    if (((yi > testy) != (yj > testy)) && (testx < (xj - xi) * (testy - yi) / (yj - yi) + xi))
+      c = !c;
+  }
+  return c;
+}
+
+bool intersects_helper(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2)
+{
+  for (unsigned int i = 0; i < polygon1.size(); i++)
+    if (intersects(polygon2, polygon1[i].x, polygon1[i].y))
+      return true;
+  return false;
+}
+
+bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2)
+{
+  return intersects_helper(polygon1, polygon2) || intersects_helper(polygon2, polygon1);
+}

src/observation_buffer.cpp

@@ -1,44 +1,13 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2008, 2013, Willow Garage, Inc.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of Willow Garage, Inc. nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Eitan Marder-Eppstein
- *********************************************************************/
 #include <costmap_2d/observation_buffer.h>
 
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
+// #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
+// #include <sensor_msgs/point_cloud2_iterator.h>
+#include<tf2/convert.h>
 #include <sensor_msgs/point_cloud2_iterator.h>
+#include <cstdint>
+
+
 
 using namespace std;
 using namespace tf2;
@@ -47,10 +16,11 @@ namespace 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,
-                                     double raytrace_range, tf2_ros::Buffer& tf2_buffer, string global_frame,
+                                     double raytrace_range, tf2::BufferCore& tf2_buffer, string global_frame,
                                      string sensor_frame, double tf_tolerance) :
     tf2_buffer_(tf2_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
-    last_updated_(ros::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
+    last_updated_(std::chrono::system_clock::now()), 
+    global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
     min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
     obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance)
 {
@@ -62,13 +32,17 @@ ObservationBuffer::~ObservationBuffer()
 
 bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
 {
-  ros::Time transform_time = ros::Time::now();
+  geometry_msgs::Point A;
+  // ros::Time transform_time = ros::Time::now();
+  double transform_time =
+    std::chrono::duration<double>(
+        std::chrono::system_clock::now().time_since_epoch()).count();
+
   std::string tf_error;
 
-  geometry_msgs::TransformStamped transformStamped;
+  if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, tf2::Time::now(), &tf_error))
-  if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, transform_time, ros::Duration(tf_tolerance_), &tf_error))
   {
-    ROS_ERROR("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(),
+    printf("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(),
               global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
     return false;
   }
@@ -86,15 +60,21 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
       origin.point = obs.origin_;
 
       // we need to transform the origin of the observation to the new global frame
-      tf2_buffer_.transform(origin, origin, new_global_frame);
+      tf2::doTransform(origin, origin, 
+        tf2_buffer_.lookupTransform(new_global_frame, 
+                        tf2::getFrameId(origin),
+                        tf2::getTimestamp(origin)));
       obs.origin_ = origin.point;
 
       // we also need to transform the cloud of the observation to the new global frame
-      tf2_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame);
+      tf2::doTransform(*(obs.cloud_), *(obs.cloud_), 
+        tf2_buffer_.lookupTransform(new_global_frame, 
+                        tf2::getFrameId(*(obs.cloud_)),
+                        tf2::getTimestamp(*(obs.cloud_))));
     }
     catch (TransformException& ex)
     {
-      ROS_ERROR("TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(),
+      printf("TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(),
                 new_global_frame.c_str(), ex.what());
       return false;
     }
@@ -124,7 +104,10 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
     local_origin.point.x = 0;
     local_origin.point.y = 0;
     local_origin.point.z = 0;
-    tf2_buffer_.transform(local_origin, global_origin, global_frame_);
+    tf2::doTransform(local_origin, global_origin, 
+        tf2_buffer_.lookupTransform(global_frame_, 
+                        tf2::getFrameId(local_origin),
+                        tf2::getTimestamp(local_origin)));
     tf2::convert(global_origin.point, observation_list_.front().origin_);
 
     // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
@@ -134,7 +117,10 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
     sensor_msgs::PointCloud2 global_frame_cloud;
 
     // transform the point cloud
-    tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_);
+    tf2::doTransform(cloud, global_frame_cloud, 
+        tf2_buffer_.lookupTransform(global_frame_, 
+                        tf2::getFrameId(cloud),
+                        tf2::getTimestamp(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
@@ -176,13 +162,13 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
   {
     // if an exception occurs, we need to remove the empty observation from the list
     observation_list_.pop_front();
-    ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(),
+    printf("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(),
               cloud.header.frame_id.c_str(), ex.what());
     return;
   }
 
   // if the update was successful, we want to update the last updated time
-  last_updated_ = ros::Time::now();
+  last_updated_ = std::chrono::system_clock::now();
 
   // we'll also remove any stale observations from the list
   purgeStaleObservations();
@@ -208,7 +194,7 @@ void ObservationBuffer::purgeStaleObservations()
   {
     list<Observation>::iterator obs_it = observation_list_.begin();
     // if we're keeping observations for no time... then we'll only keep one observation
-    if (observation_keep_time_ == ros::Duration(0.0))
+    if (observation_keep_time_ == std::chrono::duration<double>(0.0))
     {
       observation_list_.erase(++obs_it, observation_list_.end());
       return;
@@ -219,7 +205,7 @@ void ObservationBuffer::purgeStaleObservations()
     {
       Observation& obs = *obs_it;
       // check if the observation is out of date... and if it is, remove it and those that follow from the list
-      if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_)
+      if ((std::chrono::duration<double>(last_updated_.time_since_epoch()).count() - obs.cloud_->header.stamp) > observation_keep_time_.count())
       {
         observation_list_.erase(obs_it, observation_list_.end());
         return;
@@ -230,22 +216,24 @@ void ObservationBuffer::purgeStaleObservations()
 
 bool ObservationBuffer::isCurrent() const
 {
-  if (expected_update_rate_ == ros::Duration(0.0))
+  if (expected_update_rate_ == std::chrono::duration<double>(0.0))
     return true;
 
-  bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
+  bool current = (std::chrono::system_clock::now() - last_updated_) <= expected_update_rate_;
   if (!current)
   {
-    ROS_WARN(
+    printf(
         "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.",
-        topic_name_.c_str(), (ros::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
+        topic_name_.c_str(), std::chrono::duration<double>(std::chrono::system_clock::now() - last_updated_).count(), 
+        expected_update_rate_.count());
   }
   return current;
+  return true;
 }
 
 void ObservationBuffer::resetLastUpdated()
 {
-  last_updated_ = ros::Time::now();
+  last_updated_ = std::chrono::system_clock::now();
 }
 }  // namespace costmap_2d