Feature/dogm layer

.gitmodules

@@ -53,3 +53,6 @@
 [submodule "ros2_ws/src/algorithms/mapping/grid_map"]
 	path = ros2_ws/src/algorithms/mapping/grid_map
 	url = https://github.com/ANYbotics/grid_map.git
+[submodule "ros2_ws/src/algorithms/dynamic_obstacles/dogm"]
+	path = ros2_ws/src/algorithms/dynamic_obstacles/dogm
+	url = https://github.com/andrey1908/dogm.git

docker/Dockerfile

@@ -42,7 +42,6 @@ RUN if [ "$USE_CUDA" = "ON" ]; then \
     rm cuda.sh 
 
 # OpenCV
-
 COPY scripts/opencv.sh .
 RUN if [ "$USE_OPENCV" = "ON" ]; then \
       sh ./opencv.sh \
@@ -195,6 +194,14 @@ RUN apt-get update && \
         iputils-ping && \
         rm -rf /var/lib/apt/lists/*
 
+# Dependencies for dogm
+RUN apt-get update && \
+    apt-get install -y \
+        libglfw3-dev \
+        libglew-dev \
+        libglm-dev && \
+    rm -rf /var/lib/apt/lists/*
+
 RUN apt-get update && \
       apt-get upgrade -y && \
         rm -rf /var/lib/apt/lists/*

docker/env-gazebo.sh

@@ -9,7 +9,7 @@ export USE_GAZEBO=ON
 
 export USE_CUDA=ON
 export USE_TORCH=OFF
-export USE_OPENCV=OFF
+export USE_OPENCV=ON
 
 export USE_REALSENSE=OFF
 export USE_OAKD=OFF

docker/env-robot.sh

@@ -7,9 +7,9 @@ export USE_ROS1=ON
 export USE_ROS2=ON
 export USE_GAZEBO=OFF
 
-export USE_CUDA=OFF
+export USE_CUDA=ON
 export USE_TORCH=OFF
-export USE_OPENCV=OFF
+export USE_OPENCV=ON
 
 export USE_REALSENSE=ON
 export USE_OAKD=OFF

docker/env-universal.sh

@@ -9,7 +9,7 @@ export USE_GAZEBO=ON
 
 export USE_CUDA=ON
 export USE_TORCH=OFF
-export USE_OPENCV=OFF
+export USE_OPENCV=ON
 
 export USE_REALSENSE=ON
 export USE_OAKD=OFF

docker/scripts/cuda.sh

@@ -19,6 +19,7 @@ sudo apt-get update
 
 sudo apt-get install -y --no-install-recommends \
      cuda-cudart-11-2=11.2.152-1 \
+     cuda-cudart-dev-11-2=11.2.152-1 \
      cuda-compat-11-2 \
      && ln -s cuda-11.2 /usr/local/cuda
 
@@ -28,21 +29,27 @@ sudo echo "/usr/local/nvidia/lib64" >> /etc/ld.so.conf.d/nvidia.conf
 
 sudo apt-get install -y --no-install-recommends \
      cuda-libraries-11-2=11.2.2-1 \
+     cuda-libraries-dev-11-2=11.2.2-1 \
      libnpp-11-2=11.3.2.152-1 \
+     libnpp-dev-11-2=11.3.2.152-1 \
      cuda-nvtx-11-2=11.2.152-1 \
      libcublas-11-2=11.4.1.1043-1 \
+     libcublas-dev-11-2=11.4.1.1043-1 \
      libcusparse-11-2=11.4.1.1152-1 \
-     libnccl2=$NCCL_VERSION-1+cuda11.2 
+     libcusparse-dev-11-2=11.4.1.1152-1 \
+     libnccl2=$NCCL_VERSION-1+cuda11.2 \
+     cuda-nvcc-11-2
 
 # apt from auto upgrading the cublas package. See https://gitlab.com/nvidia/container-images/cuda/-/issues/88
-sudo apt-mark hold libcublas-11-2 libnccl2
+sudo apt-mark hold libcublas-11-2 libcublas-dev-11-2 libnccl2
 
 
 ##########
 # CUDDNN #
 ##########
 sudo apt-get install -y --no-install-recommends \
     libcudnn8=$CUDNN_VERSION-1+cuda11.2 \
+    libcudnn8-dev=$CUDNN_VERSION-1+cuda11.2 \
     && apt-mark hold libcudnn8 
 
 sudo rm -rf /var/lib/apt/lists/*

docker/scripts/opencv.sh

@@ -1,14 +1,57 @@
 #!/bin/bash
 
-sudo su $ROSUSER
-# opencv with Gstreamer
-cd /home/$ROSUSER
-git clone https://github.com/opencv/opencv.git -b 4.5.0
-git clone https://github.com/opencv/opencv_contrib.git -b 4.5.0
-mkdir opencv/build
+# requirements
+sudo apt-get update
+sudo apt-get install -y build-essential cmake pkg-config unzip yasm git checkinstall
+sudo apt-get install -y libjpeg-dev libpng-dev libtiff-dev
+sudo apt-get install -y libgtk-3-dev
+sudo apt-get install -y libtbb-dev
+sudo apt-get install -y libatlas-base-dev gfortran
+
+# opencv with Gstreamer and CUDA
+cd /usr/local/src
+sudo git clone https://github.com/opencv/opencv.git -b 4.5.2
+sudo git clone https://github.com/opencv/opencv_contrib.git -b 4.5.2
+sudo mkdir opencv/build
 cd opencv/build
-cmake -DOPENCV_EXTRA_MODULES_PATH=../../opencv_contrib/modules -DWITH_GSTREAMER=ON -DBUILD_opencv_python3=ON ..
-make -j4
+sudo cmake \
+    -D CMAKE_BUILD_TYPE=RELEASE \
+    -D CMAKE_INSTALL_PREFIX=/usr/local/opencv4.5+cuda \
+    -D WITH_TBB=ON \
+    -D ENABLE_FAST_MATH=1 \
+    -D CUDA_FAST_MATH=1 \
+    -D WITH_CUBLAS=1 \
+    -D WITH_CUDA=ON \
+    -D BUILD_opencv_cudacodec=OFF \
+    -D WITH_V4L=ON \
+    -D WITH_QT=OFF \
+    -D WITH_OPENGL=ON \
+    -D WITH_GSTREAMER=ON \
+    -D OPENCV_GENERATE_PKGCONFIG=ON \
+    -D OPENCV_PC_FILE_NAME=opencv.pc \
+    -D OPENCV_ENABLE_NONFREE=ON \
+    -D BUILD_opencv_python3=ON \
+    -D OPENCV_EXTRA_MODULES_PATH=../../opencv_contrib/modules \
+    -D INSTALL_PYTHON_EXAMPLES=OFF \
+    -D INSTALL_C_EXAMPLES=OFF \
+    -D BUILD_EXAMPLES=OFF ..
+sudo make -j4
 sudo make install
 
-sudo su root
+# create bash script to activate opencv 4.5
+# echo "\
+# export PATH=\"/usr/local/opencv4.5+cuda/bin:\$PATH\" \n\
+# export PKG_CONFIG_PATH=\"/usr/local/opencv4.5+cuda/lib/pkgconfig:\$PKG_CONFIG_PATH\" \n\
+# export LD_LIBRARY_PATH=\"/usr/local/opencv4.5+cuda/lib:\$LD_LIBRARY_PATH\"" > /home/${ROSUSER}/activate_opencv4.5.bash
+
+# add opencv 4.5 libraries to ldconfig
+echo /usr/local/opencv4.5+cuda/lib > /home/${ROSUSER}/opencv4.5+cuda.conf
+sudo mv /home/${ROSUSER}/opencv4.5+cuda.conf /etc/ld.so.conf.d
+sudo ldconfig
+
+# modify .zshrc to activate opencv 4.5
+# echo "\n\
+# export PATH=\"/usr/local/opencv4.5+cuda/bin:\$PATH\" \n\
+# export PKG_CONFIG_PATH=\"/usr/local/opencv4.5+cuda/lib/pkgconfig:\$PKG_CONFIG_PATH\" \n\
+# export LD_LIBRARY_PATH=\"/usr/local/opencv4.5+cuda/lib:\$LD_LIBRARY_PATH\"" >> /home/${ROSUSER}/.zshrc
+

ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/CMakeLists.txt

@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 3.5)
+project(dogm_msgs)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+# create custom messages
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/GridCell.msg"
+  "msg/GridMapInfo.msg"
+  "msg/DynamicOccupancyGrid.msg"
+  DEPENDENCIES std_msgs geometry_msgs
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/msg/DynamicOccupancyGrid.msg

@@ -0,0 +1,8 @@
+# Header (time and frame)
+std_msgs/Header header
+
+# Grid map info
+GridMapInfo info
+
+# Grid map data
+GridCell[] data

ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/msg/GridCell.msg

@@ -0,0 +1,20 @@
+# Free mass
+float32 free_mass
+
+# Occupied mass
+float32 occ_mass
+
+# Mean velocity in x direction
+float32 mean_x_vel
+
+# Mean velocity in y direction
+float32 mean_y_vel
+
+# Velocity variance in x direction
+float32 var_x_vel
+
+# Velocity variance in y direction
+float32 var_y_vel
+
+# Covariance of x and y
+float32 covar_xy_vel

ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/msg/GridMapInfo.msg

@@ -0,0 +1,11 @@
+# The map resolution [m/cell]
+float32 resolution
+
+# Map length [m]
+float32 length
+
+# Map size [cells]
+int32 size
+
+# Pose of the grid map center [m]
+geometry_msgs/Pose pose

ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>dogm_msgs</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="user@todo.todo">user</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/CMakeLists.txt

@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 3.5)
+project(dogm_plugin LANGUAGES CXX CUDA)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(OpenCV 4.5.2 EXACT REQUIRED)
+find_package(dogm REQUIRED)
+find_package(dogm_msgs REQUIRED)
+
+# build
+add_library(${PROJECT_NAME} SHARED src/dogm_layer.cu)
+include_directories(include ${OpenCV_INCLUDE_DIRS} ${DOGM_INCLUDE_DIRS})
+target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ${DOGM_LIBRARIES})
+ament_target_dependencies(${PROJECT_NAME} rclcpp nav2_costmap_2d pluginlib dogm_msgs)
+
+#install
+install(TARGETS ${PROJECT_NAME} DESTINATION lib)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+pluginlib_export_plugin_description_file(nav2_costmap_2d dogm_plugin.xml)
+ament_package()