diff --git a/CMakeLists.txt b/CMakeLists.txt index c9ab2e0..2b76125 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,11 +5,11 @@ 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) + add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/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) + add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/geometry_msgs ${CMAKE_BINARY_DIR}/geometry_msgs_build) endif() # ---- Dependencies ---- diff --git a/build/CMakeCache.txt b/build/CMakeCache.txt index bc1def4..5d86de7 100644 --- a/build/CMakeCache.txt +++ b/build/CMakeCache.txt @@ -247,7 +247,7 @@ costmap_2d_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/costmap_2d 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 +geometry_msgs_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/common_msgs/geometry_msgs //Dependencies for the target layers_LIB_DEPENDS:STATIC=general;costmap_2d;general;Boost::system;general;Boost::thread; @@ -256,13 +256,13 @@ layers_LIB_DEPENDS:STATIC=general;costmap_2d;general;Boost::system;general;Boost 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 +sensor_msgs_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/common_msgs/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 +std_msgs_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/common_msgs/std_msgs ######################## diff --git a/build/CMakeFiles/Makefile.cmake b/build/CMakeFiles/Makefile.cmake index 7d9ac80..4e3e1bf 100644 --- a/build/CMakeFiles/Makefile.cmake +++ b/build/CMakeFiles/Makefile.cmake @@ -7,13 +7,13 @@ set(CMAKE_DEPENDS_GENERATOR "Unix Makefiles") # The top level Makefile was generated from the following files: set(CMAKE_MAKEFILE_DEPENDS "CMakeCache.txt" + "/home/duongtd/robotics_core/common_msgs/geometry_msgs/CMakeLists.txt" + "/home/duongtd/robotics_core/common_msgs/sensor_msgs/CMakeLists.txt" + "/home/duongtd/robotics_core/common_msgs/std_msgs/CMakeLists.txt" "../CMakeLists.txt" "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" diff --git a/build/CMakeFiles/costmap_2d.dir/CXX.includecache b/build/CMakeFiles/costmap_2d.dir/CXX.includecache index 81711a5..30d8990 100644 --- a/build/CMakeFiles/costmap_2d.dir/CXX.includecache +++ b/build/CMakeFiles/costmap_2d.dir/CXX.includecache @@ -6,99 +6,79 @@ #IncludeRegexTransform: -../include/costmap_2d/observation.h -sensor_msgs/PointCloud2.h -- -geometry_msgs/Point.h -- +../include/costmap_2d/cost_values.h -../include/costmap_2d/observation_buffer.h +../include/costmap_2d/costmap_2d.h vector - -list -- -string -- -chrono -- -costmap_2d/observation.h -- -tf2/buffer_core.h -- -sensor_msgs/PointCloud2.h -- -geometry_msgs/PointStamped.h -- -geometry_msgs/Point.h +queue - boost/thread.hpp - - -/home/duongtd/robotics_core/costmap_2d/src/observation_buffer.cpp -costmap_2d/observation_buffer.h -- -tf2/convert.h -- -sensor_msgs/point_cloud2_iterator.h -- -cstdint +geometry_msgs/Point.h - -/home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h +../include/costmap_2d/costmap_layer.h +costmap_2d/layer.h +- +costmap_2d/layered_costmap.h +- + +../include/costmap_2d/footprint.h +geometry_msgs/Point32.h +- +geometry_msgs/Polygon.h +- +geometry_msgs/PolygonStamped.h +- +string.h +- +vector +- + +../include/costmap_2d/layer.h +costmap_2d/costmap_2d.h +- +costmap_2d/layered_costmap.h +- +string +- +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 +- + +/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h cmath - iostream - -/home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/PointStamped.h +/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point32.h + +/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Polygon.h +vector +- +geometry_msgs/Point32.h +- + +/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/PolygonStamped.h std_msgs/Header.h -/home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/std_msgs/Header.h -geometry_msgs/Point.h -/home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/geometry_msgs/Point.h - -/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointCloud2.h -cstdint - -string -- -vector -- -std_msgs/Header.h -/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/std_msgs/Header.h -sensor_msgs/PointField.h -/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/sensor_msgs/PointField.h - -/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointField.h -cstdint -- -string +geometry_msgs/Polygon.h - -/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h -sensor_msgs/PointCloud2.h -- -cstdarg -- -sstream -- -string -- -vector -- - -/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h -sensor_msgs/PointCloud2.h -- -cstdarg -- -string -- -vector -- -sensor_msgs/impl/point_cloud2_iterator.h -- - -/home/duongtd/robotics_core/std_msgs/include/std_msgs/Header.h +/home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.h string - chrono @@ -106,3 +86,25 @@ chrono cstdint - +/home/duongtd/robotics_core/costmap_2d/src/costmap_layer.cpp +costmap_2d/costmap_layer.h +- + +/home/duongtd/robotics_core/costmap_2d/src/layer.cpp +costmap_2d/layer.h +/home/duongtd/robotics_core/costmap_2d/src/costmap_2d/layer.h + +/home/duongtd/robotics_core/costmap_2d/src/layered_costmap.cpp +costmap_2d/layered_costmap.h +- +costmap_2d/footprint.h +- +cstdio +- +string +- +algorithm +- +vector +- + diff --git a/build/CMakeFiles/costmap_2d.dir/DependInfo.cmake b/build/CMakeFiles/costmap_2d.dir/DependInfo.cmake index 6ea01ef..79efda2 100644 --- a/build/CMakeFiles/costmap_2d.dir/DependInfo.cmake +++ b/build/CMakeFiles/costmap_2d.dir/DependInfo.cmake @@ -27,9 +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" + "/home/duongtd/robotics_core/common_msgs/sensor_msgs/include" + "/home/duongtd/robotics_core/common_msgs/std_msgs/include" + "/home/duongtd/robotics_core/common_msgs/geometry_msgs/include" ) # Targets to which this target links. diff --git a/build/CMakeFiles/costmap_2d.dir/depend.internal b/build/CMakeFiles/costmap_2d.dir/depend.internal index 9f3fb55..3dd7ffa 100644 --- a/build/CMakeFiles/costmap_2d.dir/depend.internal +++ b/build/CMakeFiles/costmap_2d.dir/depend.internal @@ -5,57 +5,57 @@ CMakeFiles/costmap_2d.dir/src/array_parser.cpp.o /home/duongtd/robotics_core/costmap_2d/src/array_parser.cpp CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o ../include/costmap_2d/costmap_2d.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h /home/duongtd/robotics_core/costmap_2d/src/costmap_2d.cpp - /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h 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 + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h /home/duongtd/robotics_core/costmap_2d/src/costmap_layer.cpp - /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o ../include/costmap_2d/costmap_math.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.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 ../include/costmap_2d/footprint.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point32.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Polygon.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/PolygonStamped.h + /home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.h /home/duongtd/robotics_core/costmap_2d/src/footprint.cpp - /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h - /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point32.h - /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Polygon.h - /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/PolygonStamped.h - /home/duongtd/robotics_core/std_msgs/include/std_msgs/Header.h CMakeFiles/costmap_2d.dir/src/layer.cpp.o ../include/costmap_2d/cost_values.h ../include/costmap_2d/costmap_2d.h ../include/costmap_2d/layer.h ../include/costmap_2d/layered_costmap.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h /home/duongtd/robotics_core/costmap_2d/src/layer.cpp - /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o ../include/costmap_2d/cost_values.h ../include/costmap_2d/costmap_2d.h ../include/costmap_2d/footprint.h ../include/costmap_2d/layer.h ../include/costmap_2d/layered_costmap.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point32.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Polygon.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/PolygonStamped.h + /home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.h /home/duongtd/robotics_core/costmap_2d/src/layered_costmap.cpp - /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h - /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point32.h - /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Polygon.h - /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/PolygonStamped.h - /home/duongtd/robotics_core/std_msgs/include/std_msgs/Header.h CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o ../include/costmap_2d/observation.h ../include/costmap_2d/observation_buffer.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/PointStamped.h + /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/PointCloud2.h + /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/PointField.h + /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h + /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h + /home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.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/sensor_msgs/PointCloud2.h - /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/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/std_msgs/Header.h diff --git a/build/CMakeFiles/costmap_2d.dir/depend.make b/build/CMakeFiles/costmap_2d.dir/depend.make index 04a1640..c832dfe 100644 --- a/build/CMakeFiles/costmap_2d.dir/depend.make +++ b/build/CMakeFiles/costmap_2d.dir/depend.make @@ -4,58 +4,58 @@ CMakeFiles/costmap_2d.dir/src/array_parser.cpp.o: ../src/array_parser.cpp CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o: ../include/costmap_2d/costmap_2d.h +CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o: ../src/costmap_2d.cpp -CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h 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: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: ../src/costmap_layer.cpp -CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o: ../include/costmap_2d/costmap_math.h +CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.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 +CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h +CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point32.h +CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Polygon.h +CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/PolygonStamped.h +CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: /home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.h CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: ../src/footprint.cpp -CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h -CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point32.h -CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Polygon.h -CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/PolygonStamped.h -CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: /home/duongtd/robotics_core/std_msgs/include/std_msgs/Header.h CMakeFiles/costmap_2d.dir/src/layer.cpp.o: ../include/costmap_2d/cost_values.h CMakeFiles/costmap_2d.dir/src/layer.cpp.o: ../include/costmap_2d/costmap_2d.h CMakeFiles/costmap_2d.dir/src/layer.cpp.o: ../include/costmap_2d/layer.h CMakeFiles/costmap_2d.dir/src/layer.cpp.o: ../include/costmap_2d/layered_costmap.h +CMakeFiles/costmap_2d.dir/src/layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h CMakeFiles/costmap_2d.dir/src/layer.cpp.o: ../src/layer.cpp -CMakeFiles/costmap_2d.dir/src/layer.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: ../include/costmap_2d/cost_values.h CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: ../include/costmap_2d/costmap_2d.h CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: ../include/costmap_2d/footprint.h CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: ../include/costmap_2d/layer.h CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: ../include/costmap_2d/layered_costmap.h +CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h +CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point32.h +CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Polygon.h +CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/PolygonStamped.h +CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: /home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.h CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: ../src/layered_costmap.cpp -CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h -CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point32.h -CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Polygon.h -CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/PolygonStamped.h -CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: /home/duongtd/robotics_core/std_msgs/include/std_msgs/Header.h 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: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h +CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/PointStamped.h +CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/PointCloud2.h +CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/PointField.h +CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h +CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h +CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.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/sensor_msgs/PointCloud2.h -CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/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/std_msgs/Header.h diff --git a/build/CMakeFiles/costmap_2d.dir/flags.make b/build/CMakeFiles/costmap_2d.dir/flags.make index 7405334..6ac81f3 100644 --- a/build/CMakeFiles/costmap_2d.dir/flags.make +++ b/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 -I/home/duongtd/robotics_core/sensor_msgs/include -I/home/duongtd/robotics_core/std_msgs/include -I/home/duongtd/robotics_core/geometry_msgs/include +CXX_INCLUDES = -I/home/duongtd/robotics_core/costmap_2d/include -I/usr/include/eigen3 -I/home/duongtd/robotics_core/common_msgs/sensor_msgs/include -I/home/duongtd/robotics_core/common_msgs/std_msgs/include -I/home/duongtd/robotics_core/common_msgs/geometry_msgs/include diff --git a/build/CMakeFiles/layers.dir/CXX.includecache b/build/CMakeFiles/layers.dir/CXX.includecache index cee5a8c..ec08f10 100644 --- a/build/CMakeFiles/layers.dir/CXX.includecache +++ b/build/CMakeFiles/layers.dir/CXX.includecache @@ -6,5 +6,109 @@ #IncludeRegexTransform: -/home/duongtd/robotics_core/costmap_2d/plugins/inflation_layer.cpp +../include/costmap_2d/cost_values.h + +../include/costmap_2d/costmap_2d.h +vector +- +queue +- +boost/thread.hpp +- +geometry_msgs/Point.h +- + +../include/costmap_2d/costmap_math.h +math.h +- +algorithm +- +vector +- +geometry_msgs/Point.h +- + +../include/costmap_2d/footprint.h +geometry_msgs/Point32.h +- +geometry_msgs/Polygon.h +- +geometry_msgs/PolygonStamped.h +- +string.h +- +vector +- + +../include/costmap_2d/inflation_layer.h +costmap_2d/layer.h +- +costmap_2d/layered_costmap.h +- +boost/thread.hpp +- +boost/dll/alias.hpp +- + +../include/costmap_2d/layer.h +costmap_2d/costmap_2d.h +- +costmap_2d/layered_costmap.h +- +string +- +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 +- + +/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h +cmath +- +iostream +- + +/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point32.h + +/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Polygon.h +vector +- +geometry_msgs/Point32.h +- + +/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/PolygonStamped.h +std_msgs/Header.h +- +geometry_msgs/Polygon.h +- + +/home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.h +string +- +chrono +- +cstdint +- + +/home/duongtd/robotics_core/costmap_2d/plugins/inflation_layer.cpp +algorithm +- +costmap_2d/inflation_layer.h +- +costmap_2d/costmap_math.h +- +costmap_2d/footprint.h +- +boost/thread.hpp +- diff --git a/build/CMakeFiles/layers.dir/DependInfo.cmake b/build/CMakeFiles/layers.dir/DependInfo.cmake index 6a44d8c..8e0f2b4 100644 --- a/build/CMakeFiles/layers.dir/DependInfo.cmake +++ b/build/CMakeFiles/layers.dir/DependInfo.cmake @@ -20,9 +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" + "/home/duongtd/robotics_core/common_msgs/sensor_msgs/include" + "/home/duongtd/robotics_core/common_msgs/std_msgs/include" + "/home/duongtd/robotics_core/common_msgs/geometry_msgs/include" ) # Targets to which this target links. diff --git a/build/CMakeFiles/layers.dir/depend.internal b/build/CMakeFiles/layers.dir/depend.internal index 5636432..d22ebaa 100644 --- a/build/CMakeFiles/layers.dir/depend.internal +++ b/build/CMakeFiles/layers.dir/depend.internal @@ -2,4 +2,16 @@ # Generated by "Unix Makefiles" Generator, CMake Version 3.16 CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o + ../include/costmap_2d/cost_values.h + ../include/costmap_2d/costmap_2d.h + ../include/costmap_2d/costmap_math.h + ../include/costmap_2d/footprint.h + ../include/costmap_2d/inflation_layer.h + ../include/costmap_2d/layer.h + ../include/costmap_2d/layered_costmap.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point32.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Polygon.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/PolygonStamped.h + /home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.h /home/duongtd/robotics_core/costmap_2d/plugins/inflation_layer.cpp diff --git a/build/CMakeFiles/layers.dir/depend.make b/build/CMakeFiles/layers.dir/depend.make index 8013d5e..9293b7c 100644 --- a/build/CMakeFiles/layers.dir/depend.make +++ b/build/CMakeFiles/layers.dir/depend.make @@ -1,5 +1,17 @@ # CMAKE generated file: DO NOT EDIT! # Generated by "Unix Makefiles" Generator, CMake Version 3.16 +CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o: ../include/costmap_2d/cost_values.h +CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o: ../include/costmap_2d/costmap_2d.h +CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o: ../include/costmap_2d/costmap_math.h +CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o: ../include/costmap_2d/footprint.h +CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o: ../include/costmap_2d/inflation_layer.h +CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o: ../include/costmap_2d/layer.h +CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o: ../include/costmap_2d/layered_costmap.h +CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h +CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point32.h +CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Polygon.h +CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/PolygonStamped.h +CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.h CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o: ../plugins/inflation_layer.cpp diff --git a/build/CMakeFiles/layers.dir/flags.make b/build/CMakeFiles/layers.dir/flags.make index 7405334..6ac81f3 100644 --- a/build/CMakeFiles/layers.dir/flags.make +++ b/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 -I/home/duongtd/robotics_core/sensor_msgs/include -I/home/duongtd/robotics_core/std_msgs/include -I/home/duongtd/robotics_core/geometry_msgs/include +CXX_INCLUDES = -I/home/duongtd/robotics_core/costmap_2d/include -I/usr/include/eigen3 -I/home/duongtd/robotics_core/common_msgs/sensor_msgs/include -I/home/duongtd/robotics_core/common_msgs/std_msgs/include -I/home/duongtd/robotics_core/common_msgs/geometry_msgs/include diff --git a/build/CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o b/build/CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o index 0a2a870..cd064e3 100644 Binary files a/build/CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o and b/build/CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o differ diff --git a/build/geometry_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake b/build/geometry_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake index 910436e..f2725ea 100644 --- a/build/geometry_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake +++ b/build/geometry_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake @@ -2,7 +2,7 @@ # 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_SOURCE "/home/duongtd/robotics_core/common_msgs/geometry_msgs") set(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/duongtd/robotics_core/costmap_2d/build") # Force unix paths in dependencies. diff --git a/build/geometry_msgs_build/cmake_install.cmake b/build/geometry_msgs_build/cmake_install.cmake index 745c8b7..985e418 100644 --- a/build/geometry_msgs_build/cmake_install.cmake +++ b/build/geometry_msgs_build/cmake_install.cmake @@ -1,4 +1,4 @@ -# Install script for directory: /home/duongtd/robotics_core/geometry_msgs +# Install script for directory: /home/duongtd/robotics_core/common_msgs/geometry_msgs # Set the install prefix if(NOT DEFINED CMAKE_INSTALL_PREFIX) diff --git a/build/liblayers.a b/build/liblayers.a index 65e07b5..1bce906 100644 Binary files a/build/liblayers.a and b/build/liblayers.a differ diff --git a/build/sensor_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake b/build/sensor_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake index 058c06b..5e2f6dd 100644 --- a/build/sensor_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake +++ b/build/sensor_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake @@ -2,7 +2,7 @@ # 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_SOURCE "/home/duongtd/robotics_core/common_msgs/sensor_msgs") set(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/duongtd/robotics_core/costmap_2d/build") # Force unix paths in dependencies. diff --git a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/CXX.includecache b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/CXX.includecache index 2b5b9a7..8b1ce56 100644 --- a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/CXX.includecache +++ b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/CXX.includecache @@ -6,9 +6,9 @@ #IncludeRegexTransform: -/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/BatteryState.h +/home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/BatteryState.h std_msgs/Header.h -/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/std_msgs/Header.h +/home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/std_msgs/Header.h cstdint - string @@ -18,17 +18,17 @@ vector limits - -/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/JoyFeedback.h +/home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/JoyFeedback.h cstdint - -/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/JoyFeedbackArray.h +/home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/JoyFeedbackArray.h vector - sensor_msgs/JoyFeedback.h -/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/sensor_msgs/JoyFeedback.h +/home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/sensor_msgs/JoyFeedback.h -/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointCloud2.h +/home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/PointCloud2.h cstdint - string @@ -36,27 +36,27 @@ string vector - std_msgs/Header.h -/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/std_msgs/Header.h +/home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/std_msgs/Header.h sensor_msgs/PointField.h -/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/sensor_msgs/PointField.h +/home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/sensor_msgs/PointField.h -/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointField.h +/home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/PointField.h cstdint - string - -/home/duongtd/robotics_core/sensor_msgs/test/main.cpp +/home/duongtd/robotics_core/common_msgs/sensor_msgs/test/main.cpp sensor_msgs/BatteryState.h -/home/duongtd/robotics_core/sensor_msgs/test/sensor_msgs/BatteryState.h +/home/duongtd/robotics_core/common_msgs/sensor_msgs/test/sensor_msgs/BatteryState.h sensor_msgs/JoyFeedbackArray.h -/home/duongtd/robotics_core/sensor_msgs/test/sensor_msgs/JoyFeedbackArray.h +/home/duongtd/robotics_core/common_msgs/sensor_msgs/test/sensor_msgs/JoyFeedbackArray.h sensor_msgs/PointCloud2.h -/home/duongtd/robotics_core/sensor_msgs/test/sensor_msgs/PointCloud2.h +/home/duongtd/robotics_core/common_msgs/sensor_msgs/test/sensor_msgs/PointCloud2.h iostream - -/home/duongtd/robotics_core/std_msgs/include/std_msgs/Header.h +/home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.h string - chrono diff --git a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/DependInfo.cmake b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/DependInfo.cmake index b1d5649..8643432 100644 --- a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/DependInfo.cmake +++ b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/DependInfo.cmake @@ -4,14 +4,14 @@ set(CMAKE_DEPENDS_LANGUAGES ) # 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" + "/home/duongtd/robotics_core/common_msgs/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" + "/home/duongtd/robotics_core/common_msgs/sensor_msgs/include" + "/home/duongtd/robotics_core/common_msgs/std_msgs/include" ) # Targets to which this target links. diff --git a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/build.make b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/build.make index 10fe931..e21bdf7 100644 --- a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/build.make +++ b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/build.make @@ -58,17 +58,17 @@ include sensor_msgs_build/CMakeFiles/test_battery_state.dir/progress.make 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 +sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/common_msgs/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 + 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/common_msgs/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 + 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/common_msgs/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 + 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/common_msgs/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 = \ @@ -93,6 +93,6 @@ sensor_msgs_build/CMakeFiles/test_battery_state.dir/clean: .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) + 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/common_msgs/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 diff --git a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.internal b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.internal index 218ff48..6fd2d92 100644 --- a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.internal +++ b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.internal @@ -2,10 +2,10 @@ # 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/sensor_msgs/BatteryState.h - /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/JoyFeedback.h - /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/JoyFeedbackArray.h - /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointCloud2.h - /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointField.h - /home/duongtd/robotics_core/sensor_msgs/test/main.cpp - /home/duongtd/robotics_core/std_msgs/include/std_msgs/Header.h + /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/BatteryState.h + /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/JoyFeedback.h + /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/JoyFeedbackArray.h + /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/PointCloud2.h + /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/PointField.h + /home/duongtd/robotics_core/common_msgs/sensor_msgs/test/main.cpp + /home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.h diff --git a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.make b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.make index f9f4c96..5503ebb 100644 --- a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.make +++ b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.make @@ -1,11 +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/sensor_msgs/BatteryState.h -sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/JoyFeedback.h -sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/JoyFeedbackArray.h -sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointCloud2.h -sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/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/std_msgs/Header.h +sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/BatteryState.h +sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/JoyFeedback.h +sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/JoyFeedbackArray.h +sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/PointCloud2.h +sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/PointField.h +sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/common_msgs/sensor_msgs/test/main.cpp +sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.h diff --git a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/flags.make b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/flags.make index 9b4f26c..146080a 100644 --- a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/flags.make +++ b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/flags.make @@ -6,5 +6,5 @@ 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 +CXX_INCLUDES = -I/home/duongtd/robotics_core/common_msgs/sensor_msgs/include -I/home/duongtd/robotics_core/common_msgs/std_msgs/include diff --git a/build/sensor_msgs_build/cmake_install.cmake b/build/sensor_msgs_build/cmake_install.cmake index ad2952c..5ed7d22 100644 --- a/build/sensor_msgs_build/cmake_install.cmake +++ b/build/sensor_msgs_build/cmake_install.cmake @@ -1,4 +1,4 @@ -# Install script for directory: /home/duongtd/robotics_core/sensor_msgs +# Install script for directory: /home/duongtd/robotics_core/common_msgs/sensor_msgs # Set the install prefix if(NOT DEFINED CMAKE_INSTALL_PREFIX) diff --git a/build/std_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake b/build/std_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake index c377876..165d3d4 100644 --- a/build/std_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake +++ b/build/std_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake @@ -2,7 +2,7 @@ # 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_SOURCE "/home/duongtd/robotics_core/common_msgs/std_msgs") set(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/duongtd/robotics_core/costmap_2d/build") # Force unix paths in dependencies. diff --git a/build/std_msgs_build/cmake_install.cmake b/build/std_msgs_build/cmake_install.cmake index 8a5bd63..da51daa 100644 --- a/build/std_msgs_build/cmake_install.cmake +++ b/build/std_msgs_build/cmake_install.cmake @@ -1,4 +1,4 @@ -# Install script for directory: /home/duongtd/robotics_core/std_msgs +# Install script for directory: /home/duongtd/robotics_core/common_msgs/std_msgs # Set the install prefix if(NOT DEFINED CMAKE_INSTALL_PREFIX) diff --git a/include/costmap_2d/inflation_layer.h b/include/costmap_2d/inflation_layer.h index 4a24d30..a523f46 100644 --- a/include/costmap_2d/inflation_layer.h +++ b/include/costmap_2d/inflation_layer.h @@ -3,265 +3,164 @@ #define COSTMAP_2D_INFLATION_LAYER_H_ // #include -// #include -// #include +#include +#include // #include // #include -// #include +#include +#include -// namespace costmap_2d -// { -// /** -// * @class CellData -// * @brief Storage for cell information used during obstacle inflation -// */ -// class CellData -// { -// public: -// /** -// * @brief Constructor for a CellData objects -// * @param i The index of the cell in the cost map -// * @param x The x coordinate of the cell in the cost map -// * @param y The y coordinate of the cell in the cost map -// * @param sx The x coordinate of the closest obstacle cell in the costmap -// * @param sy The y coordinate of the closest obstacle cell in the costmap -// * @return -// */ -// CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) : -// index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy) -// { -// } -// unsigned int index_; -// unsigned int x_, y_; -// unsigned int src_x_, src_y_; -// }; -// class InflationLayer : public Layer -// { -// public: -// InflationLayer(); +namespace costmap_2d +{ +/** + * @class CellData + * @brief Storage for cell information used during obstacle inflation + */ +class CellData +{ +public: + /** + * @brief Constructor for a CellData objects + * @param i The index of the cell in the cost map + * @param x The x coordinate of the cell in the cost map + * @param y The y coordinate of the cell in the cost map + * @param sx The x coordinate of the closest obstacle cell in the costmap + * @param sy The y coordinate of the closest obstacle cell in the costmap + * @return + */ + CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) : + index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy) + { + } + unsigned int index_; + unsigned int x_, y_; + unsigned int src_x_, src_y_; +}; -// virtual ~InflationLayer() -// { -// deleteKernels(); -// if (dsrv_) -// delete dsrv_; -// if (seen_) -// delete[] seen_; -// } +class InflationLayer : public Layer +{ +public: + InflationLayer(); -// 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 bool isDiscretized() -// { -// return true; -// } -// virtual void matchSize(); + virtual ~InflationLayer() + { + deleteKernels(); + // if (dsrv_) + // delete dsrv_; + if (seen_) + delete[] seen_; + } -// virtual void reset() { onInitialize(); } + 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 bool isDiscretized() + { + return true; + } + virtual void matchSize(); -// /** @brief Given a distance, compute a cost. -// * @param distance The distance from an obstacle in cells -// * @return A cost value for the distance */ -// virtual inline unsigned char computeCost(double distance) const -// { -// unsigned char cost = 0; -// if (distance == 0) -// cost = LETHAL_OBSTACLE; -// else if (distance * resolution_ <= inscribed_radius_) -// cost = INSCRIBED_INFLATED_OBSTACLE; -// else -// { -// // make sure cost falls off by Euclidean distance -// double euclidean_distance = distance * resolution_; -// double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_)); -// cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor); -// } -// return cost; -// } + virtual void reset() { onInitialize(); } -// /** -// * @brief Change the values of the inflation radius parameters -// * @param inflation_radius The new inflation radius -// * @param cost_scaling_factor The new weight -// */ -// void setInflationParameters(double inflation_radius, double cost_scaling_factor); + /** @brief Given a distance, compute a cost. + * @param distance The distance from an obstacle in cells + * @return A cost value for the distance */ + virtual inline unsigned char computeCost(double distance) const + { + unsigned char cost = 0; + if (distance == 0) + cost = LETHAL_OBSTACLE; + else if (distance * resolution_ <= inscribed_radius_) + cost = INSCRIBED_INFLATED_OBSTACLE; + else + { + // make sure cost falls off by Euclidean distance + double euclidean_distance = distance * resolution_; + double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_)); + cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor); + } + return cost; + } -// protected: -// virtual void onFootprintChanged(); -// boost::recursive_mutex* inflation_access_; + /** + * @brief Change the values of the inflation radius parameters + * @param inflation_radius The new inflation radius + * @param cost_scaling_factor The new weight + */ + void setInflationParameters(double inflation_radius, double cost_scaling_factor); -// double resolution_; -// double inflation_radius_; -// double inscribed_radius_; -// double weight_; -// bool inflate_unknown_; +protected: + virtual void onFootprintChanged(); + boost::recursive_mutex* inflation_access_; -// private: -// /** -// * @brief Lookup pre-computed distances -// * @param mx The x coordinate of the current cell -// * @param my The y coordinate of the current cell -// * @param src_x The x coordinate of the source cell -// * @param src_y The y coordinate of the source cell -// * @return -// */ -// inline double distanceLookup(int mx, int my, int src_x, int src_y) -// { -// unsigned int dx = abs(mx - src_x); -// unsigned int dy = abs(my - src_y); -// return cached_distances_[dx][dy]; -// } + double resolution_; + double inflation_radius_; + double inscribed_radius_; + double weight_; + bool inflate_unknown_; -// /** -// * @brief Lookup pre-computed costs -// * @param mx The x coordinate of the current cell -// * @param my The y coordinate of the current cell -// * @param src_x The x coordinate of the source cell -// * @param src_y The y coordinate of the source cell -// * @return -// */ -// inline unsigned char costLookup(int mx, int my, int src_x, int src_y) -// { -// unsigned int dx = abs(mx - src_x); -// unsigned int dy = abs(my - src_y); -// return cached_costs_[dx][dy]; -// } +private: + /** + * @brief Lookup pre-computed distances + * @param mx The x coordinate of the current cell + * @param my The y coordinate of the current cell + * @param src_x The x coordinate of the source cell + * @param src_y The y coordinate of the source cell + * @return + */ + inline double distanceLookup(int mx, int my, int src_x, int src_y) + { + unsigned int dx = abs(mx - src_x); + unsigned int dy = abs(my - src_y); + return cached_distances_[dx][dy]; + } -// void computeCaches(); -// void deleteKernels(); -// void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid); + /** + * @brief Lookup pre-computed costs + * @param mx The x coordinate of the current cell + * @param my The y coordinate of the current cell + * @param src_x The x coordinate of the source cell + * @param src_y The y coordinate of the source cell + * @return + */ + inline unsigned char costLookup(int mx, int my, int src_x, int src_y) + { + unsigned int dx = abs(mx - src_x); + unsigned int dy = abs(my - src_y); + return cached_costs_[dx][dy]; + } -// unsigned int cellDistance(double world_dist) -// { -// return layered_costmap_->getCostmap()->cellDistance(world_dist); -// } + void computeCaches(); + void deleteKernels(); + void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid); -// inline void enqueue(unsigned int index, unsigned int mx, unsigned int my, -// unsigned int src_x, unsigned int src_y); + unsigned int cellDistance(double world_dist) + { + return layered_costmap_->getCostmap()->cellDistance(world_dist); + } -// unsigned int cell_inflation_radius_; -// unsigned int cached_cell_inflation_radius_; -// std::map > inflation_cells_; + inline void enqueue(unsigned int index, unsigned int mx, unsigned int my, + unsigned int src_x, unsigned int src_y); -// bool* seen_; -// int seen_size_; + unsigned int cell_inflation_radius_; + unsigned int cached_cell_inflation_radius_; + std::map > inflation_cells_; -// unsigned char** cached_costs_; -// double** cached_distances_; -// double last_min_x_, last_min_y_, last_max_x_, last_max_y_; + bool* seen_; + int seen_size_; + + unsigned char** cached_costs_; + double** cached_distances_; + double last_min_x_, last_min_y_, last_max_x_, last_max_y_; // dynamic_reconfigure::Server *dsrv_; // void reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level); -// bool need_reinflation_; ///< Indicates that the entire costmap should be reinflated next time around. -// }; + bool need_reinflation_; ///< Indicates that the entire costmap should be reinflated next time around. +}; -// } // namespace costmap_2d - -// #endif // COSTMAP_2D_INFLATION_LAYER_H_ - - - - - - - - - - - - - -// ///////////////////////////////////////// - - - - - - - - - -// #ifndef INFLATION_LAYER_H_ -// #define INFLATION_LAYER_H_ - -// #include "share.h" -// #include -// #include -// #include -// #include - -// /** -// * @brief Lưu thông tin 1 cell khi thực hiện inflation -// */ -// class CellData -// { -// public: -// CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) -// : index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy) {} - -// unsigned int index_; -// unsigned int x_, y_; -// unsigned int src_x_, src_y_; -// }; - -// /** -// * @brief Lớp thực hiện "inflation" cho costmap (mở rộng vùng vật cản) -// */ -// class InflationLayer : public Layer -// { -// public: -// InflationLayer(); -// virtual ~InflationLayer(); - -// 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(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); -// virtual bool isDiscretized() { return true; } -// virtual void matchSize(); -// virtual void reset() { onInitialize(); } - -// void setInflationParameters(double inflation_radius, double cost_scaling_factor); - -// protected: -// virtual void onFootprintChanged(); - -// boost::recursive_mutex inflation_access_; - -// double resolution_; -// double inflation_radius_; -// double inscribed_radius_; -// double weight_; -// bool inflate_unknown_; - -// private: -// inline double distanceLookup(int mx, int my, int src_x, int src_y); -// inline unsigned char costLookup(int mx, int my, int src_x, int src_y); -// inline unsigned char computeCost(double distance) const; - -// void computeCaches(); -// void deleteKernels(); -// void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid); -// inline void enqueue(unsigned int index, unsigned int mx, unsigned int my, -// unsigned int src_x, unsigned int src_y); - -// unsigned int cell_inflation_radius_; -// unsigned int cached_cell_inflation_radius_; -// std::map> inflation_cells_; - -// bool* seen_; -// int seen_size_; - -// unsigned char** cached_costs_; -// double** cached_distances_; -// double last_min_x_, last_min_y_, last_max_x_, last_max_y_; - -// bool need_reinflation_; -// }; - -#endif // INFLATION_LAYER_H_ +} // namespace costmap_2d +#endif // COSTMAP_2D_INFLATION_LAYER_H_ diff --git a/include/costmap_2d/layer.h b/include/costmap_2d/layer.h index abff2c8..ef7a289 100644 --- a/include/costmap_2d/layer.h +++ b/include/costmap_2d/layer.h @@ -40,7 +40,7 @@ #include #include #include -#include "tf2/buffer_core.h" +#include namespace costmap_2d { @@ -146,6 +146,10 @@ private: std::vector footprint_spec_; }; + +using PluginPtr = std::shared_ptr; + + } // namespace costmap_2d #endif // COSTMAP_2D_LAYER_H_ diff --git a/include/costmap_2d/obstacle_layer.h b/include/costmap_2d/obstacle_layer.h new file mode 100644 index 0000000..ea5a8db --- /dev/null +++ b/include/costmap_2d/obstacle_layer.h @@ -0,0 +1,177 @@ +/********************************************************************* + * + * 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 + * David V. Lu!! + *********************************************************************/ +#ifndef COSTMAP_2D_OBSTACLE_LAYER_H_ +#define COSTMAP_2D_OBSTACLE_LAYER_H_ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace costmap_2d +{ + +class ObstacleLayer : public CostmapLayer +{ +public: + ObstacleLayer() + { + costmap_ = NULL; // this is the unsigned char* member of parent class Costmap2D. + } + + virtual ~ObstacleLayer(); + 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 activate(); + virtual void deactivate(); + virtual void reset(); + + /** + * @brief A callback to handle buffering LaserScan messages + * @param message The message returned from a message notifier + * @param buffer A pointer to the observation buffer to update + */ + void laserScanCallback(const sensor_msgs::LaserScanConstPtr& message, + const boost::shared_ptr& buffer); + + /** + * @brief A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max. + * @param message The message returned from a message notifier + * @param buffer A pointer to the observation buffer to update + */ + void laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& message, + const boost::shared_ptr& buffer); + + /** + * @brief A callback to handle buffering PointCloud messages + * @param message The message returned from a message notifier + * @param buffer A pointer to the observation buffer to update + */ + void pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message, + const boost::shared_ptr& 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::PointCloud2ConstPtr& message, + const boost::shared_ptr& buffer); + + // for testing purposes + void addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing); + void clearStaticObservations(bool marking, bool clearing); + +protected: + virtual void setupDynamicReconfigure(ros::NodeHandle& nh); + + /** + * @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& 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& clearing_observations) const; + + /** + * @brief Clear freespace based on one observation + * @param clearing_observation The observation used to raytrace + * @param min_x + * @param min_y + * @param max_x + * @param max_y + */ + virtual void raytraceFreespace(const 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 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); + + 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 + + std::vector > observation_subscribers_; ///< @brief Used for the observation message filters + std::vector > observation_notifiers_; ///< @brief Used to make sure that transforms are available for each sensor + std::vector > observation_buffers_; ///< @brief Used to store observations from various sensors + std::vector > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles + std::vector > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles + + // Used only for testing purposes + std::vector static_clearing_observations_, static_marking_observations_; + + bool rolling_window_; + dynamic_reconfigure::Server *dsrv_; + + int combination_method_; + +private: + void reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level); +}; + +} // namespace costmap_2d + +#endif // COSTMAP_2D_OBSTACLE_LAYER_H_ diff --git a/plugins/inflation_layer.cpp b/plugins/inflation_layer.cpp index 538beee..5e74c62 100644 --- a/plugins/inflation_layer.cpp +++ b/plugins/inflation_layer.cpp @@ -1,37 +1,37 @@ -// #include -// #include -// #include -// #include -// #include +#include +#include +#include +#include +#include // PLUGINLIB_EXPORT_CLASS(costmap_2d::InflationLayer, costmap_2d::Layer) -// using costmap_2d::LETHAL_OBSTACLE; -// using costmap_2d::INSCRIBED_INFLATED_OBSTACLE; -// using costmap_2d::NO_INFORMATION; +using costmap_2d::LETHAL_OBSTACLE; +using costmap_2d::INSCRIBED_INFLATED_OBSTACLE; +using costmap_2d::NO_INFORMATION; -// namespace costmap_2d -// { +namespace costmap_2d +{ -// InflationLayer::InflationLayer() -// : resolution_(0) -// , inflation_radius_(0) -// , inscribed_radius_(0) -// , weight_(0) -// , inflate_unknown_(false) -// , cell_inflation_radius_(0) -// , cached_cell_inflation_radius_(0) +InflationLayer::InflationLayer() + : resolution_(0) + , inflation_radius_(0) + , inscribed_radius_(0) + , weight_(0) + , inflate_unknown_(false) + , cell_inflation_radius_(0) + , cached_cell_inflation_radius_(0) // , dsrv_(NULL) -// , seen_(NULL) -// , cached_costs_(NULL) -// , cached_distances_(NULL) -// , last_min_x_(-std::numeric_limits::max()) -// , last_min_y_(-std::numeric_limits::max()) -// , last_max_x_(std::numeric_limits::max()) -// , last_max_y_(std::numeric_limits::max()) -// { -// inflation_access_ = new boost::recursive_mutex(); -// } + , seen_(NULL) + , cached_costs_(NULL) + , cached_distances_(NULL) + , last_min_x_(-std::numeric_limits::max()) + , last_min_y_(-std::numeric_limits::max()) + , last_max_x_(std::numeric_limits::max()) + , last_max_y_(std::numeric_limits::max()) +{ + inflation_access_ = new boost::recursive_mutex(); +} // void InflationLayer::onInitialize() // { @@ -344,4 +344,12 @@ // } // } -// } // namespace costmap_2d +// Export factory function +static PluginPtr create_inflation_plugin() { + return std::make_shared(); +} + +// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) +BOOST_DLL_ALIAS(create_inflation_plugin, create_plugin) + +} // namespace costmap_2d