diff --git a/CMakeLists.txt b/CMakeLists.txt index 2b76125..4a6932b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,15 @@ if (NOT TARGET geometry_msgs) add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/geometry_msgs ${CMAKE_BINARY_DIR}/geometry_msgs_build) endif() +if (NOT TARGET nav_msgs) + add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/nav_msgs ${CMAKE_BINARY_DIR}/nav_msgs_build) +endif() + +if (NOT TARGET map_msgs) + add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../map_msgs ${CMAKE_BINARY_DIR}/map_msgs_build) +endif() + + # ---- Dependencies ---- find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED COMPONENTS system thread) @@ -41,13 +50,16 @@ target_link_libraries(costmap_2d ${Boost_LIBRARIES} sensor_msgs geometry_msgs + nav_msgs + map_msgs + tf2 ) # # ---- Layer plugins ---- add_library(layers plugins/inflation_layer.cpp -# plugins/obstacle_layer.cpp -# plugins/static_layer.cpp + plugins/obstacle_layer.cpp + plugins/static_layer.cpp # plugins/voxel_layer.cpp ) diff --git a/build/CMakeCache.txt b/build/CMakeCache.txt index 5d86de7..41b1b3a 100644 --- a/build/CMakeCache.txt +++ b/build/CMakeCache.txt @@ -238,7 +238,7 @@ boost_thread_DIR:PATH=/usr/lib/x86_64-linux-gnu/cmake/boost_thread-1.71.0 costmap_2d_BINARY_DIR:STATIC=/home/duongtd/robotics_core/costmap_2d/build //Dependencies for the target -costmap_2d_LIB_DEPENDS:STATIC=general;Boost::system;general;Boost::thread; +costmap_2d_LIB_DEPENDS:STATIC=general;Boost::system;general;Boost::thread;general;tf2; //Value Computed by CMake costmap_2d_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/costmap_2d @@ -252,6 +252,18 @@ geometry_msgs_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/common_msgs/geometry //Dependencies for the target layers_LIB_DEPENDS:STATIC=general;costmap_2d;general;Boost::system;general;Boost::thread; +//Value Computed by CMake +map_msgs_BINARY_DIR:STATIC=/home/duongtd/robotics_core/costmap_2d/build/map_msgs_build + +//Value Computed by CMake +map_msgs_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/map_msgs + +//Value Computed by CMake +nav_msgs_BINARY_DIR:STATIC=/home/duongtd/robotics_core/costmap_2d/build/nav_msgs_build + +//Value Computed by CMake +nav_msgs_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/common_msgs/nav_msgs + //Value Computed by CMake sensor_msgs_BINARY_DIR:STATIC=/home/duongtd/robotics_core/costmap_2d/build/sensor_msgs_build @@ -379,7 +391,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=4 +CMAKE_NUMBER_OF_MAKEFILES:INTERNAL=6 //ADVANCED property for variable: CMAKE_OBJCOPY CMAKE_OBJCOPY-ADVANCED:INTERNAL=1 //ADVANCED property for variable: CMAKE_OBJDUMP diff --git a/build/CMakeFiles/CMakeError.log b/build/CMakeFiles/CMakeError.log index 4cc9111..7503d47 100644 --- a/build/CMakeFiles/CMakeError.log +++ b/build/CMakeFiles/CMakeError.log @@ -1,21 +1,21 @@ Performing C SOURCE FILE Test CMAKE_HAVE_LIBC_PTHREAD failed with the following output: Change Dir: /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp -Run Build Command(s):/usr/bin/make cmTC_c018a/fast && /usr/bin/make -f CMakeFiles/cmTC_c018a.dir/build.make CMakeFiles/cmTC_c018a.dir/build +Run Build Command(s):/usr/bin/make cmTC_6fa4e/fast && /usr/bin/make -f CMakeFiles/cmTC_6fa4e.dir/build.make CMakeFiles/cmTC_6fa4e.dir/build make[1]: Entering directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp' -Building C object CMakeFiles/cmTC_c018a.dir/src.c.o -/usr/bin/cc -DCMAKE_HAVE_LIBC_PTHREAD -fPIE -o CMakeFiles/cmTC_c018a.dir/src.c.o -c /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp/src.c -Linking C executable cmTC_c018a -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_c018a.dir/link.txt --verbose=1 -/usr/bin/cc -DCMAKE_HAVE_LIBC_PTHREAD CMakeFiles/cmTC_c018a.dir/src.c.o -o cmTC_c018a -/usr/bin/ld: CMakeFiles/cmTC_c018a.dir/src.c.o: in function `main': +Building C object CMakeFiles/cmTC_6fa4e.dir/src.c.o +/usr/bin/cc -DCMAKE_HAVE_LIBC_PTHREAD -fPIE -o CMakeFiles/cmTC_6fa4e.dir/src.c.o -c /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp/src.c +Linking C executable cmTC_6fa4e +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_6fa4e.dir/link.txt --verbose=1 +/usr/bin/cc -DCMAKE_HAVE_LIBC_PTHREAD CMakeFiles/cmTC_6fa4e.dir/src.c.o -o cmTC_6fa4e +/usr/bin/ld: CMakeFiles/cmTC_6fa4e.dir/src.c.o: in function `main': src.c:(.text+0x46): undefined reference to `pthread_create' /usr/bin/ld: src.c:(.text+0x52): undefined reference to `pthread_detach' /usr/bin/ld: src.c:(.text+0x63): undefined reference to `pthread_join' collect2: error: ld returned 1 exit status -make[1]: *** [CMakeFiles/cmTC_c018a.dir/build.make:87: cmTC_c018a] Error 1 +make[1]: *** [CMakeFiles/cmTC_6fa4e.dir/build.make:87: cmTC_6fa4e] Error 1 make[1]: Leaving directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp' -make: *** [Makefile:121: cmTC_c018a/fast] Error 2 +make: *** [Makefile:121: cmTC_6fa4e/fast] Error 2 Source file was: @@ -41,18 +41,18 @@ int main(void) Determining if the function pthread_create exists in the pthreads failed with the following output: Change Dir: /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp -Run Build Command(s):/usr/bin/make cmTC_08e7c/fast && /usr/bin/make -f CMakeFiles/cmTC_08e7c.dir/build.make CMakeFiles/cmTC_08e7c.dir/build +Run Build Command(s):/usr/bin/make cmTC_74663/fast && /usr/bin/make -f CMakeFiles/cmTC_74663.dir/build.make CMakeFiles/cmTC_74663.dir/build make[1]: Entering directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp' -Building C object CMakeFiles/cmTC_08e7c.dir/CheckFunctionExists.c.o -/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create -fPIE -o CMakeFiles/cmTC_08e7c.dir/CheckFunctionExists.c.o -c /usr/share/cmake-3.16/Modules/CheckFunctionExists.c -Linking C executable cmTC_08e7c -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_08e7c.dir/link.txt --verbose=1 -/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create CMakeFiles/cmTC_08e7c.dir/CheckFunctionExists.c.o -o cmTC_08e7c -lpthreads +Building C object CMakeFiles/cmTC_74663.dir/CheckFunctionExists.c.o +/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create -fPIE -o CMakeFiles/cmTC_74663.dir/CheckFunctionExists.c.o -c /usr/share/cmake-3.16/Modules/CheckFunctionExists.c +Linking C executable cmTC_74663 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_74663.dir/link.txt --verbose=1 +/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create CMakeFiles/cmTC_74663.dir/CheckFunctionExists.c.o -o cmTC_74663 -lpthreads /usr/bin/ld: cannot find -lpthreads collect2: error: ld returned 1 exit status -make[1]: *** [CMakeFiles/cmTC_08e7c.dir/build.make:87: cmTC_08e7c] Error 1 +make[1]: *** [CMakeFiles/cmTC_74663.dir/build.make:87: cmTC_74663] Error 1 make[1]: Leaving directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp' -make: *** [Makefile:121: cmTC_08e7c/fast] Error 2 +make: *** [Makefile:121: cmTC_74663/fast] Error 2 diff --git a/build/CMakeFiles/CMakeOutput.log b/build/CMakeFiles/CMakeOutput.log index efeec88..70bf2da 100644 --- a/build/CMakeFiles/CMakeOutput.log +++ b/build/CMakeFiles/CMakeOutput.log @@ -28,13 +28,13 @@ The CXX compiler identification is GNU, found in "/home/duongtd/robotics_core/co Determining if the C compiler works passed with the following output: Change Dir: /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp -Run Build Command(s):/usr/bin/make cmTC_049bd/fast && /usr/bin/make -f CMakeFiles/cmTC_049bd.dir/build.make CMakeFiles/cmTC_049bd.dir/build +Run Build Command(s):/usr/bin/make cmTC_2edfc/fast && /usr/bin/make -f CMakeFiles/cmTC_2edfc.dir/build.make CMakeFiles/cmTC_2edfc.dir/build make[1]: Entering directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp' -Building C object CMakeFiles/cmTC_049bd.dir/testCCompiler.c.o -/usr/bin/cc -o CMakeFiles/cmTC_049bd.dir/testCCompiler.c.o -c /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp/testCCompiler.c -Linking C executable cmTC_049bd -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_049bd.dir/link.txt --verbose=1 -/usr/bin/cc CMakeFiles/cmTC_049bd.dir/testCCompiler.c.o -o cmTC_049bd +Building C object CMakeFiles/cmTC_2edfc.dir/testCCompiler.c.o +/usr/bin/cc -o CMakeFiles/cmTC_2edfc.dir/testCCompiler.c.o -c /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp/testCCompiler.c +Linking C executable cmTC_2edfc +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_2edfc.dir/link.txt --verbose=1 +/usr/bin/cc CMakeFiles/cmTC_2edfc.dir/testCCompiler.c.o -o cmTC_2edfc make[1]: Leaving directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp' @@ -42,10 +42,10 @@ make[1]: Leaving directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFi Detecting C compiler ABI info compiled with the following output: Change Dir: /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp -Run Build Command(s):/usr/bin/make cmTC_6fd48/fast && /usr/bin/make -f CMakeFiles/cmTC_6fd48.dir/build.make CMakeFiles/cmTC_6fd48.dir/build +Run Build Command(s):/usr/bin/make cmTC_ae21c/fast && /usr/bin/make -f CMakeFiles/cmTC_ae21c.dir/build.make CMakeFiles/cmTC_ae21c.dir/build make[1]: Entering directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp' -Building C object CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o -/usr/bin/cc -v -o CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o -c /usr/share/cmake-3.16/Modules/CMakeCCompilerABI.c +Building C object CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o +/usr/bin/cc -v -o CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o -c /usr/share/cmake-3.16/Modules/CMakeCCompilerABI.c Using built-in specs. COLLECT_GCC=/usr/bin/cc OFFLOAD_TARGET_NAMES=nvptx-none:hsa @@ -54,8 +54,8 @@ Target: x86_64-linux-gnu Configured with: ../src/configure -v --with-pkgversion='Ubuntu 9.4.0-1ubuntu1~20.04.2' --with-bugurl=file:///usr/share/doc/gcc-9/README.Bugs --enable-languages=c,ada,c++,go,brig,d,fortran,objc,obj-c++,gm2 --prefix=/usr --with-gcc-major-version-only --program-suffix=-9 --program-prefix=x86_64-linux-gnu- --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --libdir=/usr/lib --enable-nls --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --with-default-libstdcxx-abi=new --enable-gnu-unique-object --disable-vtable-verify --enable-plugin --enable-default-pie --with-system-zlib --with-target-system-zlib=auto --enable-objc-gc=auto --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --enable-multilib --with-tune=generic --enable-offload-targets=nvptx-none=/build/gcc-9-9QDOt0/gcc-9-9.4.0/debian/tmp-nvptx/usr,hsa --without-cuda-driver --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu Thread model: posix gcc version 9.4.0 (Ubuntu 9.4.0-1ubuntu1~20.04.2) -COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64' - /usr/lib/gcc/x86_64-linux-gnu/9/cc1 -quiet -v -imultiarch x86_64-linux-gnu /usr/share/cmake-3.16/Modules/CMakeCCompilerABI.c -quiet -dumpbase CMakeCCompilerABI.c -mtune=generic -march=x86-64 -auxbase-strip CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o -version -fasynchronous-unwind-tables -fstack-protector-strong -Wformat -Wformat-security -fstack-clash-protection -fcf-protection -o /tmp/cc3c2RcL.s +COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64' + /usr/lib/gcc/x86_64-linux-gnu/9/cc1 -quiet -v -imultiarch x86_64-linux-gnu /usr/share/cmake-3.16/Modules/CMakeCCompilerABI.c -quiet -dumpbase CMakeCCompilerABI.c -mtune=generic -march=x86-64 -auxbase-strip CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o -version -fasynchronous-unwind-tables -fstack-protector-strong -Wformat -Wformat-security -fstack-clash-protection -fcf-protection -o /tmp/cc8d1aOs.s GNU C17 (Ubuntu 9.4.0-1ubuntu1~20.04.2) version 9.4.0 (x86_64-linux-gnu) compiled by GNU C version 9.4.0, GMP version 6.2.0, MPFR version 4.0.2, MPC version 1.1.0, isl version isl-0.22.1-GMP @@ -75,15 +75,15 @@ GNU C17 (Ubuntu 9.4.0-1ubuntu1~20.04.2) version 9.4.0 (x86_64-linux-gnu) GGC heuristics: --param ggc-min-expand=100 --param ggc-min-heapsize=131072 Compiler executable checksum: 01da938ff5dc2163489aa33cb3b747a7 -COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64' - as -v --64 -o CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o /tmp/cc3c2RcL.s +COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64' + as -v --64 -o CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o /tmp/cc8d1aOs.s GNU assembler version 2.34 (x86_64-linux-gnu) using BFD version (GNU Binutils for Ubuntu) 2.34 COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/ LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../:/lib/:/usr/lib/ -COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64' -Linking C executable cmTC_6fd48 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_6fd48.dir/link.txt --verbose=1 -/usr/bin/cc -v CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o -o cmTC_6fd48 +COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64' +Linking C executable cmTC_ae21c +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_ae21c.dir/link.txt --verbose=1 +/usr/bin/cc -v CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o -o cmTC_ae21c Using built-in specs. COLLECT_GCC=/usr/bin/cc COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper @@ -95,9 +95,9 @@ Thread model: posix gcc version 9.4.0 (Ubuntu 9.4.0-1ubuntu1~20.04.2) COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/ LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../:/lib/:/usr/lib/ -COLLECT_GCC_OPTIONS='-v' '-o' 'cmTC_6fd48' '-mtune=generic' '-march=x86-64' - /usr/lib/gcc/x86_64-linux-gnu/9/collect2 -plugin /usr/lib/gcc/x86_64-linux-gnu/9/liblto_plugin.so -plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper -plugin-opt=-fresolution=/tmp/cconKWCg.res -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lc -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lgcc_s --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -dynamic-linker /lib64/ld-linux-x86-64.so.2 -pie -z now -z relro -o cmTC_6fd48 /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o -L/usr/lib/gcc/x86_64-linux-gnu/9 -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/9/../../.. CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o -lgcc --push-state --as-needed -lgcc_s --pop-state -lc -lgcc --push-state --as-needed -lgcc_s --pop-state /usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o -COLLECT_GCC_OPTIONS='-v' '-o' 'cmTC_6fd48' '-mtune=generic' '-march=x86-64' +COLLECT_GCC_OPTIONS='-v' '-o' 'cmTC_ae21c' '-mtune=generic' '-march=x86-64' + /usr/lib/gcc/x86_64-linux-gnu/9/collect2 -plugin /usr/lib/gcc/x86_64-linux-gnu/9/liblto_plugin.so -plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper -plugin-opt=-fresolution=/tmp/ccRAp0BV.res -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lc -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lgcc_s --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -dynamic-linker /lib64/ld-linux-x86-64.so.2 -pie -z now -z relro -o cmTC_ae21c /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o -L/usr/lib/gcc/x86_64-linux-gnu/9 -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/9/../../.. CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o -lgcc --push-state --as-needed -lgcc_s --pop-state -lc -lgcc --push-state --as-needed -lgcc_s --pop-state /usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o +COLLECT_GCC_OPTIONS='-v' '-o' 'cmTC_ae21c' '-mtune=generic' '-march=x86-64' make[1]: Leaving directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp' @@ -121,10 +121,10 @@ Parsed C implicit link information from above output: link line regex: [^( *|.*[/\])(ld|CMAKE_LINK_STARTFILE-NOTFOUND|([^/\]+-)?ld|collect2)[^/\]*( |$)] ignore line: [Change Dir: /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp] ignore line: [] - ignore line: [Run Build Command(s):/usr/bin/make cmTC_6fd48/fast && /usr/bin/make -f CMakeFiles/cmTC_6fd48.dir/build.make CMakeFiles/cmTC_6fd48.dir/build] + ignore line: [Run Build Command(s):/usr/bin/make cmTC_ae21c/fast && /usr/bin/make -f CMakeFiles/cmTC_ae21c.dir/build.make CMakeFiles/cmTC_ae21c.dir/build] ignore line: [make[1]: Entering directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp'] - ignore line: [Building C object CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o] - ignore line: [/usr/bin/cc -v -o CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o -c /usr/share/cmake-3.16/Modules/CMakeCCompilerABI.c] + ignore line: [Building C object CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o] + ignore line: [/usr/bin/cc -v -o CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o -c /usr/share/cmake-3.16/Modules/CMakeCCompilerABI.c] ignore line: [Using built-in specs.] ignore line: [COLLECT_GCC=/usr/bin/cc] ignore line: [OFFLOAD_TARGET_NAMES=nvptx-none:hsa] @@ -133,8 +133,8 @@ Parsed C implicit link information from above output: ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 9.4.0-1ubuntu1~20.04.2' --with-bugurl=file:///usr/share/doc/gcc-9/README.Bugs --enable-languages=c ada c++ go brig d fortran objc obj-c++ gm2 --prefix=/usr --with-gcc-major-version-only --program-suffix=-9 --program-prefix=x86_64-linux-gnu- --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --libdir=/usr/lib --enable-nls --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --with-default-libstdcxx-abi=new --enable-gnu-unique-object --disable-vtable-verify --enable-plugin --enable-default-pie --with-system-zlib --with-target-system-zlib=auto --enable-objc-gc=auto --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32 m64 mx32 --enable-multilib --with-tune=generic --enable-offload-targets=nvptx-none=/build/gcc-9-9QDOt0/gcc-9-9.4.0/debian/tmp-nvptx/usr hsa --without-cuda-driver --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu] ignore line: [Thread model: posix] ignore line: [gcc version 9.4.0 (Ubuntu 9.4.0-1ubuntu1~20.04.2) ] - ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64'] - ignore line: [ /usr/lib/gcc/x86_64-linux-gnu/9/cc1 -quiet -v -imultiarch x86_64-linux-gnu /usr/share/cmake-3.16/Modules/CMakeCCompilerABI.c -quiet -dumpbase CMakeCCompilerABI.c -mtune=generic -march=x86-64 -auxbase-strip CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o -version -fasynchronous-unwind-tables -fstack-protector-strong -Wformat -Wformat-security -fstack-clash-protection -fcf-protection -o /tmp/cc3c2RcL.s] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64'] + ignore line: [ /usr/lib/gcc/x86_64-linux-gnu/9/cc1 -quiet -v -imultiarch x86_64-linux-gnu /usr/share/cmake-3.16/Modules/CMakeCCompilerABI.c -quiet -dumpbase CMakeCCompilerABI.c -mtune=generic -march=x86-64 -auxbase-strip CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o -version -fasynchronous-unwind-tables -fstack-protector-strong -Wformat -Wformat-security -fstack-clash-protection -fcf-protection -o /tmp/cc8d1aOs.s] ignore line: [GNU C17 (Ubuntu 9.4.0-1ubuntu1~20.04.2) version 9.4.0 (x86_64-linux-gnu)] ignore line: [ compiled by GNU C version 9.4.0 GMP version 6.2.0 MPFR version 4.0.2 MPC version 1.1.0 isl version isl-0.22.1-GMP] ignore line: [] @@ -154,15 +154,15 @@ Parsed C implicit link information from above output: ignore line: [] ignore line: [GGC heuristics: --param ggc-min-expand=100 --param ggc-min-heapsize=131072] ignore line: [Compiler executable checksum: 01da938ff5dc2163489aa33cb3b747a7] - ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64'] - ignore line: [ as -v --64 -o CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o /tmp/cc3c2RcL.s] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64'] + ignore line: [ as -v --64 -o CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o /tmp/cc8d1aOs.s] ignore line: [GNU assembler version 2.34 (x86_64-linux-gnu) using BFD version (GNU Binutils for Ubuntu) 2.34] ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/] ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../:/lib/:/usr/lib/] - ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64'] - ignore line: [Linking C executable cmTC_6fd48] - ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_6fd48.dir/link.txt --verbose=1] - ignore line: [/usr/bin/cc -v CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o -o cmTC_6fd48 ] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o' '-c' '-mtune=generic' '-march=x86-64'] + ignore line: [Linking C executable cmTC_ae21c] + ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_ae21c.dir/link.txt --verbose=1] + ignore line: [/usr/bin/cc -v CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o -o cmTC_ae21c ] ignore line: [Using built-in specs.] ignore line: [COLLECT_GCC=/usr/bin/cc] ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper] @@ -174,13 +174,13 @@ Parsed C implicit link information from above output: ignore line: [gcc version 9.4.0 (Ubuntu 9.4.0-1ubuntu1~20.04.2) ] ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/] ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../:/lib/:/usr/lib/] - ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'cmTC_6fd48' '-mtune=generic' '-march=x86-64'] - link line: [ /usr/lib/gcc/x86_64-linux-gnu/9/collect2 -plugin /usr/lib/gcc/x86_64-linux-gnu/9/liblto_plugin.so -plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper -plugin-opt=-fresolution=/tmp/cconKWCg.res -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lc -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lgcc_s --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -dynamic-linker /lib64/ld-linux-x86-64.so.2 -pie -z now -z relro -o cmTC_6fd48 /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o -L/usr/lib/gcc/x86_64-linux-gnu/9 -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/9/../../.. CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o -lgcc --push-state --as-needed -lgcc_s --pop-state -lc -lgcc --push-state --as-needed -lgcc_s --pop-state /usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'cmTC_ae21c' '-mtune=generic' '-march=x86-64'] + link line: [ /usr/lib/gcc/x86_64-linux-gnu/9/collect2 -plugin /usr/lib/gcc/x86_64-linux-gnu/9/liblto_plugin.so -plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper -plugin-opt=-fresolution=/tmp/ccRAp0BV.res -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lc -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lgcc_s --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -dynamic-linker /lib64/ld-linux-x86-64.so.2 -pie -z now -z relro -o cmTC_ae21c /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o -L/usr/lib/gcc/x86_64-linux-gnu/9 -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/9/../../.. CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o -lgcc --push-state --as-needed -lgcc_s --pop-state -lc -lgcc --push-state --as-needed -lgcc_s --pop-state /usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o] arg [/usr/lib/gcc/x86_64-linux-gnu/9/collect2] ==> ignore arg [-plugin] ==> ignore arg [/usr/lib/gcc/x86_64-linux-gnu/9/liblto_plugin.so] ==> ignore arg [-plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper] ==> ignore - arg [-plugin-opt=-fresolution=/tmp/cconKWCg.res] ==> ignore + arg [-plugin-opt=-fresolution=/tmp/ccRAp0BV.res] ==> ignore arg [-plugin-opt=-pass-through=-lgcc] ==> ignore arg [-plugin-opt=-pass-through=-lgcc_s] ==> ignore arg [-plugin-opt=-pass-through=-lc] ==> ignore @@ -198,7 +198,7 @@ Parsed C implicit link information from above output: arg [-znow] ==> ignore arg [-zrelro] ==> ignore arg [-o] ==> ignore - arg [cmTC_6fd48] ==> ignore + arg [cmTC_ae21c] ==> ignore arg [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o] ==> ignore arg [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o] ==> ignore arg [/usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o] ==> ignore @@ -210,7 +210,7 @@ Parsed C implicit link information from above output: arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu] arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib] arg [-L/usr/lib/gcc/x86_64-linux-gnu/9/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/9/../../..] - arg [CMakeFiles/cmTC_6fd48.dir/CMakeCCompilerABI.c.o] ==> ignore + arg [CMakeFiles/cmTC_ae21c.dir/CMakeCCompilerABI.c.o] ==> ignore arg [-lgcc] ==> lib [gcc] arg [--push-state] ==> ignore arg [--as-needed] ==> ignore @@ -240,13 +240,13 @@ Parsed C implicit link information from above output: Determining if the CXX compiler works passed with the following output: Change Dir: /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp -Run Build Command(s):/usr/bin/make cmTC_c9cbf/fast && /usr/bin/make -f CMakeFiles/cmTC_c9cbf.dir/build.make CMakeFiles/cmTC_c9cbf.dir/build +Run Build Command(s):/usr/bin/make cmTC_deb8a/fast && /usr/bin/make -f CMakeFiles/cmTC_deb8a.dir/build.make CMakeFiles/cmTC_deb8a.dir/build make[1]: Entering directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp' -Building CXX object CMakeFiles/cmTC_c9cbf.dir/testCXXCompiler.cxx.o -/usr/bin/c++ -o CMakeFiles/cmTC_c9cbf.dir/testCXXCompiler.cxx.o -c /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp/testCXXCompiler.cxx -Linking CXX executable cmTC_c9cbf -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_c9cbf.dir/link.txt --verbose=1 -/usr/bin/c++ CMakeFiles/cmTC_c9cbf.dir/testCXXCompiler.cxx.o -o cmTC_c9cbf +Building CXX object CMakeFiles/cmTC_deb8a.dir/testCXXCompiler.cxx.o +/usr/bin/c++ -o CMakeFiles/cmTC_deb8a.dir/testCXXCompiler.cxx.o -c /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp/testCXXCompiler.cxx +Linking CXX executable cmTC_deb8a +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_deb8a.dir/link.txt --verbose=1 +/usr/bin/c++ CMakeFiles/cmTC_deb8a.dir/testCXXCompiler.cxx.o -o cmTC_deb8a make[1]: Leaving directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp' @@ -254,10 +254,10 @@ make[1]: Leaving directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFi Detecting CXX compiler ABI info compiled with the following output: Change Dir: /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp -Run Build Command(s):/usr/bin/make cmTC_4e5e7/fast && /usr/bin/make -f CMakeFiles/cmTC_4e5e7.dir/build.make CMakeFiles/cmTC_4e5e7.dir/build +Run Build Command(s):/usr/bin/make cmTC_68632/fast && /usr/bin/make -f CMakeFiles/cmTC_68632.dir/build.make CMakeFiles/cmTC_68632.dir/build make[1]: Entering directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp' -Building CXX object CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o -/usr/bin/c++ -v -o CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-3.16/Modules/CMakeCXXCompilerABI.cpp +Building CXX object CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o +/usr/bin/c++ -v -o CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-3.16/Modules/CMakeCXXCompilerABI.cpp Using built-in specs. COLLECT_GCC=/usr/bin/c++ OFFLOAD_TARGET_NAMES=nvptx-none:hsa @@ -266,8 +266,8 @@ Target: x86_64-linux-gnu Configured with: ../src/configure -v --with-pkgversion='Ubuntu 9.4.0-1ubuntu1~20.04.2' --with-bugurl=file:///usr/share/doc/gcc-9/README.Bugs --enable-languages=c,ada,c++,go,brig,d,fortran,objc,obj-c++,gm2 --prefix=/usr --with-gcc-major-version-only --program-suffix=-9 --program-prefix=x86_64-linux-gnu- --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --libdir=/usr/lib --enable-nls --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --with-default-libstdcxx-abi=new --enable-gnu-unique-object --disable-vtable-verify --enable-plugin --enable-default-pie --with-system-zlib --with-target-system-zlib=auto --enable-objc-gc=auto --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --enable-multilib --with-tune=generic --enable-offload-targets=nvptx-none=/build/gcc-9-9QDOt0/gcc-9-9.4.0/debian/tmp-nvptx/usr,hsa --without-cuda-driver --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu Thread model: posix gcc version 9.4.0 (Ubuntu 9.4.0-1ubuntu1~20.04.2) -COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64' - /usr/lib/gcc/x86_64-linux-gnu/9/cc1plus -quiet -v -imultiarch x86_64-linux-gnu -D_GNU_SOURCE /usr/share/cmake-3.16/Modules/CMakeCXXCompilerABI.cpp -quiet -dumpbase CMakeCXXCompilerABI.cpp -mtune=generic -march=x86-64 -auxbase-strip CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o -version -fasynchronous-unwind-tables -fstack-protector-strong -Wformat -Wformat-security -fstack-clash-protection -fcf-protection -o /tmp/cc7HlAOK.s +COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64' + /usr/lib/gcc/x86_64-linux-gnu/9/cc1plus -quiet -v -imultiarch x86_64-linux-gnu -D_GNU_SOURCE /usr/share/cmake-3.16/Modules/CMakeCXXCompilerABI.cpp -quiet -dumpbase CMakeCXXCompilerABI.cpp -mtune=generic -march=x86-64 -auxbase-strip CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o -version -fasynchronous-unwind-tables -fstack-protector-strong -Wformat -Wformat-security -fstack-clash-protection -fcf-protection -o /tmp/ccLmzvLr.s GNU C++14 (Ubuntu 9.4.0-1ubuntu1~20.04.2) version 9.4.0 (x86_64-linux-gnu) compiled by GNU C version 9.4.0, GMP version 6.2.0, MPFR version 4.0.2, MPC version 1.1.0, isl version isl-0.22.1-GMP @@ -291,15 +291,15 @@ GNU C++14 (Ubuntu 9.4.0-1ubuntu1~20.04.2) version 9.4.0 (x86_64-linux-gnu) GGC heuristics: --param ggc-min-expand=100 --param ggc-min-heapsize=131072 Compiler executable checksum: 3d1eba838554fa2348dba760e4770469 -COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64' - as -v --64 -o CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o /tmp/cc7HlAOK.s +COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64' + as -v --64 -o CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o /tmp/ccLmzvLr.s GNU assembler version 2.34 (x86_64-linux-gnu) using BFD version (GNU Binutils for Ubuntu) 2.34 COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/ LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../:/lib/:/usr/lib/ -COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64' -Linking CXX executable cmTC_4e5e7 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_4e5e7.dir/link.txt --verbose=1 -/usr/bin/c++ -v CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o -o cmTC_4e5e7 +COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64' +Linking CXX executable cmTC_68632 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_68632.dir/link.txt --verbose=1 +/usr/bin/c++ -v CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o -o cmTC_68632 Using built-in specs. COLLECT_GCC=/usr/bin/c++ COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper @@ -311,9 +311,9 @@ Thread model: posix gcc version 9.4.0 (Ubuntu 9.4.0-1ubuntu1~20.04.2) COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/ LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../:/lib/:/usr/lib/ -COLLECT_GCC_OPTIONS='-v' '-o' 'cmTC_4e5e7' '-shared-libgcc' '-mtune=generic' '-march=x86-64' - /usr/lib/gcc/x86_64-linux-gnu/9/collect2 -plugin /usr/lib/gcc/x86_64-linux-gnu/9/liblto_plugin.so -plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper -plugin-opt=-fresolution=/tmp/cc83fSqd.res -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lc -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lgcc --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -dynamic-linker /lib64/ld-linux-x86-64.so.2 -pie -z now -z relro -o cmTC_4e5e7 /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o -L/usr/lib/gcc/x86_64-linux-gnu/9 -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/9/../../.. CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o -COLLECT_GCC_OPTIONS='-v' '-o' 'cmTC_4e5e7' '-shared-libgcc' '-mtune=generic' '-march=x86-64' +COLLECT_GCC_OPTIONS='-v' '-o' 'cmTC_68632' '-shared-libgcc' '-mtune=generic' '-march=x86-64' + /usr/lib/gcc/x86_64-linux-gnu/9/collect2 -plugin /usr/lib/gcc/x86_64-linux-gnu/9/liblto_plugin.so -plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper -plugin-opt=-fresolution=/tmp/cc23gVST.res -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lc -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lgcc --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -dynamic-linker /lib64/ld-linux-x86-64.so.2 -pie -z now -z relro -o cmTC_68632 /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o -L/usr/lib/gcc/x86_64-linux-gnu/9 -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/9/../../.. CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o +COLLECT_GCC_OPTIONS='-v' '-o' 'cmTC_68632' '-shared-libgcc' '-mtune=generic' '-march=x86-64' make[1]: Leaving directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp' @@ -343,10 +343,10 @@ Parsed CXX implicit link information from above output: link line regex: [^( *|.*[/\])(ld|CMAKE_LINK_STARTFILE-NOTFOUND|([^/\]+-)?ld|collect2)[^/\]*( |$)] ignore line: [Change Dir: /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp] ignore line: [] - ignore line: [Run Build Command(s):/usr/bin/make cmTC_4e5e7/fast && /usr/bin/make -f CMakeFiles/cmTC_4e5e7.dir/build.make CMakeFiles/cmTC_4e5e7.dir/build] + ignore line: [Run Build Command(s):/usr/bin/make cmTC_68632/fast && /usr/bin/make -f CMakeFiles/cmTC_68632.dir/build.make CMakeFiles/cmTC_68632.dir/build] ignore line: [make[1]: Entering directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp'] - ignore line: [Building CXX object CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o] - ignore line: [/usr/bin/c++ -v -o CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-3.16/Modules/CMakeCXXCompilerABI.cpp] + ignore line: [Building CXX object CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o] + ignore line: [/usr/bin/c++ -v -o CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-3.16/Modules/CMakeCXXCompilerABI.cpp] ignore line: [Using built-in specs.] ignore line: [COLLECT_GCC=/usr/bin/c++] ignore line: [OFFLOAD_TARGET_NAMES=nvptx-none:hsa] @@ -355,8 +355,8 @@ Parsed CXX implicit link information from above output: ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 9.4.0-1ubuntu1~20.04.2' --with-bugurl=file:///usr/share/doc/gcc-9/README.Bugs --enable-languages=c ada c++ go brig d fortran objc obj-c++ gm2 --prefix=/usr --with-gcc-major-version-only --program-suffix=-9 --program-prefix=x86_64-linux-gnu- --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --libdir=/usr/lib --enable-nls --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --with-default-libstdcxx-abi=new --enable-gnu-unique-object --disable-vtable-verify --enable-plugin --enable-default-pie --with-system-zlib --with-target-system-zlib=auto --enable-objc-gc=auto --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32 m64 mx32 --enable-multilib --with-tune=generic --enable-offload-targets=nvptx-none=/build/gcc-9-9QDOt0/gcc-9-9.4.0/debian/tmp-nvptx/usr hsa --without-cuda-driver --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu] ignore line: [Thread model: posix] ignore line: [gcc version 9.4.0 (Ubuntu 9.4.0-1ubuntu1~20.04.2) ] - ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64'] - ignore line: [ /usr/lib/gcc/x86_64-linux-gnu/9/cc1plus -quiet -v -imultiarch x86_64-linux-gnu -D_GNU_SOURCE /usr/share/cmake-3.16/Modules/CMakeCXXCompilerABI.cpp -quiet -dumpbase CMakeCXXCompilerABI.cpp -mtune=generic -march=x86-64 -auxbase-strip CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o -version -fasynchronous-unwind-tables -fstack-protector-strong -Wformat -Wformat-security -fstack-clash-protection -fcf-protection -o /tmp/cc7HlAOK.s] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64'] + ignore line: [ /usr/lib/gcc/x86_64-linux-gnu/9/cc1plus -quiet -v -imultiarch x86_64-linux-gnu -D_GNU_SOURCE /usr/share/cmake-3.16/Modules/CMakeCXXCompilerABI.cpp -quiet -dumpbase CMakeCXXCompilerABI.cpp -mtune=generic -march=x86-64 -auxbase-strip CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o -version -fasynchronous-unwind-tables -fstack-protector-strong -Wformat -Wformat-security -fstack-clash-protection -fcf-protection -o /tmp/ccLmzvLr.s] ignore line: [GNU C++14 (Ubuntu 9.4.0-1ubuntu1~20.04.2) version 9.4.0 (x86_64-linux-gnu)] ignore line: [ compiled by GNU C version 9.4.0 GMP version 6.2.0 MPFR version 4.0.2 MPC version 1.1.0 isl version isl-0.22.1-GMP] ignore line: [] @@ -380,15 +380,15 @@ Parsed CXX implicit link information from above output: ignore line: [] ignore line: [GGC heuristics: --param ggc-min-expand=100 --param ggc-min-heapsize=131072] ignore line: [Compiler executable checksum: 3d1eba838554fa2348dba760e4770469] - ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64'] - ignore line: [ as -v --64 -o CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o /tmp/cc7HlAOK.s] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64'] + ignore line: [ as -v --64 -o CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o /tmp/ccLmzvLr.s] ignore line: [GNU assembler version 2.34 (x86_64-linux-gnu) using BFD version (GNU Binutils for Ubuntu) 2.34] ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/] ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../:/lib/:/usr/lib/] - ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64'] - ignore line: [Linking CXX executable cmTC_4e5e7] - ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_4e5e7.dir/link.txt --verbose=1] - ignore line: [/usr/bin/c++ -v CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o -o cmTC_4e5e7 ] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o' '-c' '-shared-libgcc' '-mtune=generic' '-march=x86-64'] + ignore line: [Linking CXX executable cmTC_68632] + ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_68632.dir/link.txt --verbose=1] + ignore line: [/usr/bin/c++ -v CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o -o cmTC_68632 ] ignore line: [Using built-in specs.] ignore line: [COLLECT_GCC=/usr/bin/c++] ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper] @@ -400,13 +400,13 @@ Parsed CXX implicit link information from above output: ignore line: [gcc version 9.4.0 (Ubuntu 9.4.0-1ubuntu1~20.04.2) ] ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/] ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/9/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/9/../../../:/lib/:/usr/lib/] - ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'cmTC_4e5e7' '-shared-libgcc' '-mtune=generic' '-march=x86-64'] - link line: [ /usr/lib/gcc/x86_64-linux-gnu/9/collect2 -plugin /usr/lib/gcc/x86_64-linux-gnu/9/liblto_plugin.so -plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper -plugin-opt=-fresolution=/tmp/cc83fSqd.res -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lc -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lgcc --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -dynamic-linker /lib64/ld-linux-x86-64.so.2 -pie -z now -z relro -o cmTC_4e5e7 /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o -L/usr/lib/gcc/x86_64-linux-gnu/9 -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/9/../../.. CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o] + ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'cmTC_68632' '-shared-libgcc' '-mtune=generic' '-march=x86-64'] + link line: [ /usr/lib/gcc/x86_64-linux-gnu/9/collect2 -plugin /usr/lib/gcc/x86_64-linux-gnu/9/liblto_plugin.so -plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper -plugin-opt=-fresolution=/tmp/cc23gVST.res -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lc -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lgcc --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -dynamic-linker /lib64/ld-linux-x86-64.so.2 -pie -z now -z relro -o cmTC_68632 /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o -L/usr/lib/gcc/x86_64-linux-gnu/9 -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/9/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/9/../../.. CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/9/crtendS.o /usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crtn.o] arg [/usr/lib/gcc/x86_64-linux-gnu/9/collect2] ==> ignore arg [-plugin] ==> ignore arg [/usr/lib/gcc/x86_64-linux-gnu/9/liblto_plugin.so] ==> ignore arg [-plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/9/lto-wrapper] ==> ignore - arg [-plugin-opt=-fresolution=/tmp/cc83fSqd.res] ==> ignore + arg [-plugin-opt=-fresolution=/tmp/cc23gVST.res] ==> ignore arg [-plugin-opt=-pass-through=-lgcc_s] ==> ignore arg [-plugin-opt=-pass-through=-lgcc] ==> ignore arg [-plugin-opt=-pass-through=-lc] ==> ignore @@ -424,7 +424,7 @@ Parsed CXX implicit link information from above output: arg [-znow] ==> ignore arg [-zrelro] ==> ignore arg [-o] ==> ignore - arg [cmTC_4e5e7] ==> ignore + arg [cmTC_68632] ==> ignore arg [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/Scrt1.o] ==> ignore arg [/usr/lib/gcc/x86_64-linux-gnu/9/../../../x86_64-linux-gnu/crti.o] ==> ignore arg [/usr/lib/gcc/x86_64-linux-gnu/9/crtbeginS.o] ==> ignore @@ -436,7 +436,7 @@ Parsed CXX implicit link information from above output: arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu] arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib] arg [-L/usr/lib/gcc/x86_64-linux-gnu/9/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/9/../../..] - arg [CMakeFiles/cmTC_4e5e7.dir/CMakeCXXCompilerABI.cpp.o] ==> ignore + arg [CMakeFiles/cmTC_68632.dir/CMakeCXXCompilerABI.cpp.o] ==> ignore arg [-lstdc++] ==> lib [stdc++] arg [-lm] ==> lib [m] arg [-lgcc_s] ==> lib [gcc_s] @@ -462,13 +462,13 @@ Parsed CXX implicit link information from above output: Determining if the include file pthread.h exists passed with the following output: Change Dir: /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp -Run Build Command(s):/usr/bin/make cmTC_4fb05/fast && /usr/bin/make -f CMakeFiles/cmTC_4fb05.dir/build.make CMakeFiles/cmTC_4fb05.dir/build +Run Build Command(s):/usr/bin/make cmTC_bb3bb/fast && /usr/bin/make -f CMakeFiles/cmTC_bb3bb.dir/build.make CMakeFiles/cmTC_bb3bb.dir/build make[1]: Entering directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp' -Building C object CMakeFiles/cmTC_4fb05.dir/CheckIncludeFile.c.o -/usr/bin/cc -fPIE -o CMakeFiles/cmTC_4fb05.dir/CheckIncludeFile.c.o -c /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp/CheckIncludeFile.c -Linking C executable cmTC_4fb05 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_4fb05.dir/link.txt --verbose=1 -/usr/bin/cc CMakeFiles/cmTC_4fb05.dir/CheckIncludeFile.c.o -o cmTC_4fb05 +Building C object CMakeFiles/cmTC_bb3bb.dir/CheckIncludeFile.c.o +/usr/bin/cc -fPIE -o CMakeFiles/cmTC_bb3bb.dir/CheckIncludeFile.c.o -c /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp/CheckIncludeFile.c +Linking C executable cmTC_bb3bb +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_bb3bb.dir/link.txt --verbose=1 +/usr/bin/cc CMakeFiles/cmTC_bb3bb.dir/CheckIncludeFile.c.o -o cmTC_bb3bb make[1]: Leaving directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp' @@ -476,13 +476,13 @@ make[1]: Leaving directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFi Determining if the function pthread_create exists in the pthread passed with the following output: Change Dir: /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp -Run Build Command(s):/usr/bin/make cmTC_0fd95/fast && /usr/bin/make -f CMakeFiles/cmTC_0fd95.dir/build.make CMakeFiles/cmTC_0fd95.dir/build +Run Build Command(s):/usr/bin/make cmTC_16980/fast && /usr/bin/make -f CMakeFiles/cmTC_16980.dir/build.make CMakeFiles/cmTC_16980.dir/build make[1]: Entering directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp' -Building C object CMakeFiles/cmTC_0fd95.dir/CheckFunctionExists.c.o -/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create -fPIE -o CMakeFiles/cmTC_0fd95.dir/CheckFunctionExists.c.o -c /usr/share/cmake-3.16/Modules/CheckFunctionExists.c -Linking C executable cmTC_0fd95 -/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_0fd95.dir/link.txt --verbose=1 -/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create CMakeFiles/cmTC_0fd95.dir/CheckFunctionExists.c.o -o cmTC_0fd95 -lpthread +Building C object CMakeFiles/cmTC_16980.dir/CheckFunctionExists.c.o +/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create -fPIE -o CMakeFiles/cmTC_16980.dir/CheckFunctionExists.c.o -c /usr/share/cmake-3.16/Modules/CheckFunctionExists.c +Linking C executable cmTC_16980 +/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_16980.dir/link.txt --verbose=1 +/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create CMakeFiles/cmTC_16980.dir/CheckFunctionExists.c.o -o cmTC_16980 -lpthread make[1]: Leaving directory '/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/CMakeTmp' diff --git a/build/CMakeFiles/Makefile.cmake b/build/CMakeFiles/Makefile.cmake index 4e3e1bf..863c1b3 100644 --- a/build/CMakeFiles/Makefile.cmake +++ b/build/CMakeFiles/Makefile.cmake @@ -8,12 +8,14 @@ set(CMAKE_DEPENDS_GENERATOR "Unix Makefiles") set(CMAKE_MAKEFILE_DEPENDS "CMakeCache.txt" "/home/duongtd/robotics_core/common_msgs/geometry_msgs/CMakeLists.txt" + "/home/duongtd/robotics_core/common_msgs/nav_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/map_msgs/CMakeLists.txt" "/usr/lib/cmake/eigen3/Eigen3Config.cmake" "/usr/lib/cmake/eigen3/Eigen3ConfigVersion.cmake" "/usr/lib/cmake/eigen3/Eigen3Targets.cmake" @@ -34,28 +36,96 @@ set(CMAKE_MAKEFILE_DEPENDS "/usr/lib/x86_64-linux-gnu/cmake/boost_thread-1.71.0/boost_thread-config.cmake" "/usr/lib/x86_64-linux-gnu/cmake/boost_thread-1.71.0/libboost_thread-variant-shared.cmake" "/usr/lib/x86_64-linux-gnu/cmake/boost_thread-1.71.0/libboost_thread-variant-static.cmake" + "/usr/share/cmake-3.16/Modules/CMakeCCompiler.cmake.in" + "/usr/share/cmake-3.16/Modules/CMakeCCompilerABI.c" "/usr/share/cmake-3.16/Modules/CMakeCInformation.cmake" + "/usr/share/cmake-3.16/Modules/CMakeCXXCompiler.cmake.in" + "/usr/share/cmake-3.16/Modules/CMakeCXXCompilerABI.cpp" "/usr/share/cmake-3.16/Modules/CMakeCXXInformation.cmake" "/usr/share/cmake-3.16/Modules/CMakeCheckCompilerFlagCommonPatterns.cmake" "/usr/share/cmake-3.16/Modules/CMakeCommonLanguageInclude.cmake" + "/usr/share/cmake-3.16/Modules/CMakeCompilerIdDetection.cmake" + "/usr/share/cmake-3.16/Modules/CMakeDetermineCCompiler.cmake" + "/usr/share/cmake-3.16/Modules/CMakeDetermineCXXCompiler.cmake" + "/usr/share/cmake-3.16/Modules/CMakeDetermineCompileFeatures.cmake" + "/usr/share/cmake-3.16/Modules/CMakeDetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/CMakeDetermineCompilerABI.cmake" + "/usr/share/cmake-3.16/Modules/CMakeDetermineCompilerId.cmake" + "/usr/share/cmake-3.16/Modules/CMakeDetermineSystem.cmake" + "/usr/share/cmake-3.16/Modules/CMakeFindBinUtils.cmake" "/usr/share/cmake-3.16/Modules/CMakeFindDependencyMacro.cmake" "/usr/share/cmake-3.16/Modules/CMakeGenericSystem.cmake" "/usr/share/cmake-3.16/Modules/CMakeInitializeConfigs.cmake" "/usr/share/cmake-3.16/Modules/CMakeLanguageInformation.cmake" + "/usr/share/cmake-3.16/Modules/CMakeParseImplicitIncludeInfo.cmake" + "/usr/share/cmake-3.16/Modules/CMakeParseImplicitLinkInfo.cmake" + "/usr/share/cmake-3.16/Modules/CMakeSystem.cmake.in" "/usr/share/cmake-3.16/Modules/CMakeSystemSpecificInformation.cmake" "/usr/share/cmake-3.16/Modules/CMakeSystemSpecificInitialize.cmake" + "/usr/share/cmake-3.16/Modules/CMakeTestCCompiler.cmake" + "/usr/share/cmake-3.16/Modules/CMakeTestCXXCompiler.cmake" + "/usr/share/cmake-3.16/Modules/CMakeTestCompilerCommon.cmake" + "/usr/share/cmake-3.16/Modules/CMakeUnixFindMake.cmake" "/usr/share/cmake-3.16/Modules/CheckCSourceCompiles.cmake" + "/usr/share/cmake-3.16/Modules/CheckFunctionExists.c" + "/usr/share/cmake-3.16/Modules/CheckIncludeFile.c.in" "/usr/share/cmake-3.16/Modules/CheckIncludeFile.cmake" "/usr/share/cmake-3.16/Modules/CheckLibraryExists.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/ADSP-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/ARMCC-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/ARMClang-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/AppleClang-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/Borland-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/Bruce-C-DetermineCompiler.cmake" "/usr/share/cmake-3.16/Modules/Compiler/CMakeCommonCompilerMacros.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/Clang-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/Clang-DetermineCompilerInternal.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/Comeau-CXX-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/Compaq-C-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/Compaq-CXX-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/Cray-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/Embarcadero-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/Fujitsu-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/GHS-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/GNU-C-DetermineCompiler.cmake" "/usr/share/cmake-3.16/Modules/Compiler/GNU-C.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/GNU-CXX-DetermineCompiler.cmake" "/usr/share/cmake-3.16/Modules/Compiler/GNU-CXX.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/GNU-FindBinUtils.cmake" "/usr/share/cmake-3.16/Modules/Compiler/GNU.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/HP-C-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/HP-CXX-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/IAR-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/IBMCPP-C-DetermineVersionInternal.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/IBMCPP-CXX-DetermineVersionInternal.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/Intel-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/MSVC-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/NVIDIA-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/OpenWatcom-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/PGI-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/PathScale-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/SCO-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/SDCC-C-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/SunPro-C-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/SunPro-CXX-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/TI-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/TinyCC-C-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/VisualAge-C-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/VisualAge-CXX-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/Watcom-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/XL-C-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/XL-CXX-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/XLClang-C-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/XLClang-CXX-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/zOS-C-DetermineCompiler.cmake" + "/usr/share/cmake-3.16/Modules/Compiler/zOS-CXX-DetermineCompiler.cmake" "/usr/share/cmake-3.16/Modules/FindBoost.cmake" "/usr/share/cmake-3.16/Modules/FindPackageHandleStandardArgs.cmake" "/usr/share/cmake-3.16/Modules/FindPackageMessage.cmake" "/usr/share/cmake-3.16/Modules/FindThreads.cmake" "/usr/share/cmake-3.16/Modules/Internal/CMakeCheckCompilerFlag.cmake" + "/usr/share/cmake-3.16/Modules/Internal/FeatureTesting.cmake" + "/usr/share/cmake-3.16/Modules/Platform/Linux-Determine-CXX.cmake" "/usr/share/cmake-3.16/Modules/Platform/Linux-GNU-C.cmake" "/usr/share/cmake-3.16/Modules/Platform/Linux-GNU-CXX.cmake" "/usr/share/cmake-3.16/Modules/Platform/Linux-GNU.cmake" @@ -71,10 +141,17 @@ set(CMAKE_MAKEFILE_OUTPUTS # Byproducts of CMake generate step: set(CMAKE_MAKEFILE_PRODUCTS + "CMakeFiles/3.16.3/CMakeSystem.cmake" + "CMakeFiles/3.16.3/CMakeCCompiler.cmake" + "CMakeFiles/3.16.3/CMakeCXXCompiler.cmake" + "CMakeFiles/3.16.3/CMakeCCompiler.cmake" + "CMakeFiles/3.16.3/CMakeCXXCompiler.cmake" "CMakeFiles/CMakeDirectoryInformation.cmake" "sensor_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake" "std_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake" "geometry_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake" + "nav_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake" + "map_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake" ) # Dependency information for all targets: diff --git a/build/CMakeFiles/Makefile2 b/build/CMakeFiles/Makefile2 index 1c91ec5..69e91b2 100644 --- a/build/CMakeFiles/Makefile2 +++ b/build/CMakeFiles/Makefile2 @@ -57,12 +57,16 @@ all: CMakeFiles/layers.dir/all all: CMakeFiles/costmap_2d.dir/all all: sensor_msgs_build/all all: geometry_msgs_build/all +all: nav_msgs_build/all +all: map_msgs_build/all .PHONY : all # The main recursive "preinstall" target. preinstall: sensor_msgs_build/preinstall preinstall: geometry_msgs_build/preinstall +preinstall: nav_msgs_build/preinstall +preinstall: map_msgs_build/preinstall .PHONY : preinstall @@ -71,6 +75,8 @@ clean: CMakeFiles/layers.dir/clean clean: CMakeFiles/costmap_2d.dir/clean clean: sensor_msgs_build/clean clean: geometry_msgs_build/clean +clean: nav_msgs_build/clean +clean: map_msgs_build/clean .PHONY : clean @@ -92,6 +98,42 @@ geometry_msgs_build/clean: .PHONY : geometry_msgs_build/clean +#============================================================================= +# Directory level rules for directory map_msgs_build + +# Recursive "all" directory target. +map_msgs_build/all: + +.PHONY : map_msgs_build/all + +# Recursive "preinstall" directory target. +map_msgs_build/preinstall: + +.PHONY : map_msgs_build/preinstall + +# Recursive "clean" directory target. +map_msgs_build/clean: + +.PHONY : map_msgs_build/clean + +#============================================================================= +# Directory level rules for directory nav_msgs_build + +# Recursive "all" directory target. +nav_msgs_build/all: + +.PHONY : nav_msgs_build/all + +# Recursive "preinstall" directory target. +nav_msgs_build/preinstall: + +.PHONY : nav_msgs_build/preinstall + +# Recursive "clean" directory target. +nav_msgs_build/clean: + +.PHONY : nav_msgs_build/clean + #============================================================================= # Directory level rules for directory sensor_msgs_build @@ -137,12 +179,12 @@ std_msgs_build/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=10,11 "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,12,13 "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 11 + $(CMAKE_COMMAND) -E cmake_progress_start /home/duongtd/robotics_core/costmap_2d/build/CMakeFiles 13 $(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 @@ -191,7 +233,7 @@ CMakeFiles/costmap_2d.dir/clean: 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" + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --progress-dir=/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles --progress-num=14,15 "Built target test_battery_state" .PHONY : sensor_msgs_build/CMakeFiles/test_battery_state.dir/all # Build rule for subdir invocation for target. diff --git a/build/CMakeFiles/TargetDirectories.txt b/build/CMakeFiles/TargetDirectories.txt index cfe26a2..3b8c856 100644 --- a/build/CMakeFiles/TargetDirectories.txt +++ b/build/CMakeFiles/TargetDirectories.txt @@ -9,3 +9,7 @@ /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 +/home/duongtd/robotics_core/costmap_2d/build/nav_msgs_build/CMakeFiles/rebuild_cache.dir +/home/duongtd/robotics_core/costmap_2d/build/nav_msgs_build/CMakeFiles/edit_cache.dir +/home/duongtd/robotics_core/costmap_2d/build/map_msgs_build/CMakeFiles/rebuild_cache.dir +/home/duongtd/robotics_core/costmap_2d/build/map_msgs_build/CMakeFiles/edit_cache.dir diff --git a/build/CMakeFiles/costmap_2d.dir/CXX.includecache b/build/CMakeFiles/costmap_2d.dir/CXX.includecache index 30d8990..415e8c5 100644 --- a/build/CMakeFiles/costmap_2d.dir/CXX.includecache +++ b/build/CMakeFiles/costmap_2d.dir/CXX.includecache @@ -6,6 +6,12 @@ #IncludeRegexTransform: +../include/costmap_2d/array_parser.h +vector +- +string +- + ../include/costmap_2d/cost_values.h ../include/costmap_2d/costmap_2d.h @@ -24,6 +30,16 @@ costmap_2d/layer.h costmap_2d/layered_costmap.h - +../include/costmap_2d/costmap_math.h +math.h +- +algorithm +- +vector +- +geometry_msgs/Point.h +- + ../include/costmap_2d/footprint.h geometry_msgs/Point32.h - @@ -58,6 +74,34 @@ vector string - +../include/costmap_2d/observation.h +sensor_msgs/PointCloud2.h +- +geometry_msgs/Point.h +- + +../include/costmap_2d/observation_buffer.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 +- +boost/thread.hpp +- + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h cmath - @@ -66,6 +110,12 @@ 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/PointStamped.h +std_msgs/Header.h +/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/std_msgs/Header.h +geometry_msgs/Point.h +/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/geometry_msgs/Point.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Polygon.h vector - @@ -78,6 +128,48 @@ std_msgs/Header.h geometry_msgs/Polygon.h - +/home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/PointCloud2.h +cstdint +- +string +- +vector +- +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/common_msgs/sensor_msgs/include/sensor_msgs/sensor_msgs/PointField.h + +/home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/PointField.h +cstdint +- +string +- + +/home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h +sensor_msgs/PointCloud2.h +- +cstdarg +- +sstream +- +string +- +vector +- + +/home/duongtd/robotics_core/common_msgs/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/common_msgs/std_msgs/include/std_msgs/Header.h string - @@ -86,10 +178,44 @@ chrono cstdint - +/home/duongtd/robotics_core/costmap_2d/src/array_parser.cpp +cstdio +- +string +- +sstream +- +vector +- + +/home/duongtd/robotics_core/costmap_2d/src/costmap_2d.cpp +costmap_2d/costmap_2d.h +- +cstdio +- + /home/duongtd/robotics_core/costmap_2d/src/costmap_layer.cpp costmap_2d/costmap_layer.h - +/home/duongtd/robotics_core/costmap_2d/src/costmap_math.cpp +costmap_2d/costmap_math.h +- + +/home/duongtd/robotics_core/costmap_2d/src/footprint.cpp +costmap_2d/costmap_math.h +- +boost/tokenizer.hpp +- +boost/foreach.hpp +- +boost/algorithm/string.hpp +- +costmap_2d/footprint.h +- +costmap_2d/array_parser.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 @@ -108,3 +234,13 @@ algorithm vector - +/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 +- + diff --git a/build/CMakeFiles/costmap_2d.dir/DependInfo.cmake b/build/CMakeFiles/costmap_2d.dir/DependInfo.cmake index 79efda2..ccceada 100644 --- a/build/CMakeFiles/costmap_2d.dir/DependInfo.cmake +++ b/build/CMakeFiles/costmap_2d.dir/DependInfo.cmake @@ -30,6 +30,8 @@ set(CMAKE_CXX_TARGET_INCLUDE_PATH "/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" + "/home/duongtd/robotics_core/common_msgs/nav_msgs/include" + "/home/duongtd/robotics_core/map_msgs/include" ) # Targets to which this target links. diff --git a/build/CMakeFiles/costmap_2d.dir/flags.make b/build/CMakeFiles/costmap_2d.dir/flags.make index 6ac81f3..528ab90 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/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 +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 -I/home/duongtd/robotics_core/common_msgs/nav_msgs/include -I/home/duongtd/robotics_core/map_msgs/include diff --git a/build/CMakeFiles/costmap_2d.dir/src/layer.cpp.o b/build/CMakeFiles/costmap_2d.dir/src/layer.cpp.o index 4693ff1..ea2b69a 100644 Binary files a/build/CMakeFiles/costmap_2d.dir/src/layer.cpp.o and b/build/CMakeFiles/costmap_2d.dir/src/layer.cpp.o differ diff --git a/build/CMakeFiles/layers.dir/CXX.includecache b/build/CMakeFiles/layers.dir/CXX.includecache index ec08f10..288f7a8 100644 --- a/build/CMakeFiles/layers.dir/CXX.includecache +++ b/build/CMakeFiles/layers.dir/CXX.includecache @@ -18,6 +18,12 @@ boost/thread.hpp geometry_msgs/Point.h - +../include/costmap_2d/costmap_layer.h +costmap_2d/layer.h +- +costmap_2d/layered_costmap.h +- + ../include/costmap_2d/costmap_math.h math.h - @@ -28,28 +34,6 @@ 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 - @@ -72,24 +56,74 @@ vector string - +../include/costmap_2d/static_layer.h +costmap_2d/costmap_layer.h +- +costmap_2d/layered_costmap.h +- +nav_msgs/OccupancyGrid.h +- +map_msgs/OccupancyGridUpdate.h +- +string +- +geometry_msgs/TransformStamped.h +- + /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/Pose.h +cmath +- +iostream +- +geometry_msgs/Point.h +- +geometry_msgs/Quaternion.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/Quaternion.h + +/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Transform.h +geometry_msgs/Vector3.h +/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/geometry_msgs/Vector3.h +geometry_msgs/Quaternion.h +/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/geometry_msgs/Quaternion.h + +/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/TransformStamped.h +std_msgs/Header.h +/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/std_msgs/Header.h +geometry_msgs/Transform.h +/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/geometry_msgs/Transform.h +string +- + +/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Vector3.h + +/home/duongtd/robotics_core/common_msgs/nav_msgs/include/nav_msgs/MapMetaData.h +cstdint +- +string +- vector - -geometry_msgs/Point32.h -- +geometry_msgs/Pose.h +/home/duongtd/robotics_core/common_msgs/nav_msgs/include/nav_msgs/geometry_msgs/Pose.h -/home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/PolygonStamped.h -std_msgs/Header.h +/home/duongtd/robotics_core/common_msgs/nav_msgs/include/nav_msgs/OccupancyGrid.h +cstdint - -geometry_msgs/Polygon.h +string +- +vector +- +std_msgs/Header.h +/home/duongtd/robotics_core/common_msgs/nav_msgs/include/nav_msgs/std_msgs/Header.h +nav_msgs/MapMetaData.h - /home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.h @@ -100,15 +134,21 @@ chrono cstdint - -/home/duongtd/robotics_core/costmap_2d/plugins/inflation_layer.cpp -algorithm -- -costmap_2d/inflation_layer.h +/home/duongtd/robotics_core/costmap_2d/plugins/static_layer.cpp +costmap_2d/static_layer.h - costmap_2d/costmap_math.h - -costmap_2d/footprint.h +tf2/convert.h - -boost/thread.hpp +tf2/utils.h +- +boost/dll/alias.hpp - +/home/duongtd/robotics_core/map_msgs/include/map_msgs/OccupancyGridUpdate.h +vector +- +std_msgs/Header.h +/home/duongtd/robotics_core/map_msgs/include/map_msgs/std_msgs/Header.h + diff --git a/build/CMakeFiles/layers.dir/DependInfo.cmake b/build/CMakeFiles/layers.dir/DependInfo.cmake index 8e0f2b4..c1eff33 100644 --- a/build/CMakeFiles/layers.dir/DependInfo.cmake +++ b/build/CMakeFiles/layers.dir/DependInfo.cmake @@ -5,6 +5,8 @@ set(CMAKE_DEPENDS_LANGUAGES # The set of files for implicit dependencies of each language: set(CMAKE_DEPENDS_CHECK_CXX "/home/duongtd/robotics_core/costmap_2d/plugins/inflation_layer.cpp" "/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o" + "/home/duongtd/robotics_core/costmap_2d/plugins/obstacle_layer.cpp" "/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o" + "/home/duongtd/robotics_core/costmap_2d/plugins/static_layer.cpp" "/home/duongtd/robotics_core/costmap_2d/build/CMakeFiles/layers.dir/plugins/static_layer.cpp.o" ) set(CMAKE_CXX_COMPILER_ID "GNU") @@ -23,6 +25,8 @@ set(CMAKE_CXX_TARGET_INCLUDE_PATH "/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" + "/home/duongtd/robotics_core/common_msgs/nav_msgs/include" + "/home/duongtd/robotics_core/map_msgs/include" ) # Targets to which this target links. diff --git a/build/CMakeFiles/layers.dir/build.make b/build/CMakeFiles/layers.dir/build.make index 803d64f..8df3b80 100644 --- a/build/CMakeFiles/layers.dir/build.make +++ b/build/CMakeFiles/layers.dir/build.make @@ -70,17 +70,47 @@ CMakeFiles/layers.dir/plugins/inflation_layer.cpp.s: cmake_force @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/layers.dir/plugins/inflation_layer.cpp.s" /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/duongtd/robotics_core/costmap_2d/plugins/inflation_layer.cpp -o CMakeFiles/layers.dir/plugins/inflation_layer.cpp.s +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: CMakeFiles/layers.dir/flags.make +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: ../plugins/obstacle_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_2) "Building CXX object CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o" + /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -o CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o -c /home/duongtd/robotics_core/costmap_2d/plugins/obstacle_layer.cpp + +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.i: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.i" + /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /home/duongtd/robotics_core/costmap_2d/plugins/obstacle_layer.cpp > CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.i + +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.s: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.s" + /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/duongtd/robotics_core/costmap_2d/plugins/obstacle_layer.cpp -o CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.s + +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: CMakeFiles/layers.dir/flags.make +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: ../plugins/static_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_3) "Building CXX object CMakeFiles/layers.dir/plugins/static_layer.cpp.o" + /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -o CMakeFiles/layers.dir/plugins/static_layer.cpp.o -c /home/duongtd/robotics_core/costmap_2d/plugins/static_layer.cpp + +CMakeFiles/layers.dir/plugins/static_layer.cpp.i: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/layers.dir/plugins/static_layer.cpp.i" + /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /home/duongtd/robotics_core/costmap_2d/plugins/static_layer.cpp > CMakeFiles/layers.dir/plugins/static_layer.cpp.i + +CMakeFiles/layers.dir/plugins/static_layer.cpp.s: cmake_force + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/layers.dir/plugins/static_layer.cpp.s" + /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/duongtd/robotics_core/costmap_2d/plugins/static_layer.cpp -o CMakeFiles/layers.dir/plugins/static_layer.cpp.s + # Object files for target layers layers_OBJECTS = \ -"CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o" +"CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o" \ +"CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o" \ +"CMakeFiles/layers.dir/plugins/static_layer.cpp.o" # External object files for target layers layers_EXTERNAL_OBJECTS = liblayers.a: CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o +liblayers.a: CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o +liblayers.a: CMakeFiles/layers.dir/plugins/static_layer.cpp.o liblayers.a: CMakeFiles/layers.dir/build.make liblayers.a: CMakeFiles/layers.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 static library liblayers.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_4) "Linking CXX static library liblayers.a" $(CMAKE_COMMAND) -P CMakeFiles/layers.dir/cmake_clean_target.cmake $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/layers.dir/link.txt --verbose=$(VERBOSE) diff --git a/build/CMakeFiles/layers.dir/cmake_clean.cmake b/build/CMakeFiles/layers.dir/cmake_clean.cmake index 1089c36..2570365 100644 --- a/build/CMakeFiles/layers.dir/cmake_clean.cmake +++ b/build/CMakeFiles/layers.dir/cmake_clean.cmake @@ -1,5 +1,7 @@ file(REMOVE_RECURSE "CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o" + "CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o" + "CMakeFiles/layers.dir/plugins/static_layer.cpp.o" "liblayers.a" "liblayers.pdb" ) diff --git a/build/CMakeFiles/layers.dir/depend.internal b/build/CMakeFiles/layers.dir/depend.internal index d22ebaa..6e0ed21 100644 --- a/build/CMakeFiles/layers.dir/depend.internal +++ b/build/CMakeFiles/layers.dir/depend.internal @@ -15,3 +15,50 @@ CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o /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 +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o + ../include/costmap_2d/cost_values.h + ../include/costmap_2d/costmap_2d.h + ../include/costmap_2d/costmap_layer.h + ../include/costmap_2d/footprint.h + ../include/costmap_2d/layer.h + ../include/costmap_2d/layered_costmap.h + ../include/costmap_2d/observation.h + ../include/costmap_2d/observation_buffer.h + ../include/costmap_2d/obstacle_layer.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/PointStamped.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/geometry_msgs/include/geometry_msgs/Pose.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Quaternion.h + /home/duongtd/robotics_core/common_msgs/nav_msgs/include/nav_msgs/MapMetaData.h + /home/duongtd/robotics_core/common_msgs/nav_msgs/include/nav_msgs/OccupancyGrid.h + /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/ChannelFloat32.h + /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/LaserScan.h + /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/PointCloud.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/point_cloud_conversion.h + /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/point_field_conversion.h + /home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.h + /home/duongtd/robotics_core/costmap_2d/plugins/obstacle_layer.cpp +CMakeFiles/layers.dir/plugins/static_layer.cpp.o + ../include/costmap_2d/cost_values.h + ../include/costmap_2d/costmap_2d.h + ../include/costmap_2d/costmap_layer.h + ../include/costmap_2d/costmap_math.h + ../include/costmap_2d/layer.h + ../include/costmap_2d/layered_costmap.h + ../include/costmap_2d/static_layer.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/Pose.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Quaternion.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Transform.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/TransformStamped.h + /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Vector3.h + /home/duongtd/robotics_core/common_msgs/nav_msgs/include/nav_msgs/MapMetaData.h + /home/duongtd/robotics_core/common_msgs/nav_msgs/include/nav_msgs/OccupancyGrid.h + /home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.h + /home/duongtd/robotics_core/costmap_2d/plugins/static_layer.cpp + /home/duongtd/robotics_core/map_msgs/include/map_msgs/OccupancyGridUpdate.h diff --git a/build/CMakeFiles/layers.dir/depend.make b/build/CMakeFiles/layers.dir/depend.make index 9293b7c..b8295cf 100644 --- a/build/CMakeFiles/layers.dir/depend.make +++ b/build/CMakeFiles/layers.dir/depend.make @@ -15,3 +15,50 @@ CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o: /home/duongtd/robotics_core 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 +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: ../include/costmap_2d/cost_values.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: ../include/costmap_2d/costmap_2d.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: ../include/costmap_2d/costmap_layer.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: ../include/costmap_2d/footprint.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: ../include/costmap_2d/layer.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: ../include/costmap_2d/layered_costmap.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: ../include/costmap_2d/observation.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: ../include/costmap_2d/observation_buffer.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: ../include/costmap_2d/obstacle_layer.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point32.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/PointStamped.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Polygon.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/PolygonStamped.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Pose.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Quaternion.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/nav_msgs/include/nav_msgs/MapMetaData.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/nav_msgs/include/nav_msgs/OccupancyGrid.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/ChannelFloat32.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/LaserScan.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/PointCloud.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/PointCloud2.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/PointField.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/point_cloud_conversion.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/sensor_msgs/include/sensor_msgs/point_field_conversion.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.h +CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o: ../plugins/obstacle_layer.cpp + +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: ../include/costmap_2d/cost_values.h +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: ../include/costmap_2d/costmap_2d.h +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: ../include/costmap_2d/costmap_layer.h +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: ../include/costmap_2d/costmap_math.h +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: ../include/costmap_2d/layer.h +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: ../include/costmap_2d/layered_costmap.h +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: ../include/costmap_2d/static_layer.h +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Point.h +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Pose.h +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Quaternion.h +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Transform.h +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/TransformStamped.h +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/geometry_msgs/include/geometry_msgs/Vector3.h +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/nav_msgs/include/nav_msgs/MapMetaData.h +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/nav_msgs/include/nav_msgs/OccupancyGrid.h +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: /home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.h +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: ../plugins/static_layer.cpp +CMakeFiles/layers.dir/plugins/static_layer.cpp.o: /home/duongtd/robotics_core/map_msgs/include/map_msgs/OccupancyGridUpdate.h + diff --git a/build/CMakeFiles/layers.dir/flags.make b/build/CMakeFiles/layers.dir/flags.make index 6ac81f3..528ab90 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/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 +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 -I/home/duongtd/robotics_core/common_msgs/nav_msgs/include -I/home/duongtd/robotics_core/map_msgs/include diff --git a/build/CMakeFiles/layers.dir/link.txt b/build/CMakeFiles/layers.dir/link.txt index 6e4bd22..5eeb2c2 100644 --- a/build/CMakeFiles/layers.dir/link.txt +++ b/build/CMakeFiles/layers.dir/link.txt @@ -1,2 +1,2 @@ -/usr/bin/ar qc liblayers.a CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o +/usr/bin/ar qc liblayers.a CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o CMakeFiles/layers.dir/plugins/static_layer.cpp.o /usr/bin/ranlib liblayers.a diff --git a/build/CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o b/build/CMakeFiles/layers.dir/plugins/inflation_layer.cpp.o index cd064e3..cfb7ed0 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/CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o b/build/CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o new file mode 100644 index 0000000..1d79471 Binary files /dev/null and b/build/CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o differ diff --git a/build/CMakeFiles/layers.dir/plugins/static_layer.cpp.o b/build/CMakeFiles/layers.dir/plugins/static_layer.cpp.o new file mode 100644 index 0000000..c768e25 Binary files /dev/null and b/build/CMakeFiles/layers.dir/plugins/static_layer.cpp.o differ diff --git a/build/CMakeFiles/layers.dir/progress.make b/build/CMakeFiles/layers.dir/progress.make index 17875e3..635d1db 100644 --- a/build/CMakeFiles/layers.dir/progress.make +++ b/build/CMakeFiles/layers.dir/progress.make @@ -1,3 +1,5 @@ CMAKE_PROGRESS_1 = 10 CMAKE_PROGRESS_2 = 11 +CMAKE_PROGRESS_3 = 12 +CMAKE_PROGRESS_4 = 13 diff --git a/build/CMakeFiles/progress.marks b/build/CMakeFiles/progress.marks index b1bd38b..60d3b2f 100644 --- a/build/CMakeFiles/progress.marks +++ b/build/CMakeFiles/progress.marks @@ -1 +1 @@ -13 +15 diff --git a/build/Makefile b/build/Makefile index b0ace63..6d5fd12 100644 --- a/build/Makefile +++ b/build/Makefile @@ -176,6 +176,60 @@ plugins/inflation_layer.cpp.s: $(MAKE) -f CMakeFiles/layers.dir/build.make CMakeFiles/layers.dir/plugins/inflation_layer.cpp.s .PHONY : plugins/inflation_layer.cpp.s +plugins/obstacle_layer.o: plugins/obstacle_layer.cpp.o + +.PHONY : plugins/obstacle_layer.o + +# target to build an object file +plugins/obstacle_layer.cpp.o: + $(MAKE) -f CMakeFiles/layers.dir/build.make CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.o +.PHONY : plugins/obstacle_layer.cpp.o + +plugins/obstacle_layer.i: plugins/obstacle_layer.cpp.i + +.PHONY : plugins/obstacle_layer.i + +# target to preprocess a source file +plugins/obstacle_layer.cpp.i: + $(MAKE) -f CMakeFiles/layers.dir/build.make CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.i +.PHONY : plugins/obstacle_layer.cpp.i + +plugins/obstacle_layer.s: plugins/obstacle_layer.cpp.s + +.PHONY : plugins/obstacle_layer.s + +# target to generate assembly for a file +plugins/obstacle_layer.cpp.s: + $(MAKE) -f CMakeFiles/layers.dir/build.make CMakeFiles/layers.dir/plugins/obstacle_layer.cpp.s +.PHONY : plugins/obstacle_layer.cpp.s + +plugins/static_layer.o: plugins/static_layer.cpp.o + +.PHONY : plugins/static_layer.o + +# target to build an object file +plugins/static_layer.cpp.o: + $(MAKE) -f CMakeFiles/layers.dir/build.make CMakeFiles/layers.dir/plugins/static_layer.cpp.o +.PHONY : plugins/static_layer.cpp.o + +plugins/static_layer.i: plugins/static_layer.cpp.i + +.PHONY : plugins/static_layer.i + +# target to preprocess a source file +plugins/static_layer.cpp.i: + $(MAKE) -f CMakeFiles/layers.dir/build.make CMakeFiles/layers.dir/plugins/static_layer.cpp.i +.PHONY : plugins/static_layer.cpp.i + +plugins/static_layer.s: plugins/static_layer.cpp.s + +.PHONY : plugins/static_layer.s + +# target to generate assembly for a file +plugins/static_layer.cpp.s: + $(MAKE) -f CMakeFiles/layers.dir/build.make CMakeFiles/layers.dir/plugins/static_layer.cpp.s +.PHONY : plugins/static_layer.cpp.s + src/array_parser.o: src/array_parser.cpp.o .PHONY : src/array_parser.o @@ -406,6 +460,12 @@ help: @echo "... plugins/inflation_layer.o" @echo "... plugins/inflation_layer.i" @echo "... plugins/inflation_layer.s" + @echo "... plugins/obstacle_layer.o" + @echo "... plugins/obstacle_layer.i" + @echo "... plugins/obstacle_layer.s" + @echo "... plugins/static_layer.o" + @echo "... plugins/static_layer.i" + @echo "... plugins/static_layer.s" @echo "... src/array_parser.o" @echo "... src/array_parser.i" @echo "... src/array_parser.s" diff --git a/build/cmake_install.cmake b/build/cmake_install.cmake index 1167b56..e68bee0 100644 --- a/build/cmake_install.cmake +++ b/build/cmake_install.cmake @@ -41,6 +41,8 @@ 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") + include("/home/duongtd/robotics_core/costmap_2d/build/nav_msgs_build/cmake_install.cmake") + include("/home/duongtd/robotics_core/costmap_2d/build/map_msgs_build/cmake_install.cmake") endif() diff --git a/build/libcostmap_2d.a b/build/libcostmap_2d.a index 6fa58eb..6f29041 100644 Binary files a/build/libcostmap_2d.a and b/build/libcostmap_2d.a differ diff --git a/build/liblayers.a b/build/liblayers.a index 1bce906..a828405 100644 Binary files a/build/liblayers.a and b/build/liblayers.a differ diff --git a/build/map_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake b/build/map_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake new file mode 100644 index 0000000..1f736df --- /dev/null +++ b/build/map_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/map_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}) diff --git a/build/map_msgs_build/CMakeFiles/progress.marks b/build/map_msgs_build/CMakeFiles/progress.marks new file mode 100644 index 0000000..573541a --- /dev/null +++ b/build/map_msgs_build/CMakeFiles/progress.marks @@ -0,0 +1 @@ +0 diff --git a/build/map_msgs_build/Makefile b/build/map_msgs_build/Makefile new file mode 100644 index 0000000..c3ad8a2 --- /dev/null +++ b/build/map_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/map_msgs_build/CMakeFiles/progress.marks + cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f CMakeFiles/Makefile2 map_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 map_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 map_msgs_build/preinstall +.PHONY : preinstall + +# Prepare targets for installation. +preinstall/fast: + cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f CMakeFiles/Makefile2 map_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 + diff --git a/build/map_msgs_build/cmake_install.cmake b/build/map_msgs_build/cmake_install.cmake new file mode 100644 index 0000000..0716e6b --- /dev/null +++ b/build/map_msgs_build/cmake_install.cmake @@ -0,0 +1,39 @@ +# Install script for directory: /home/duongtd/robotics_core/map_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() + diff --git a/build/nav_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake b/build/nav_msgs_build/CMakeFiles/CMakeDirectoryInformation.cmake new file mode 100644 index 0000000..921b3ab --- /dev/null +++ b/build/nav_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/common_msgs/nav_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}) diff --git a/build/nav_msgs_build/CMakeFiles/progress.marks b/build/nav_msgs_build/CMakeFiles/progress.marks new file mode 100644 index 0000000..573541a --- /dev/null +++ b/build/nav_msgs_build/CMakeFiles/progress.marks @@ -0,0 +1 @@ +0 diff --git a/build/nav_msgs_build/Makefile b/build/nav_msgs_build/Makefile new file mode 100644 index 0000000..cb4471f --- /dev/null +++ b/build/nav_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/nav_msgs_build/CMakeFiles/progress.marks + cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f CMakeFiles/Makefile2 nav_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 nav_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 nav_msgs_build/preinstall +.PHONY : preinstall + +# Prepare targets for installation. +preinstall/fast: + cd /home/duongtd/robotics_core/costmap_2d/build && $(MAKE) -f CMakeFiles/Makefile2 nav_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 + diff --git a/build/nav_msgs_build/cmake_install.cmake b/build/nav_msgs_build/cmake_install.cmake new file mode 100644 index 0000000..b36b698 --- /dev/null +++ b/build/nav_msgs_build/cmake_install.cmake @@ -0,0 +1,39 @@ +# Install script for directory: /home/duongtd/robotics_core/common_msgs/nav_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() + 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 8b1ce56..5880a6d 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 @@ -55,6 +55,8 @@ sensor_msgs/PointCloud2.h /home/duongtd/robotics_core/common_msgs/sensor_msgs/test/sensor_msgs/PointCloud2.h iostream - +yaml-cpp/yaml.h +- /home/duongtd/robotics_core/common_msgs/std_msgs/include/std_msgs/Header.h string diff --git a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/link.txt b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/link.txt index 1a1485e..fdf1b18 100644 --- a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/link.txt +++ b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/link.txt @@ -1 +1 @@ -/usr/bin/c++ CMakeFiles/test_battery_state.dir/test/main.cpp.o -o test_battery_state +/usr/bin/c++ CMakeFiles/test_battery_state.dir/test/main.cpp.o -o test_battery_state -lyaml-cpp diff --git a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/progress.make b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/progress.make index 7df1340..8063b3b 100644 --- a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/progress.make +++ b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/progress.make @@ -1,3 +1,3 @@ -CMAKE_PROGRESS_1 = 12 -CMAKE_PROGRESS_2 = 13 +CMAKE_PROGRESS_1 = 14 +CMAKE_PROGRESS_2 = 15 diff --git a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o index bfd6774..5af4ddb 100644 Binary files a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o and b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o differ diff --git a/build/sensor_msgs_build/test_battery_state b/build/sensor_msgs_build/test_battery_state index f0551bf..530077a 100755 Binary files a/build/sensor_msgs_build/test_battery_state and b/build/sensor_msgs_build/test_battery_state differ diff --git a/config/config.yaml b/config/config.yaml new file mode 100644 index 0000000..d17193e --- /dev/null +++ b/config/config.yaml @@ -0,0 +1,5 @@ +inflation_layer: + enabled: true + inflate_unknown: false + cost_scaling_factor: 15.0 + inflation_radius: 0.55 \ No newline at end of file diff --git a/include/costmap_2d/inflation_layer.h b/include/costmap_2d/inflation_layer.h index a523f46..50a6137 100644 --- a/include/costmap_2d/inflation_layer.h +++ b/include/costmap_2d/inflation_layer.h @@ -2,14 +2,13 @@ #ifndef COSTMAP_2D_INFLATION_LAYER_H_ #define COSTMAP_2D_INFLATION_LAYER_H_ -// #include #include #include -// #include -// #include #include #include - +#include +#include +#include namespace costmap_2d { @@ -46,8 +45,6 @@ public: virtual ~InflationLayer() { deleteKernels(); - // if (dsrv_) - // delete dsrv_; if (seen_) delete[] seen_; } @@ -155,8 +152,7 @@ private: 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 getParams(); bool need_reinflation_; ///< Indicates that the entire costmap should be reinflated next time around. }; diff --git a/include/costmap_2d/layer.h b/include/costmap_2d/layer.h index ef7a289..69fe5dc 100644 --- a/include/costmap_2d/layer.h +++ b/include/costmap_2d/layer.h @@ -51,7 +51,7 @@ class Layer public: Layer(); - void initialize(LayeredCostmap* parent, std::string name, std::shared_ptr *tf); + void initialize(LayeredCostmap* parent, std::string name, tf2::BufferCore *tf); /** * @brief This is called by the LayeredCostmap to poll this plugin as to how @@ -140,7 +140,7 @@ protected: bool current_; bool enabled_; std::string name_; - std::shared_ptr *tf_; + tf2::BufferCore *tf_; private: std::vector footprint_spec_; diff --git a/include/costmap_2d/obstacle_layer.h b/include/costmap_2d/obstacle_layer.h index ea5a8db..61524e2 100644 --- a/include/costmap_2d/obstacle_layer.h +++ b/include/costmap_2d/obstacle_layer.h @@ -38,7 +38,6 @@ #ifndef COSTMAP_2D_OBSTACLE_LAYER_H_ #define COSTMAP_2D_OBSTACLE_LAYER_H_ -#include #include #include #include @@ -46,14 +45,14 @@ #include #include -#include +// #include #include #include #include -#include -#include -#include -#include +// #include +// #include +// #include +// #include #include namespace costmap_2d @@ -77,44 +76,43 @@ public: 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 + // * @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 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 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); + // /** + // * @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 @@ -152,10 +150,10 @@ protected: std::string global_frame_; ///< @brief The global frame for the costmap double max_obstacle_height_; ///< @brief Max Obstacle Height - laser_geometry::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds + // 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_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 @@ -164,12 +162,11 @@ protected: 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); + bool getParams(); }; } // namespace costmap_2d diff --git a/include/costmap_2d/static_layer.h b/include/costmap_2d/static_layer.h index da76095..c40bfb1 100644 --- a/include/costmap_2d/static_layer.h +++ b/include/costmap_2d/static_layer.h @@ -3,12 +3,13 @@ #include #include -#include -#include +// #include +// #include #include #include -#include +// #include #include +#include namespace costmap_2d { @@ -28,10 +29,10 @@ public: virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); virtual void matchSize(); + virtual void incomingMap(const nav_msgs::OccupancyGrid& new_map); + virtual void incomingUpdate(const map_msgs::OccupancyGridUpdate& update); protected: virtual unsigned char interpretValue(unsigned char value); - virtual void incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map); - virtual void incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update); unsigned char* threshold_; std::string base_frame_id_; std::string global_frame_; ///< @brief The global frame for the costmap @@ -45,7 +46,7 @@ protected: bool use_maximum_; bool first_map_only_; ///< @brief Store the first static map and reuse it on reinitializing bool trinary_costmap_; - ros::Subscriber map_sub_, map_update_sub_; + // ros::Subscriber map_sub_, map_update_sub_; unsigned char lethal_threshold_, unknown_cost_value_; private: @@ -55,8 +56,8 @@ private: * map along with its size will determine what parts of the costmap's * static map are overwritten. */ - void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level); - dynamic_reconfigure::Server *dsrv_; + // void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level); + // dynamic_reconfigure::Server *dsrv_; }; } // namespace costmap_2d diff --git a/plugins/inflation_layer.cpp b/plugins/inflation_layer.cpp index 5e74c62..4691ce5 100644 --- a/plugins/inflation_layer.cpp +++ b/plugins/inflation_layer.cpp @@ -33,34 +33,48 @@ InflationLayer::InflationLayer() inflation_access_ = new boost::recursive_mutex(); } -// void InflationLayer::onInitialize() -// { -// { -// boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); -// ros::NodeHandle nh("~/" + name_), g_nh; -// current_ = true; -// if (seen_) -// delete[] seen_; -// seen_ = NULL; -// seen_size_ = 0; -// need_reinflation_ = false; +void InflationLayer::onInitialize() +{ + { + boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); + current_ = true; + if (seen_) + delete[] seen_; + seen_ = NULL; + seen_size_ = 0; + need_reinflation_ = false; -// dynamic_reconfigure::Server::CallbackType cb = -// [this](auto& config, auto level){ reconfigureCB(config, level); }; + getParams(); + } -// if (dsrv_ != NULL){ -// dsrv_->clearCallback(); -// dsrv_->setCallback(cb); -// } -// else -// { -// dsrv_ = new dynamic_reconfigure::Server(ros::NodeHandle("~/" + name_)); -// dsrv_->setCallback(cb); -// } -// } + matchSize(); +} -// matchSize(); -// } +bool InflationLayer::getParams() +{ + try { + YAML::Node config = YAML::LoadFile("../cfg/config.yaml"); + double cost_scaling_factor = config["inflation_layer"]["cost_scaling_factor"].as(); + double inflation_radius = config["inflation_layer"]["inflation_radius"].as(); + setInflationParameters(inflation_radius, cost_scaling_factor); + + bool enabled = config["inflation_layer"]["enabled"].as(); + bool inflate_unknown = config["inflation_layer"]["inflate_unknown"].as(); + + if (enabled_ != enabled || inflate_unknown_ != inflate_unknown) { + enabled_ = enabled; + inflate_unknown_ = inflate_unknown; + need_reinflation_ = true; + } + } + catch (const YAML::BadFile& e) { + std::cerr << "Cannot open YAML file: " << e.what() << std::endl; + return false; + } + + return true; + +} // void InflationLayer::reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level) // { @@ -73,276 +87,278 @@ InflationLayer::InflationLayer() // } // } -// void InflationLayer::matchSize() -// { -// boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); -// costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap(); -// resolution_ = costmap->getResolution(); -// cell_inflation_radius_ = cellDistance(inflation_radius_); -// computeCaches(); +void InflationLayer::matchSize() +{ + boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); + costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap(); + resolution_ = costmap->getResolution(); + cell_inflation_radius_ = cellDistance(inflation_radius_); + computeCaches(); -// unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY(); -// if (seen_) -// delete[] seen_; -// seen_size_ = size_x * size_y; -// seen_ = new bool[seen_size_]; -// } + unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY(); + if (seen_) + delete[] seen_; + seen_size_ = size_x * size_y; + seen_ = new bool[seen_size_]; +} -// void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, -// double* min_y, double* max_x, double* max_y) -// { -// if (need_reinflation_) -// { -// last_min_x_ = *min_x; -// last_min_y_ = *min_y; -// last_max_x_ = *max_x; -// last_max_y_ = *max_y; -// // For some reason when I make these -::max() it does not -// // work with Costmap2D::worldToMapEnforceBounds(), so I'm using -// // -::max() instead. -// *min_x = -std::numeric_limits::max(); -// *min_y = -std::numeric_limits::max(); -// *max_x = std::numeric_limits::max(); -// *max_y = std::numeric_limits::max(); -// need_reinflation_ = false; -// } -// else -// { -// double tmp_min_x = last_min_x_; -// double tmp_min_y = last_min_y_; -// double tmp_max_x = last_max_x_; -// double tmp_max_y = last_max_y_; -// last_min_x_ = *min_x; -// last_min_y_ = *min_y; -// last_max_x_ = *max_x; -// last_max_y_ = *max_y; -// *min_x = std::min(tmp_min_x, *min_x) - inflation_radius_; -// *min_y = std::min(tmp_min_y, *min_y) - inflation_radius_; -// *max_x = std::max(tmp_max_x, *max_x) + inflation_radius_; -// *max_y = std::max(tmp_max_y, *max_y) + inflation_radius_; -// } -// } +void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, + double* min_y, double* max_x, double* max_y) +{ + if (need_reinflation_) + { + last_min_x_ = *min_x; + last_min_y_ = *min_y; + last_max_x_ = *max_x; + last_max_y_ = *max_y; + // For some reason when I make these -::max() it does not + // work with Costmap2D::worldToMapEnforceBounds(), so I'm using + // -::max() instead. + *min_x = -std::numeric_limits::max(); + *min_y = -std::numeric_limits::max(); + *max_x = std::numeric_limits::max(); + *max_y = std::numeric_limits::max(); + need_reinflation_ = false; + } + else + { + double tmp_min_x = last_min_x_; + double tmp_min_y = last_min_y_; + double tmp_max_x = last_max_x_; + double tmp_max_y = last_max_y_; + last_min_x_ = *min_x; + last_min_y_ = *min_y; + last_max_x_ = *max_x; + last_max_y_ = *max_y; + *min_x = std::min(tmp_min_x, *min_x) - inflation_radius_; + *min_y = std::min(tmp_min_y, *min_y) - inflation_radius_; + *max_x = std::max(tmp_max_x, *max_x) + inflation_radius_; + *max_y = std::max(tmp_max_y, *max_y) + inflation_radius_; + } +} -// void InflationLayer::onFootprintChanged() -// { -// inscribed_radius_ = layered_costmap_->getInscribedRadius(); -// cell_inflation_radius_ = cellDistance(inflation_radius_); -// computeCaches(); -// need_reinflation_ = true; +void InflationLayer::onFootprintChanged() +{ + inscribed_radius_ = layered_costmap_->getInscribedRadius(); + cell_inflation_radius_ = cellDistance(inflation_radius_); + computeCaches(); + need_reinflation_ = true; -// ROS_DEBUG("InflationLayer::onFootprintChanged(): num footprint points: %lu," -// " inscribed_radius_ = %.3f, inflation_radius_ = %.3f", -// layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_); -// } + printf("InflationLayer::onFootprintChanged(): num footprint points: %lu," + " inscribed_radius_ = %.3f, inflation_radius_ = %.3f", + layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_); +} -// void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) -// { -// boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); -// if (cell_inflation_radius_ == 0) -// return; +void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) +{ + boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); + if (cell_inflation_radius_ == 0) + return; -// // make sure the inflation list is empty at the beginning of the cycle (should always be true) -// ROS_ASSERT_MSG(inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation"); + // make sure the inflation list is empty at the beginning of the cycle (should always be true) + if (!inflation_cells_.empty()) { + throw std::runtime_error("The inflation list must be empty at the beginning of inflation"); +} -// unsigned char* master_array = master_grid.getCharMap(); -// unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY(); + unsigned char* master_array = master_grid.getCharMap(); + unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY(); -// if (seen_ == NULL) { -// ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL"); -// seen_size_ = size_x * size_y; -// seen_ = new bool[seen_size_]; -// } -// else if (seen_size_ != size_x * size_y) -// { -// ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong"); -// delete[] seen_; -// seen_size_ = size_x * size_y; -// seen_ = new bool[seen_size_]; -// } -// memset(seen_, false, size_x * size_y * sizeof(bool)); + if (seen_ == NULL) { + std::cerr <<"InflationLayer::updateCosts(): seen_ array is NULL" < to emulate the priority queue used before, with a notable performance boost + // Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle + // We use a map to emulate the priority queue used before, with a notable performance boost -// // Start with lethal obstacles: by definition distance is 0.0 -// std::vector& obs_bin = inflation_cells_[0.0]; -// for (int j = min_j; j < max_j; j++) -// { -// for (int i = min_i; i < max_i; i++) -// { -// int index = master_grid.getIndex(i, j); -// unsigned char cost = master_array[index]; -// if (cost == LETHAL_OBSTACLE) -// { -// obs_bin.push_back(CellData(index, i, j, i, j)); -// } -// } -// } + // Start with lethal obstacles: by definition distance is 0.0 + std::vector& obs_bin = inflation_cells_[0.0]; + for (int j = min_j; j < max_j; j++) + { + for (int i = min_i; i < max_i; i++) + { + int index = master_grid.getIndex(i, j); + unsigned char cost = master_array[index]; + if (cost == LETHAL_OBSTACLE) + { + obs_bin.push_back(CellData(index, i, j, i, j)); + } + } + } -// // Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they -// // can overtake previously inserted but farther away cells -// std::map >::iterator bin; -// for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin) -// { -// for (int i = 0; i < bin->second.size(); ++i) -// { -// // process all cells at distance dist_bin.first -// const CellData& cell = bin->second[i]; + // Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they + // can overtake previously inserted but farther away cells + std::map >::iterator bin; + for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin) + { + for (int i = 0; i < bin->second.size(); ++i) + { + // process all cells at distance dist_bin.first + const CellData& cell = bin->second[i]; -// unsigned int index = cell.index_; + unsigned int index = cell.index_; -// // ignore if already visited -// if (seen_[index]) -// { -// continue; -// } + // ignore if already visited + if (seen_[index]) + { + continue; + } -// seen_[index] = true; + seen_[index] = true; -// unsigned int mx = cell.x_; -// unsigned int my = cell.y_; -// unsigned int sx = cell.src_x_; -// unsigned int sy = cell.src_y_; + unsigned int mx = cell.x_; + unsigned int my = cell.y_; + unsigned int sx = cell.src_x_; + unsigned int sy = cell.src_y_; -// // assign the cost associated with the distance from an obstacle to the cell -// unsigned char cost = costLookup(mx, my, sx, sy); -// unsigned char old_cost = master_array[index]; -// if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE))) -// master_array[index] = cost; -// else -// master_array[index] = std::max(old_cost, cost); + // assign the cost associated with the distance from an obstacle to the cell + unsigned char cost = costLookup(mx, my, sx, sy); + unsigned char old_cost = master_array[index]; + if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE))) + master_array[index] = cost; + else + master_array[index] = std::max(old_cost, cost); -// // attempt to put the neighbors of the current cell onto the inflation list -// if (mx > 0) -// enqueue(index - 1, mx - 1, my, sx, sy); -// if (my > 0) -// enqueue(index - size_x, mx, my - 1, sx, sy); -// if (mx < size_x - 1) -// enqueue(index + 1, mx + 1, my, sx, sy); -// if (my < size_y - 1) -// enqueue(index + size_x, mx, my + 1, sx, sy); -// } -// } + // attempt to put the neighbors of the current cell onto the inflation list + if (mx > 0) + enqueue(index - 1, mx - 1, my, sx, sy); + if (my > 0) + enqueue(index - size_x, mx, my - 1, sx, sy); + if (mx < size_x - 1) + enqueue(index + 1, mx + 1, my, sx, sy); + if (my < size_y - 1) + enqueue(index + size_x, mx, my + 1, sx, sy); + } + } -// inflation_cells_.clear(); -// } + inflation_cells_.clear(); +} -// /** -// * @brief Given an index of a cell in the costmap, place it into a list pending for obstacle inflation -// * @param grid The costmap -// * @param index The index of the cell -// * @param mx The x coordinate of the cell (can be computed from the index, but saves time to store it) -// * @param my The y coordinate of the cell (can be computed from the index, but saves time to store it) -// * @param src_x The x index of the obstacle point inflation started at -// * @param src_y The y index of the obstacle point inflation started at -// */ -// inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my, -// unsigned int src_x, unsigned int src_y) -// { -// if (!seen_[index]) -// { -// // we compute our distance table one cell further than the inflation radius dictates so we can make the check below -// double distance = distanceLookup(mx, my, src_x, src_y); +/** + * @brief Given an index of a cell in the costmap, place it into a list pending for obstacle inflation + * @param grid The costmap + * @param index The index of the cell + * @param mx The x coordinate of the cell (can be computed from the index, but saves time to store it) + * @param my The y coordinate of the cell (can be computed from the index, but saves time to store it) + * @param src_x The x index of the obstacle point inflation started at + * @param src_y The y index of the obstacle point inflation started at + */ +inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my, + unsigned int src_x, unsigned int src_y) +{ + if (!seen_[index]) + { + // we compute our distance table one cell further than the inflation radius dictates so we can make the check below + double distance = distanceLookup(mx, my, src_x, src_y); -// // we only want to put the cell in the list if it is within the inflation radius of the obstacle point -// if (distance > cell_inflation_radius_) -// return; + // we only want to put the cell in the list if it is within the inflation radius of the obstacle point + if (distance > cell_inflation_radius_) + return; -// // push the cell data onto the inflation list and mark -// inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y)); -// } -// } + // push the cell data onto the inflation list and mark + inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y)); + } +} -// void InflationLayer::computeCaches() -// { -// if (cell_inflation_radius_ == 0) -// return; +void InflationLayer::computeCaches() +{ + if (cell_inflation_radius_ == 0) + return; -// // based on the inflation radius... compute distance and cost caches -// if (cell_inflation_radius_ != cached_cell_inflation_radius_) -// { -// deleteKernels(); + // based on the inflation radius... compute distance and cost caches + if (cell_inflation_radius_ != cached_cell_inflation_radius_) + { + deleteKernels(); -// cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2]; -// cached_distances_ = new double*[cell_inflation_radius_ + 2]; + cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2]; + cached_distances_ = new double*[cell_inflation_radius_ + 2]; -// for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) -// { -// cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2]; -// cached_distances_[i] = new double[cell_inflation_radius_ + 2]; -// for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) -// { -// cached_distances_[i][j] = hypot(i, j); -// } -// } + for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) + { + cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2]; + cached_distances_[i] = new double[cell_inflation_radius_ + 2]; + for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) + { + cached_distances_[i][j] = hypot(i, j); + } + } -// cached_cell_inflation_radius_ = cell_inflation_radius_; -// } + cached_cell_inflation_radius_ = cell_inflation_radius_; + } -// for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) -// { -// for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) -// { -// cached_costs_[i][j] = computeCost(cached_distances_[i][j]); -// } -// } -// } + for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) + { + for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) + { + cached_costs_[i][j] = computeCost(cached_distances_[i][j]); + } + } +} -// void InflationLayer::deleteKernels() -// { -// if (cached_distances_ != NULL) -// { -// for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i) -// { -// if (cached_distances_[i]) -// delete[] cached_distances_[i]; -// } -// if (cached_distances_) -// delete[] cached_distances_; -// cached_distances_ = NULL; -// } +void InflationLayer::deleteKernels() +{ + if (cached_distances_ != NULL) + { + for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i) + { + if (cached_distances_[i]) + delete[] cached_distances_[i]; + } + if (cached_distances_) + delete[] cached_distances_; + cached_distances_ = NULL; + } -// if (cached_costs_ != NULL) -// { -// for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i) -// { -// if (cached_costs_[i]) -// delete[] cached_costs_[i]; -// } -// delete[] cached_costs_; -// cached_costs_ = NULL; -// } -// } + if (cached_costs_ != NULL) + { + for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i) + { + if (cached_costs_[i]) + delete[] cached_costs_[i]; + } + delete[] cached_costs_; + cached_costs_ = NULL; + } +} -// void InflationLayer::setInflationParameters(double inflation_radius, double cost_scaling_factor) -// { -// if (weight_ != cost_scaling_factor || inflation_radius_ != inflation_radius) -// { -// // Lock here so that reconfiguring the inflation radius doesn't cause segfaults -// // when accessing the cached arrays -// boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); +void InflationLayer::setInflationParameters(double inflation_radius, double cost_scaling_factor) +{ + if (weight_ != cost_scaling_factor || inflation_radius_ != inflation_radius) + { + // Lock here so that reconfiguring the inflation radius doesn't cause segfaults + // when accessing the cached arrays + boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); -// inflation_radius_ = inflation_radius; -// cell_inflation_radius_ = cellDistance(inflation_radius_); -// weight_ = cost_scaling_factor; -// need_reinflation_ = true; -// computeCaches(); -// } -// } + inflation_radius_ = inflation_radius; + cell_inflation_radius_ = cellDistance(inflation_radius_); + weight_ = cost_scaling_factor; + need_reinflation_ = true; + computeCaches(); + } +} // Export factory function static PluginPtr create_inflation_plugin() { diff --git a/plugins/obstacle_layer.cpp b/plugins/obstacle_layer.cpp index 1f713a3..149f28c 100644 --- a/plugins/obstacle_layer.cpp +++ b/plugins/obstacle_layer.cpp @@ -1,623 +1,586 @@ -/********************************************************************* - * - * 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!! - *********************************************************************/ #include -#include -#include - -#include -#include - -PLUGINLIB_EXPORT_CLASS(costmap_2d::ObstacleLayer, costmap_2d::Layer) - -using costmap_2d::NO_INFORMATION; -using costmap_2d::LETHAL_OBSTACLE; -using costmap_2d::FREE_SPACE; - -using costmap_2d::ObservationBuffer; -using costmap_2d::Observation; - -namespace costmap_2d -{ - -void ObstacleLayer::onInitialize() -{ - ros::NodeHandle nh("~/" + name_), g_nh; - rolling_window_ = layered_costmap_->isRolling(); - - bool track_unknown_space; - nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown()); - if (track_unknown_space) - default_value_ = NO_INFORMATION; - else - default_value_ = FREE_SPACE; - - ObstacleLayer::matchSize(); - current_ = true; - - global_frame_ = layered_costmap_->getGlobalFrameID(); - double transform_tolerance; - nh.param("transform_tolerance", transform_tolerance, 0.2); - - std::string topics_string; - // get the topics that we'll subscribe to from the parameter server - nh.param("observation_sources", topics_string, std::string("")); - ROS_INFO(" Subscribed to Topics: %s", topics_string.c_str()); - - // now we need to split the topics based on whitespace which we can use a stringstream for - std::stringstream ss(topics_string); - - std::string source; - while (ss >> source) - { - ros::NodeHandle source_node(nh, source); - - // get the parameters for the specific topic - double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height; - std::string topic, sensor_frame, data_type; - bool inf_is_valid, clearing, marking; - - source_node.param("topic", topic, source); - source_node.param("sensor_frame", sensor_frame, std::string("")); - source_node.param("observation_persistence", observation_keep_time, 0.0); - source_node.param("expected_update_rate", expected_update_rate, 0.0); - source_node.param("data_type", data_type, std::string("PointCloud")); - source_node.param("min_obstacle_height", min_obstacle_height, 0.0); - source_node.param("max_obstacle_height", max_obstacle_height, 2.0); - source_node.param("inf_is_valid", inf_is_valid, false); - source_node.param("clearing", clearing, false); - source_node.param("marking", marking, true); - - if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan")) - { - ROS_FATAL("Only topics that use point clouds or laser scans are currently supported"); - throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported"); - } - - std::string raytrace_range_param_name, obstacle_range_param_name; - - // get the obstacle range for the sensor - double obstacle_range = 2.5; - if (source_node.searchParam("obstacle_range", obstacle_range_param_name)) - { - source_node.getParam(obstacle_range_param_name, obstacle_range); - } - - // get the raytrace range for the sensor - double raytrace_range = 3.0; - if (source_node.searchParam("raytrace_range", raytrace_range_param_name)) - { - source_node.getParam(raytrace_range_param_name, raytrace_range); - } - - ROS_DEBUG("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.c_str(), - sensor_frame.c_str()); - - // create an observation buffer - observation_buffers_.push_back( - boost::shared_ptr < ObservationBuffer - > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height, - max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_, - sensor_frame, transform_tolerance))); - - // check if we'll add this buffer to our marking observation buffers - if (marking) - marking_buffers_.push_back(observation_buffers_.back()); - - // check if we'll also add this buffer to our clearing observation buffers - if (clearing) - clearing_buffers_.push_back(observation_buffers_.back()); - - ROS_DEBUG( - "Created an observation buffer for source %s, topic %s, global frame: %s, " - "expected update rate: %.2f, observation persistence: %.2f", - source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time); - - // create a callback for the topic - if (data_type == "LaserScan") - { - boost::shared_ptr < message_filters::Subscriber - > sub(new message_filters::Subscriber(g_nh, topic, 50)); - - boost::shared_ptr > filter( - new tf2_ros::MessageFilter(*sub, *tf_, global_frame_, 50, g_nh)); - - if (inf_is_valid) - { - filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanValidInfCallback(msg, buffer); }); - } - else - { - filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanCallback(msg, buffer); }); - } - - observation_subscribers_.push_back(sub); - observation_notifiers_.push_back(filter); - - observation_notifiers_.back()->setTolerance(ros::Duration(0.05)); - } - else if (data_type == "PointCloud") - { - boost::shared_ptr < message_filters::Subscriber - > sub(new message_filters::Subscriber(g_nh, topic, 50)); - - if (inf_is_valid) - { - ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations."); - } - - boost::shared_ptr < tf2_ros::MessageFilter - > filter(new tf2_ros::MessageFilter(*sub, *tf_, global_frame_, 50, g_nh)); - filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloudCallback(msg, buffer); }); - - observation_subscribers_.push_back(sub); - observation_notifiers_.push_back(filter); - } - else - { - boost::shared_ptr < message_filters::Subscriber - > sub(new message_filters::Subscriber(g_nh, topic, 50)); - - if (inf_is_valid) - { - ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations."); - } - - boost::shared_ptr < tf2_ros::MessageFilter - > filter(new tf2_ros::MessageFilter(*sub, *tf_, global_frame_, 50, g_nh)); - filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloud2Callback(msg, buffer); }); - - observation_subscribers_.push_back(sub); - observation_notifiers_.push_back(filter); - } - - if (sensor_frame != "") - { - std::vector < std::string > target_frames; - target_frames.push_back(global_frame_); - target_frames.push_back(sensor_frame); - observation_notifiers_.back()->setTargetFrames(target_frames); - } - } - - dsrv_ = NULL; - setupDynamicReconfigure(nh); -} - -void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle& nh) -{ - dsrv_ = new dynamic_reconfigure::Server(nh); - dynamic_reconfigure::Server::CallbackType cb = - [this](auto& config, auto level){ reconfigureCB(config, level); }; - dsrv_->setCallback(cb); -} - -ObstacleLayer::~ObstacleLayer() -{ - if (dsrv_) - delete dsrv_; -} -void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level) -{ - enabled_ = config.enabled; - footprint_clearing_enabled_ = config.footprint_clearing_enabled; - max_obstacle_height_ = config.max_obstacle_height; - combination_method_ = config.combination_method; -} - -void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message, - const boost::shared_ptr& buffer) -{ - // project the laser into a point cloud - sensor_msgs::PointCloud2 cloud; - cloud.header = message->header; - - // project the scan into a point cloud - try - { - projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_); - } - catch (tf2::TransformException &ex) - { - ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(), - ex.what()); - projector_.projectLaser(*message, cloud); - } - catch (std::runtime_error &ex) - { - ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what()); - return; //ignore this message - } - - // buffer the point cloud - buffer->lock(); - buffer->bufferCloud(cloud); - buffer->unlock(); -} - -void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message, - const boost::shared_ptr& buffer) -{ - // Filter positive infinities ("Inf"s) to max_range. - float epsilon = 0.0001; // a tenth of a millimeter - sensor_msgs::LaserScan message = *raw_message; - for (size_t i = 0; i < message.ranges.size(); i++) - { - float range = message.ranges[ i ]; - if (!std::isfinite(range) && range > 0) - { - message.ranges[ i ] = message.range_max - epsilon; - } - } - - // project the laser into a point cloud - sensor_msgs::PointCloud2 cloud; - cloud.header = message.header; - - // project the scan into a point cloud - try - { - projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_); - } - catch (tf2::TransformException &ex) - { - ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", - global_frame_.c_str(), ex.what()); - projector_.projectLaser(message, cloud); - } - catch (std::runtime_error &ex) - { - ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what()); - return; //ignore this message - } - - // buffer the point cloud - buffer->lock(); - buffer->bufferCloud(cloud); - buffer->unlock(); -} - -void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message, - const boost::shared_ptr& buffer) -{ - sensor_msgs::PointCloud2 cloud2; - - if (!sensor_msgs::convertPointCloudToPointCloud2(*message, cloud2)) - { - ROS_ERROR("Failed to convert a PointCloud to a PointCloud2, dropping message"); - return; - } - - // buffer the point cloud - buffer->lock(); - buffer->bufferCloud(cloud2); - buffer->unlock(); -} - -void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message, - const boost::shared_ptr& buffer) -{ - // buffer the point cloud - buffer->lock(); - buffer->bufferCloud(*message); - buffer->unlock(); -} - -void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, - double* min_y, double* max_x, double* max_y) -{ - if (rolling_window_) - updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); - useExtraBounds(min_x, min_y, max_x, max_y); - - bool current = true; - std::vector observations, clearing_observations; - - // get the marking observations - current = current && getMarkingObservations(observations); - - // get the clearing observations - current = current && getClearingObservations(clearing_observations); - - // update the global current status - current_ = current; - - // raytrace freespace - for (unsigned int i = 0; i < clearing_observations.size(); ++i) - { - raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y); - } - - // place the new obstacles into a priority queue... each with a priority of zero to begin with - for (std::vector::const_iterator it = observations.begin(); it != observations.end(); ++it) - { - const Observation& obs = *it; - - const sensor_msgs::PointCloud2& cloud = *(obs.cloud_); - - double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_; - - sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x"); - sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); - sensor_msgs::PointCloud2ConstIterator iter_z(cloud, "z"); - - for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z) - { - double px = *iter_x, py = *iter_y, pz = *iter_z; - - // if the obstacle is too high or too far away from the robot we won't add it - if (pz > max_obstacle_height_) - { - ROS_DEBUG("The point is too high"); - continue; - } - - // compute the squared distance from the hitpoint to the pointcloud's origin - double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y) - + (pz - obs.origin_.z) * (pz - obs.origin_.z); - - // if the point is far enough away... we won't consider it - if (sq_dist >= sq_obstacle_range) - { - ROS_DEBUG("The point is too far away"); - continue; - } - - // now we need to compute the map coordinates for the observation - unsigned int mx, my; - if (!worldToMap(px, py, mx, my)) - { - ROS_DEBUG("Computing map coords failed"); - continue; - } - - unsigned int index = getIndex(mx, my); - costmap_[index] = LETHAL_OBSTACLE; - touch(px, py, min_x, min_y, max_x, max_y); - } - } - - updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); -} - -void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, - double* max_x, double* max_y) -{ - if (!footprint_clearing_enabled_) return; - transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); - - for (unsigned int i = 0; i < transformed_footprint_.size(); i++) - { - touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y); - } -} - -void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) -{ - if (footprint_clearing_enabled_) - { - setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE); - } +// #include +// #include + +// #include +// #include + +// PLUGINLIB_EXPORT_CLASS(costmap_2d::ObstacleLayer, costmap_2d::Layer) + +// using costmap_2d::NO_INFORMATION; +// using costmap_2d::LETHAL_OBSTACLE; +// using costmap_2d::FREE_SPACE; + +// using costmap_2d::ObservationBuffer; +// using costmap_2d::Observation; + +// namespace costmap_2d +// { + +// void ObstacleLayer::onInitialize() +// { +// ros::NodeHandle nh("~/" + name_), g_nh; +// rolling_window_ = layered_costmap_->isRolling(); + +// bool track_unknown_space; +// nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown()); +// if (track_unknown_space) +// default_value_ = NO_INFORMATION; +// else +// default_value_ = FREE_SPACE; + +// ObstacleLayer::matchSize(); +// current_ = true; + +// global_frame_ = layered_costmap_->getGlobalFrameID(); +// double transform_tolerance; +// nh.param("transform_tolerance", transform_tolerance, 0.2); + +// std::string topics_string; +// // get the topics that we'll subscribe to from the parameter server +// nh.param("observation_sources", topics_string, std::string("")); +// ROS_INFO(" Subscribed to Topics: %s", topics_string.c_str()); + +// // now we need to split the topics based on whitespace which we can use a stringstream for +// std::stringstream ss(topics_string); + +// std::string source; +// while (ss >> source) +// { +// ros::NodeHandle source_node(nh, source); + +// // get the parameters for the specific topic +// double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height; +// std::string topic, sensor_frame, data_type; +// bool inf_is_valid, clearing, marking; + +// source_node.param("topic", topic, source); +// source_node.param("sensor_frame", sensor_frame, std::string("")); +// source_node.param("observation_persistence", observation_keep_time, 0.0); +// source_node.param("expected_update_rate", expected_update_rate, 0.0); +// source_node.param("data_type", data_type, std::string("PointCloud")); +// source_node.param("min_obstacle_height", min_obstacle_height, 0.0); +// source_node.param("max_obstacle_height", max_obstacle_height, 2.0); +// source_node.param("inf_is_valid", inf_is_valid, false); +// source_node.param("clearing", clearing, false); +// source_node.param("marking", marking, true); + +// if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan")) +// { +// ROS_FATAL("Only topics that use point clouds or laser scans are currently supported"); +// throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported"); +// } + +// std::string raytrace_range_param_name, obstacle_range_param_name; + +// // get the obstacle range for the sensor +// double obstacle_range = 2.5; +// if (source_node.searchParam("obstacle_range", obstacle_range_param_name)) +// { +// source_node.getParam(obstacle_range_param_name, obstacle_range); +// } + +// // get the raytrace range for the sensor +// double raytrace_range = 3.0; +// if (source_node.searchParam("raytrace_range", raytrace_range_param_name)) +// { +// source_node.getParam(raytrace_range_param_name, raytrace_range); +// } + +// ROS_DEBUG("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.c_str(), +// sensor_frame.c_str()); + +// // create an observation buffer +// observation_buffers_.push_back( +// boost::shared_ptr < ObservationBuffer +// > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height, +// max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_, +// sensor_frame, transform_tolerance))); + +// // check if we'll add this buffer to our marking observation buffers +// if (marking) +// marking_buffers_.push_back(observation_buffers_.back()); + +// // check if we'll also add this buffer to our clearing observation buffers +// if (clearing) +// clearing_buffers_.push_back(observation_buffers_.back()); + +// ROS_DEBUG( +// "Created an observation buffer for source %s, topic %s, global frame: %s, " +// "expected update rate: %.2f, observation persistence: %.2f", +// source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time); + +// // create a callback for the topic +// if (data_type == "LaserScan") +// { +// boost::shared_ptr < message_filters::Subscriber +// > sub(new message_filters::Subscriber(g_nh, topic, 50)); + +// boost::shared_ptr > filter( +// new tf2_ros::MessageFilter(*sub, *tf_, global_frame_, 50, g_nh)); + +// if (inf_is_valid) +// { +// filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanValidInfCallback(msg, buffer); }); +// } +// else +// { +// filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanCallback(msg, buffer); }); +// } + +// observation_subscribers_.push_back(sub); +// observation_notifiers_.push_back(filter); + +// observation_notifiers_.back()->setTolerance(ros::Duration(0.05)); +// } +// else if (data_type == "PointCloud") +// { +// boost::shared_ptr < message_filters::Subscriber +// > sub(new message_filters::Subscriber(g_nh, topic, 50)); + +// if (inf_is_valid) +// { +// ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations."); +// } + +// boost::shared_ptr < tf2_ros::MessageFilter +// > filter(new tf2_ros::MessageFilter(*sub, *tf_, global_frame_, 50, g_nh)); +// filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloudCallback(msg, buffer); }); + +// observation_subscribers_.push_back(sub); +// observation_notifiers_.push_back(filter); +// } +// else +// { +// boost::shared_ptr < message_filters::Subscriber +// > sub(new message_filters::Subscriber(g_nh, topic, 50)); + +// if (inf_is_valid) +// { +// ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations."); +// } + +// boost::shared_ptr < tf2_ros::MessageFilter +// > filter(new tf2_ros::MessageFilter(*sub, *tf_, global_frame_, 50, g_nh)); +// filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloud2Callback(msg, buffer); }); + +// observation_subscribers_.push_back(sub); +// observation_notifiers_.push_back(filter); +// } + +// if (sensor_frame != "") +// { +// std::vector < std::string > target_frames; +// target_frames.push_back(global_frame_); +// target_frames.push_back(sensor_frame); +// observation_notifiers_.back()->setTargetFrames(target_frames); +// } +// } + +// dsrv_ = NULL; +// setupDynamicReconfigure(nh); +// } + +// void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle& nh) +// { +// dsrv_ = new dynamic_reconfigure::Server(nh); +// dynamic_reconfigure::Server::CallbackType cb = +// [this](auto& config, auto level){ reconfigureCB(config, level); }; +// dsrv_->setCallback(cb); +// } + +// ObstacleLayer::~ObstacleLayer() +// { +// if (dsrv_) +// delete dsrv_; +// } +// void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level) +// { +// enabled_ = config.enabled; +// footprint_clearing_enabled_ = config.footprint_clearing_enabled; +// max_obstacle_height_ = config.max_obstacle_height; +// combination_method_ = config.combination_method; +// } + +// void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message, +// const boost::shared_ptr& buffer) +// { +// // project the laser into a point cloud +// sensor_msgs::PointCloud2 cloud; +// cloud.header = message->header; + +// // project the scan into a point cloud +// try +// { +// projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_); +// } +// catch (tf2::TransformException &ex) +// { +// ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(), +// ex.what()); +// projector_.projectLaser(*message, cloud); +// } +// catch (std::runtime_error &ex) +// { +// ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what()); +// return; //ignore this message +// } + +// // buffer the point cloud +// buffer->lock(); +// buffer->bufferCloud(cloud); +// buffer->unlock(); +// } + +// void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message, +// const boost::shared_ptr& buffer) +// { +// // Filter positive infinities ("Inf"s) to max_range. +// float epsilon = 0.0001; // a tenth of a millimeter +// sensor_msgs::LaserScan message = *raw_message; +// for (size_t i = 0; i < message.ranges.size(); i++) +// { +// float range = message.ranges[ i ]; +// if (!std::isfinite(range) && range > 0) +// { +// message.ranges[ i ] = message.range_max - epsilon; +// } +// } + +// // project the laser into a point cloud +// sensor_msgs::PointCloud2 cloud; +// cloud.header = message.header; + +// // project the scan into a point cloud +// try +// { +// projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_); +// } +// catch (tf2::TransformException &ex) +// { +// ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", +// global_frame_.c_str(), ex.what()); +// projector_.projectLaser(message, cloud); +// } +// catch (std::runtime_error &ex) +// { +// ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what()); +// return; //ignore this message +// } + +// // buffer the point cloud +// buffer->lock(); +// buffer->bufferCloud(cloud); +// buffer->unlock(); +// } + +// void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message, +// const boost::shared_ptr& buffer) +// { +// sensor_msgs::PointCloud2 cloud2; + +// if (!sensor_msgs::convertPointCloudToPointCloud2(*message, cloud2)) +// { +// ROS_ERROR("Failed to convert a PointCloud to a PointCloud2, dropping message"); +// return; +// } + +// // buffer the point cloud +// buffer->lock(); +// buffer->bufferCloud(cloud2); +// buffer->unlock(); +// } + +// void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message, +// const boost::shared_ptr& buffer) +// { +// // buffer the point cloud +// buffer->lock(); +// buffer->bufferCloud(*message); +// buffer->unlock(); +// } + +// void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, +// double* min_y, double* max_x, double* max_y) +// { +// if (rolling_window_) +// updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); +// useExtraBounds(min_x, min_y, max_x, max_y); + +// bool current = true; +// std::vector observations, clearing_observations; + +// // get the marking observations +// current = current && getMarkingObservations(observations); + +// // get the clearing observations +// current = current && getClearingObservations(clearing_observations); + +// // update the global current status +// current_ = current; + +// // raytrace freespace +// for (unsigned int i = 0; i < clearing_observations.size(); ++i) +// { +// raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y); +// } + +// // place the new obstacles into a priority queue... each with a priority of zero to begin with +// for (std::vector::const_iterator it = observations.begin(); it != observations.end(); ++it) +// { +// const Observation& obs = *it; + +// const sensor_msgs::PointCloud2& cloud = *(obs.cloud_); + +// double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_; + +// sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x"); +// sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); +// sensor_msgs::PointCloud2ConstIterator iter_z(cloud, "z"); + +// for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z) +// { +// double px = *iter_x, py = *iter_y, pz = *iter_z; + +// // if the obstacle is too high or too far away from the robot we won't add it +// if (pz > max_obstacle_height_) +// { +// ROS_DEBUG("The point is too high"); +// continue; +// } + +// // compute the squared distance from the hitpoint to the pointcloud's origin +// double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y) +// + (pz - obs.origin_.z) * (pz - obs.origin_.z); + +// // if the point is far enough away... we won't consider it +// if (sq_dist >= sq_obstacle_range) +// { +// ROS_DEBUG("The point is too far away"); +// continue; +// } + +// // now we need to compute the map coordinates for the observation +// unsigned int mx, my; +// if (!worldToMap(px, py, mx, my)) +// { +// ROS_DEBUG("Computing map coords failed"); +// continue; +// } + +// unsigned int index = getIndex(mx, my); +// costmap_[index] = LETHAL_OBSTACLE; +// touch(px, py, min_x, min_y, max_x, max_y); +// } +// } + +// updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); +// } + +// void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, +// double* max_x, double* max_y) +// { +// if (!footprint_clearing_enabled_) return; +// transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); + +// for (unsigned int i = 0; i < transformed_footprint_.size(); i++) +// { +// touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y); +// } +// } + +// void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) +// { +// if (footprint_clearing_enabled_) +// { +// setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE); +// } - switch (combination_method_) - { - case 0: // Overwrite - updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); - break; - case 1: // Maximum - updateWithMax(master_grid, min_i, min_j, max_i, max_j); - break; - default: // Nothing - break; - } +// switch (combination_method_) +// { +// case 0: // Overwrite +// updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); +// break; +// case 1: // Maximum +// updateWithMax(master_grid, min_i, min_j, max_i, max_j); +// break; +// default: // Nothing +// break; +// } -} +// } -void ObstacleLayer::addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing) -{ - if (marking) - static_marking_observations_.push_back(obs); - if (clearing) - static_clearing_observations_.push_back(obs); -} +// void ObstacleLayer::addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing) +// { +// if (marking) +// static_marking_observations_.push_back(obs); +// if (clearing) +// static_clearing_observations_.push_back(obs); +// } -void ObstacleLayer::clearStaticObservations(bool marking, bool clearing) -{ - if (marking) - static_marking_observations_.clear(); - if (clearing) - static_clearing_observations_.clear(); -} +// void ObstacleLayer::clearStaticObservations(bool marking, bool clearing) +// { +// if (marking) +// static_marking_observations_.clear(); +// if (clearing) +// static_clearing_observations_.clear(); +// } -bool ObstacleLayer::getMarkingObservations(std::vector& marking_observations) const -{ - bool current = true; - // get the marking observations - for (unsigned int i = 0; i < marking_buffers_.size(); ++i) - { - marking_buffers_[i]->lock(); - marking_buffers_[i]->getObservations(marking_observations); - current = marking_buffers_[i]->isCurrent() && current; - marking_buffers_[i]->unlock(); - } - marking_observations.insert(marking_observations.end(), - static_marking_observations_.begin(), static_marking_observations_.end()); - return current; -} +// bool ObstacleLayer::getMarkingObservations(std::vector& marking_observations) const +// { +// bool current = true; +// // get the marking observations +// for (unsigned int i = 0; i < marking_buffers_.size(); ++i) +// { +// marking_buffers_[i]->lock(); +// marking_buffers_[i]->getObservations(marking_observations); +// current = marking_buffers_[i]->isCurrent() && current; +// marking_buffers_[i]->unlock(); +// } +// marking_observations.insert(marking_observations.end(), +// static_marking_observations_.begin(), static_marking_observations_.end()); +// return current; +// } -bool ObstacleLayer::getClearingObservations(std::vector& clearing_observations) const -{ - bool current = true; - // get the clearing observations - for (unsigned int i = 0; i < clearing_buffers_.size(); ++i) - { - clearing_buffers_[i]->lock(); - clearing_buffers_[i]->getObservations(clearing_observations); - current = clearing_buffers_[i]->isCurrent() && current; - clearing_buffers_[i]->unlock(); - } - clearing_observations.insert(clearing_observations.end(), - static_clearing_observations_.begin(), static_clearing_observations_.end()); - return current; -} +// bool ObstacleLayer::getClearingObservations(std::vector& clearing_observations) const +// { +// bool current = true; +// // get the clearing observations +// for (unsigned int i = 0; i < clearing_buffers_.size(); ++i) +// { +// clearing_buffers_[i]->lock(); +// clearing_buffers_[i]->getObservations(clearing_observations); +// current = clearing_buffers_[i]->isCurrent() && current; +// clearing_buffers_[i]->unlock(); +// } +// clearing_observations.insert(clearing_observations.end(), +// static_clearing_observations_.begin(), static_clearing_observations_.end()); +// return current; +// } -void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y, - double* max_x, double* max_y) -{ - double ox = clearing_observation.origin_.x; - double oy = clearing_observation.origin_.y; - const sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_); +// void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y, +// double* max_x, double* max_y) +// { +// double ox = clearing_observation.origin_.x; +// double oy = clearing_observation.origin_.y; +// const sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_); - // get the map coordinates of the origin of the sensor - unsigned int x0, y0; - if (!worldToMap(ox, oy, x0, y0)) - { - ROS_WARN_THROTTLE( - 1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.", - ox, oy); - return; - } +// // get the map coordinates of the origin of the sensor +// unsigned int x0, y0; +// if (!worldToMap(ox, oy, x0, y0)) +// { +// ROS_WARN_THROTTLE( +// 1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.", +// ox, oy); +// return; +// } - // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later - double origin_x = origin_x_, origin_y = origin_y_; - double map_end_x = origin_x + size_x_ * resolution_; - double map_end_y = origin_y + size_y_ * resolution_; +// // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later +// double origin_x = origin_x_, origin_y = origin_y_; +// double map_end_x = origin_x + size_x_ * resolution_; +// double map_end_y = origin_y + size_y_ * resolution_; - touch(ox, oy, min_x, min_y, max_x, max_y); +// touch(ox, oy, min_x, min_y, max_x, max_y); - // for each point in the cloud, we want to trace a line from the origin and clear obstacles along it - sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x"); - sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); +// // for each point in the cloud, we want to trace a line from the origin and clear obstacles along it +// sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x"); +// sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); - for (; iter_x != iter_x.end(); ++iter_x, ++iter_y) - { - double wx = *iter_x; - double wy = *iter_y; +// for (; iter_x != iter_x.end(); ++iter_x, ++iter_y) +// { +// double wx = *iter_x; +// double wy = *iter_y; - // now we also need to make sure that the enpoint we're raytracing - // to isn't off the costmap and scale if necessary - double a = wx - ox; - double b = wy - oy; +// // now we also need to make sure that the enpoint we're raytracing +// // to isn't off the costmap and scale if necessary +// double a = wx - ox; +// double b = wy - oy; - // the minimum value to raytrace from is the origin - if (wx < origin_x) - { - double t = (origin_x - ox) / a; - wx = origin_x; - wy = oy + b * t; - } - if (wy < origin_y) - { - double t = (origin_y - oy) / b; - wx = ox + a * t; - wy = origin_y; - } +// // the minimum value to raytrace from is the origin +// if (wx < origin_x) +// { +// double t = (origin_x - ox) / a; +// wx = origin_x; +// wy = oy + b * t; +// } +// if (wy < origin_y) +// { +// double t = (origin_y - oy) / b; +// wx = ox + a * t; +// wy = origin_y; +// } - // the maximum value to raytrace to is the end of the map - if (wx > map_end_x) - { - double t = (map_end_x - ox) / a; - wx = map_end_x - .001; - wy = oy + b * t; - } - if (wy > map_end_y) - { - double t = (map_end_y - oy) / b; - wx = ox + a * t; - wy = map_end_y - .001; - } +// // the maximum value to raytrace to is the end of the map +// if (wx > map_end_x) +// { +// double t = (map_end_x - ox) / a; +// wx = map_end_x - .001; +// wy = oy + b * t; +// } +// if (wy > map_end_y) +// { +// double t = (map_end_y - oy) / b; +// wx = ox + a * t; +// wy = map_end_y - .001; +// } - // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint - unsigned int x1, y1; +// // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint +// unsigned int x1, y1; - // check for legality just in case - if (!worldToMap(wx, wy, x1, y1)) - continue; +// // check for legality just in case +// if (!worldToMap(wx, wy, x1, y1)) +// continue; - unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_); - MarkCell marker(costmap_, FREE_SPACE); - // and finally... we can execute our trace to clear obstacles along that line - raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); +// unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_); +// MarkCell marker(costmap_, FREE_SPACE); +// // and finally... we can execute our trace to clear obstacles along that line +// raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); - updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y); - } -} +// updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y); +// } +// } -void ObstacleLayer::activate() -{ - // if we're stopped we need to re-subscribe to topics - for (unsigned int i = 0; i < observation_subscribers_.size(); ++i) - { - if (observation_subscribers_[i] != NULL) - observation_subscribers_[i]->subscribe(); - } +// void ObstacleLayer::activate() +// { +// // if we're stopped we need to re-subscribe to topics +// for (unsigned int i = 0; i < observation_subscribers_.size(); ++i) +// { +// if (observation_subscribers_[i] != NULL) +// observation_subscribers_[i]->subscribe(); +// } - for (unsigned int i = 0; i < observation_buffers_.size(); ++i) - { - if (observation_buffers_[i]) - observation_buffers_[i]->resetLastUpdated(); - } -} -void ObstacleLayer::deactivate() -{ - for (unsigned int i = 0; i < observation_subscribers_.size(); ++i) - { - if (observation_subscribers_[i] != NULL) - observation_subscribers_[i]->unsubscribe(); - } -} +// for (unsigned int i = 0; i < observation_buffers_.size(); ++i) +// { +// if (observation_buffers_[i]) +// observation_buffers_[i]->resetLastUpdated(); +// } +// } +// void ObstacleLayer::deactivate() +// { +// for (unsigned int i = 0; i < observation_subscribers_.size(); ++i) +// { +// if (observation_subscribers_[i] != NULL) +// observation_subscribers_[i]->unsubscribe(); +// } +// } -void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, - double* min_x, double* min_y, double* max_x, double* max_y) -{ - double dx = wx-ox, dy = wy-oy; - double full_distance = hypot(dx, dy); - double scale = std::min(1.0, range / full_distance); - double ex = ox + dx * scale, ey = oy + dy * scale; - touch(ex, ey, min_x, min_y, max_x, max_y); -} +// void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, +// double* min_x, double* min_y, double* max_x, double* max_y) +// { +// double dx = wx-ox, dy = wy-oy; +// double full_distance = hypot(dx, dy); +// double scale = std::min(1.0, range / full_distance); +// double ex = ox + dx * scale, ey = oy + dy * scale; +// touch(ex, ey, min_x, min_y, max_x, max_y); +// } -void ObstacleLayer::reset() -{ - deactivate(); - resetMaps(); - current_ = true; - activate(); -} +// void ObstacleLayer::reset() +// { +// deactivate(); +// resetMaps(); +// current_ = true; +// activate(); +// } -} // namespace costmap_2d +// } // namespace costmap_2d diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp index 2b633dd..dbf930d 100644 --- a/plugins/static_layer.cpp +++ b/plugins/static_layer.cpp @@ -1,48 +1,10 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2008, 2013, Willow Garage, Inc. - * Copyright (c) 2015, Fetch Robotics, 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!! - *********************************************************************/ + #include #include -#include #include -#include - -PLUGINLIB_EXPORT_CLASS(costmap_2d::StaticLayer, costmap_2d::Layer) +#include +// #include +#include using costmap_2d::NO_INFORMATION; using costmap_2d::LETHAL_OBSTACLE; @@ -51,92 +13,78 @@ using costmap_2d::FREE_SPACE; namespace costmap_2d { -StaticLayer::StaticLayer() : dsrv_(NULL) +StaticLayer::StaticLayer() { threshold_ = &lethal_threshold_; } StaticLayer::~StaticLayer() -{ - if (dsrv_) - delete dsrv_; -} +{} void StaticLayer::onInitialize() { - ros::NodeHandle nh("~/" + name_), g_nh; + // ros::NodeHandle nh("~/" + name_), g_nh; current_ = true; global_frame_ = layered_costmap_->getGlobalFrameID(); - nh.param("map_topic", map_topic_, std::string("map")); - nh.param("first_map_only", first_map_only_, false); - nh.param("subscribe_to_updates", subscribe_to_updates_, false); + // nh.param("map_topic", map_topic_, std::string("map")); + // nh.param("first_map_only", first_map_only_, false); + // nh.param("subscribe_to_updates", subscribe_to_updates_, false); - nh.param("track_unknown_space", track_unknown_space_, true); - nh.param("use_maximum", use_maximum_, false); + // nh.param("track_unknown_space", track_unknown_space_, true); + // nh.param("use_maximum", use_maximum_, false); - int temp_lethal_threshold, temp_unknown_cost_value; - nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100)); - nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1)); - nh.param("trinary_costmap", trinary_costmap_, true); - nh.param("base_frame_id", base_frame_id_, std::string("base_footprint")); + // int temp_lethal_threshold, temp_unknown_cost_value; + // nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100)); + // nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1)); + // nh.param("trinary_costmap", trinary_costmap_, true); + // nh.param("base_frame_id", base_frame_id_, std::string("base_footprint")); - // ROS_WARN("global_frame_[%s] map_frame_[%s] map_topic[%s] first_map_only[%d] subscribe_to_updates[%d] track_unknown_space[%d] use_maximum[%d] \n lethal_cost_threshold[%d] unknown_cost_value[%d] trinary_costmap[%d]", - // global_frame_.c_str(), map_frame_.c_str(),map_topic_.c_str(), first_map_only_, subscribe_to_updates_, track_unknown_space_, use_maximum_, temp_lethal_threshold, temp_unknown_cost_value, trinary_costmap_); - lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); - unknown_cost_value_ = temp_unknown_cost_value; + // lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); + // unknown_cost_value_ = temp_unknown_cost_value; // Only resubscribe if topic has changed - if (map_sub_.getTopic() != ros::names::resolve(map_topic_)) - { - // we'll subscribe to the latched topic that the map server uses - ROS_INFO("Requesting the map..."); - map_sub_ = g_nh.subscribe(map_topic_, 1, &StaticLayer::incomingMap, this); - map_received_ = false; - has_updated_data_ = false; + // if (map_sub_.getTopic() != ros::names::resolve(map_topic_)) + // { + // // we'll subscribe to the latched topic that the map server uses + // printf("Requesting the map..."); + // map_sub_ = g_nh.subscribe(map_topic_, 1, &StaticLayer::incomingMap, this); + // map_received_ = false; + // has_updated_data_ = false; - ros::Rate r(10); - while (!map_received_ && g_nh.ok()) - { - ros::spinOnce(); - r.sleep(); - } + // ros::Rate r(10); + // while (!map_received_ && g_nh.ok()) + // { + // ros::spinOnce(); + // r.sleep(); + // } - ROS_INFO("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution()); + // printf("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution()); - if (subscribe_to_updates_) - { - ROS_INFO("Subscribing to updates"); - map_update_sub_ = g_nh.subscribe(map_topic_ + "_updates", 10, &StaticLayer::incomingUpdate, this); - } - } - else - { - has_updated_data_ = true; - } + // if (subscribe_to_updates_) + // { + // printf("Subscribing to updates"); + // map_update_sub_ = g_nh.subscribe(map_topic_ + "_updates", 10, &StaticLayer::incomingUpdate, this); + // } + // } + // else + // { + // has_updated_data_ = true; + // } - if (dsrv_) - { - delete dsrv_; - } - - dsrv_ = new dynamic_reconfigure::Server(nh); - dynamic_reconfigure::Server::CallbackType cb = - [this](auto& config, auto level){ reconfigureCB(config, level); }; - dsrv_->setCallback(cb); } -void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level) -{ - if (config.enabled != enabled_) - { - enabled_ = config.enabled; - has_updated_data_ = true; - x_ = y_ = 0; - width_ = size_x_; - height_ = size_y_; - } -} +// void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level) +// { +// if (config.enabled != enabled_) +// { +// enabled_ = config.enabled; +// has_updated_data_ = true; +// x_ = y_ = 0; +// width_ = size_x_; +// height_ = size_y_; +// } +// } void StaticLayer::matchSize() { @@ -166,36 +114,36 @@ unsigned char StaticLayer::interpretValue(unsigned char value) return scale * LETHAL_OBSTACLE; } -void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map) +void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map) { - unsigned int size_x = new_map->info.width, size_y = new_map->info.height; + unsigned int size_x = new_map.info.width, size_y = new_map.info.height; - ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution); + printf("Received a %d X %d map at %f m/pix", size_x, size_y, new_map.info.resolution); // resize costmap if size, resolution or origin do not match Costmap2D* master = layered_costmap_->getCostmap(); if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x || master->getSizeInCellsY() != size_y || - master->getResolution() != new_map->info.resolution || - master->getOriginX() != new_map->info.origin.position.x || - master->getOriginY() != new_map->info.origin.position.y)) + master->getResolution() != new_map.info.resolution || + master->getOriginX() != new_map.info.origin.position.x || + master->getOriginY() != new_map.info.origin.position.y)) { // Update the size of the layered costmap (and all layers, including this one) - ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution); - layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x, - new_map->info.origin.position.y, + printf("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map.info.resolution); + layered_costmap_->resizeMap(size_x, size_y, new_map.info.resolution, new_map.info.origin.position.x, + new_map.info.origin.position.y, true /* set size_locked to true, prevents reconfigureCb from overriding map size*/); } else if (size_x_ != size_x || size_y_ != size_y || - resolution_ != new_map->info.resolution || - origin_x_ != new_map->info.origin.position.x || - origin_y_ != new_map->info.origin.position.y) + resolution_ != new_map.info.resolution || + origin_x_ != new_map.info.origin.position.x || + origin_y_ != new_map.info.origin.position.y) { // only update the size of the costmap stored locally in this layer - ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution); - resizeMap(size_x, size_y, new_map->info.resolution, - new_map->info.origin.position.x, new_map->info.origin.position.y); + printf("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map.info.resolution); + resizeMap(size_x, size_y, new_map.info.resolution, + new_map.info.origin.position.x, new_map.info.origin.position.y); } unsigned int index = 0; @@ -205,12 +153,12 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map) { for (unsigned int j = 0; j < size_x; ++j) { - unsigned char value = new_map->data[index]; + unsigned char value = new_map.data[index]; costmap_[index] = interpretValue(value); ++index; } } - map_frame_ = new_map->header.frame_id; + map_frame_ = new_map.header.frame_id; // we have a new map, update full size of map x_ = y_ = 0; @@ -222,27 +170,27 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map) // shutdown the map subscrber if firt_map_only_ flag is on if (first_map_only_) { - ROS_INFO("Shutting down the map subscriber. first_map_only flag is on"); - map_sub_.shutdown(); + printf("Shutting down the map subscriber. first_map_only flag is on"); + // map_sub_.shutdown(); } } -void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update) +void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdate& update) { unsigned int di = 0; - for (unsigned int y = 0; y < update->height ; y++) + for (unsigned int y = 0; y < update.height ; y++) { - unsigned int index_base = (update->y + y) * size_x_; - for (unsigned int x = 0; x < update->width ; x++) + unsigned int index_base = (update.y + y) * size_x_; + for (unsigned int x = 0; x < update.width ; x++) { - unsigned int index = index_base + x + update->x; - costmap_[index] = interpretValue(update->data[di++]); + unsigned int index = index_base + x + update.x; + costmap_[index] = interpretValue(update.data[di++]); } } - x_ = update->x; - y_ = update->y; - width_ = update->width; - height_ = update->height; + x_ = update.x; + y_ = update.y; + width_ = update.width; + height_ = update.height; has_updated_data_ = true; } @@ -251,12 +199,12 @@ void StaticLayer::activate() onInitialize(); } -void StaticLayer::deactivate() -{ - map_sub_.shutdown(); - if (subscribe_to_updates_) - map_update_sub_.shutdown(); -} +// void StaticLayer::deactivate() +// { +// map_sub_.shutdown(); +// if (subscribe_to_updates_) +// map_update_sub_.shutdown(); +// } void StaticLayer::reset() { @@ -314,18 +262,23 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int double wx, wy; // Might even be in a different frame geometry_msgs::TransformStamped transform; + tf2::TransformStampedMsg transformMsg; try { - transform = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0)); + // transformMsg = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0)); + tf_->canTransform(map_frame_, global_frame_, tf2::Time()); + tf_->lookupTransform(map_frame_, + global_frame_, + tf2::Time()); } catch (tf2::TransformException ex) { - ROS_ERROR("%s", ex.what()); + printf("%s", ex.what()); return; } // Copy map data given proper transformations tf2::Transform tf2_transform; - tf2::convert(transform.transform, tf2_transform); + // tf2::convert(transform.transform, tf2_transform); for (unsigned int i = min_i; i < max_i; ++i) { for (unsigned int j = min_j; j < max_j; ++j) @@ -334,7 +287,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy); // Transform from global_frame_ to map_frame_ tf2::Vector3 p(wx, wy, 0); - p = tf2_transform*p; + // p = tf2_transform*p; // Set master_grid with cell from map if (worldToMap(p.x(), p.y(), mx, my)) { @@ -348,4 +301,12 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int } } +// Export factory function +static PluginPtr create_static_plugin() { + return std::make_shared(); +} + +// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) +BOOST_DLL_ALIAS(create_static_plugin, create_plugin) + } // namespace costmap_2d diff --git a/src/layer.cpp b/src/layer.cpp index 06ea46b..dd8d1ea 100644 --- a/src/layer.cpp +++ b/src/layer.cpp @@ -11,7 +11,7 @@ Layer::Layer() , tf_(NULL) {} -void Layer::initialize(LayeredCostmap* parent, std::string name, std::shared_ptr *tf) +void Layer::initialize(LayeredCostmap* parent, std::string name, tf2::BufferCore *tf) { layered_costmap_ = parent; name_ = name; diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp index 5d57ea0..97ff080 100644 --- a/src/observation_buffer.cpp +++ b/src/observation_buffer.cpp @@ -2,7 +2,6 @@ // #include // #include -// #include #include #include #include