diff --git a/.gitmodules b/.gitmodules index 6d69f62..f2e2a05 100755 --- a/.gitmodules +++ b/.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 diff --git a/docker/Dockerfile b/docker/Dockerfile index f1e1d2b..e580503 100755 --- a/docker/Dockerfile +++ b/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/* diff --git a/docker/env-gazebo.sh b/docker/env-gazebo.sh index 79c67db..29b8601 100755 --- a/docker/env-gazebo.sh +++ b/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 diff --git a/docker/env-robot.sh b/docker/env-robot.sh index 9094944..d7056a7 100755 --- a/docker/env-robot.sh +++ b/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 diff --git a/docker/env-universal.sh b/docker/env-universal.sh index 6d67cb3..623764f 100755 --- a/docker/env-universal.sh +++ b/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 diff --git a/docker/scripts/cuda.sh b/docker/scripts/cuda.sh index db51911..781e08f 100755 --- a/docker/scripts/cuda.sh +++ b/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,14 +29,19 @@ 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 ########## @@ -43,6 +49,7 @@ sudo apt-mark hold libcublas-11-2 libnccl2 ########## 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/* diff --git a/docker/scripts/opencv.sh b/docker/scripts/opencv.sh index f9329f4..5a51318 100755 --- a/docker/scripts/opencv.sh +++ b/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 + diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm b/ros2_ws/src/algorithms/dynamic_obstacles/dogm new file mode 160000 index 0000000..132ba01 --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm @@ -0,0 +1 @@ +Subproject commit 132ba01361bebe1ce4f676d1637d5b8cf99e4ca3 diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/CMakeLists.txt b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/CMakeLists.txt new file mode 100644 index 0000000..4d730d1 --- /dev/null +++ b/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( 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() diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/msg/DynamicOccupancyGrid.msg b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/msg/DynamicOccupancyGrid.msg new file mode 100644 index 0000000..70990b9 --- /dev/null +++ b/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 diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/msg/GridCell.msg b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/msg/GridCell.msg new file mode 100644 index 0000000..afc9a0f --- /dev/null +++ b/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 \ No newline at end of file diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/msg/GridMapInfo.msg b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/msg/GridMapInfo.msg new file mode 100644 index 0000000..6ea57d1 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/package.xml b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/package.xml new file mode 100644 index 0000000..a77ed07 --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/package.xml @@ -0,0 +1,25 @@ + + + + dogm_msgs + 0.0.0 + TODO: Package description + user + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + std_msgs + geometry_msgs + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/CMakeLists.txt b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/CMakeLists.txt new file mode 100644 index 0000000..9e1ef76 --- /dev/null +++ b/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() diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/README.md b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/README.md new file mode 100644 index 0000000..ae934fe --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/README.md @@ -0,0 +1,104 @@ +# DOGM plugin + +## Overview + +DOGM plugin to build dynamic occupancy map. + +## Configuration + +Parameters for DOGM are described [here](https://docs.google.com/document/d/13jvR1O_fP9Rz0Z_3aojrFFGzsZLsNm-Wte2rHdSkeFQ/edit?usp=sharing) in section "Настройка параметров DOGM". + +There are also some parameters that are not listed in the doc above. Here they are. + +| Parameter | Type | Definition | +|--------------------------------|-------|---------------------------------------------------------| +| opencv_visualization | bool | Whether to use opencv visualization for dynamic map | +| motion_compensation | bool | Whether to use motion compensation | +| normalized_occupancy_threshold | float | Threshold for occupied state (occupancy is from 0 to 1) | + +Example fully-described XML with default parameter values: + +``` +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 5 + height: 5 + resolution: 0.05 + footprint: '[[0.14, 0.16], [0.14, -0.16], [-0.14, -0.16], [-0.14, 0.16]]' + plugins: ["voxel_layer", "inflation_layer", "dogm_layer"] + + dogm_layer: + plugin: "dogm_plugin/DogmLayer" + enabled: True + opencv_visualization: True + motion_compensation: False + size: 5.0 + resolution: 0.2 + particle_count: 3000000 + new_born_particle_count: 300000 + persistence_prob: 0.99 + stddev_process_noise_position: 0.1 + stddev_process_noise_velocity: 1.0 + birth_prob: 0.02 + stddev_velocity: 30.0 + init_max_velocity: 30.0 + normalized_occupancy_threshold: 0.5 + + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.75 + + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: point_cloud scan + point_cloud: + topic: /camera/points + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "PointCloud2" + raytrace_max_range: 2.0 + raytrace_min_range: 0.0 + obstacle_max_range: 1.5 + obstacle_min_range: 0.0 + min_obstacle_height: 0.1 + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True +``` + +## Topics + +| Topic | Type | Description | +|------------|----------------------------------|------------------------------| +| `dogm_map` | `dogm_msgs/DynamicOccupancyGrid` | Output dynamic occupancy map | diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/dogm_plugin.xml b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/dogm_plugin.xml new file mode 100644 index 0000000..79108fb --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/dogm_plugin.xml @@ -0,0 +1,5 @@ + + + Dogm plugin + + \ No newline at end of file diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/include/dogm_plugin/dogm_layer.h b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/include/dogm_plugin/dogm_layer.h new file mode 100644 index 0000000..aa816a5 --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/include/dogm_plugin/dogm_layer.h @@ -0,0 +1,82 @@ +#ifndef DOGM_LAYER_HPP_ +#define DOGM_LAYER_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/layer.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include +#include +#include +#include +#include +#include "dogm_msgs/msg/dynamic_occupancy_grid.hpp" +#include "std_msgs/msg/string.hpp" + +namespace dogm_plugin { + +class DogmLayer : public nav2_costmap_2d::Layer { +public: + DogmLayer(); + ~DogmLayer(); + + virtual void onInitialize(); + virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, + double* min_x, double* min_y, + double* max_x, double* max_y); + virtual void updateCosts(nav2_costmap_2d::Costmap2D& master_grid, + int min_i, int min_j, int max_i, int max_j); + + /** + * @brief Converts costmap to measurement grid to feed it to DOGM. + */ + void costMapToMeasurementGrid(nav2_costmap_2d::Costmap2D& master_grid, + int min_i, int min_j, int max_i, int max_j); + + /** + * @brief Converts dynamic map to custom ROS message and sends it. + */ + void publishDynamicGrid(); + virtual void reset(); + +private: + /** + * @brief Computes transform from measurement grid (dynamic grid) coordinate system to costmap coordinate system. + */ + cv::Mat getTransformFromMeasurementGridToCostMap(nav2_costmap_2d::Costmap2D& master_grid, + int min_i, int min_j, int max_i, int max_j); + /** + * @brief Gets data from master grid and transfers it to GPU. + */ + cv::cuda::GpuMat getMasterArrayDevice(nav2_costmap_2d::Costmap2D& master_grid, + int min_i, int min_j, int max_i, int max_j); + /** + * @brief Sets unknown cells in master grid on GPU as free. + */ + void setUnknownAsFree(cv::cuda::GpuMat master_array_device); + /** + * @brief Transforms master grid to measurement grid (dynamic grid) system. + */ + cv::cuda::GpuMat transformCostMapToMeasurementGrid(cv::cuda::GpuMat master_array_device, cv::Mat measurement_grid_to_costmap); + /** + * @brief Fills measurement grid to feed it to DOGM. + */ + void fillMeasurementGrid(cv::cuda::GpuMat measurement_grid_device); + +private: + bool opencv_visualization_; + bool motion_compensation_; + dogm::DOGM::Params params_; + float normalized_occupancy_threshold_; + std::unique_ptr dogm_map_; + dogm::MeasurementCell* measurement_grid_; + std::shared_ptr> publisher_; + + double robot_x_; + double robot_y_; + bool is_first_measurement_; + rclcpp::Time last_time_stamp_; +}; + +} // namespace dogm_plugin + +#endif // DOGM_LAYER_HPP_ \ No newline at end of file diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/package.xml b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/package.xml new file mode 100644 index 0000000..8e3f93f --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/package.xml @@ -0,0 +1,25 @@ + + + + dogm_plugin + 0.0.0 + TODO: Package description + user + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + nav2_costmap_2d + pluginlib + dogm + dogm_msgs + + + + ament_cmake + + diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/src/dogm_layer.cu b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/src/dogm_layer.cu new file mode 100644 index 0000000..1a39a88 --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/src/dogm_layer.cu @@ -0,0 +1,281 @@ +#include "dogm_plugin/dogm_layer.h" +#include +#include + +namespace dogm_plugin { + +__global__ void setUnknownAsFreeKernel(cv::cuda::PtrStepSzb master_array); +__global__ void fillMeasurementGridKernel(dogm::MeasurementCell* __restrict__ measurement_grid, const cv::cuda::PtrStepSzi source, + float occupancy_threshold); + +DogmLayer::DogmLayer() {} + +void DogmLayer::onInitialize() { + declareParameter("enabled", rclcpp::ParameterValue(true)); + node_->get_parameter(name_ + "." + "enabled", enabled_); + + declareParameter("opencv_visualization", rclcpp::ParameterValue(false)); + node_->get_parameter(name_ + "." + "opencv_visualization", opencv_visualization_); + declareParameter("motion_compensation", rclcpp::ParameterValue(true)); + node_->get_parameter(name_ + "." + "motion_compensation", motion_compensation_); + + declareParameter("size", rclcpp::ParameterValue(5.0f)); + node_->get_parameter(name_ + "." + "size", params_.size); + declareParameter("resolution", rclcpp::ParameterValue(0.2f)); + node_->get_parameter(name_ + "." + "resolution", params_.resolution); + declareParameter("particle_count", rclcpp::ParameterValue(3 * static_cast(1e6))); + node_->get_parameter(name_ + "." + "particle_count", params_.particle_count); + declareParameter("new_born_particle_count", rclcpp::ParameterValue(3 * static_cast(1e5))); + node_->get_parameter(name_ + "." + "new_born_particle_count", params_.new_born_particle_count); + declareParameter("persistence_prob", rclcpp::ParameterValue(0.99f)); + node_->get_parameter(name_ + "." + "persistence_prob", params_.persistence_prob); + declareParameter("stddev_process_noise_position", rclcpp::ParameterValue(0.1f)); + node_->get_parameter(name_ + "." + "stddev_process_noise_position", params_.stddev_process_noise_position); + declareParameter("stddev_process_noise_velocity", rclcpp::ParameterValue(1.0f)); + node_->get_parameter(name_ + "." + "stddev_process_noise_velocity", params_.stddev_process_noise_velocity); + declareParameter("birth_prob", rclcpp::ParameterValue(0.02f)); + node_->get_parameter(name_ + "." + "birth_prob", params_.birth_prob); + declareParameter("stddev_velocity", rclcpp::ParameterValue(30.0f)); + node_->get_parameter(name_ + "." + "stddev_velocity", params_.stddev_velocity); + declareParameter("init_max_velocity", rclcpp::ParameterValue(30.0f)); + node_->get_parameter(name_ + "." + "init_max_velocity", params_.init_max_velocity); + + declareParameter("normalized_occupancy_threshold", rclcpp::ParameterValue(0.5f)); + node_->get_parameter(name_ + "." + "normalized_occupancy_threshold", normalized_occupancy_threshold_); + + dogm_map_ = std::make_unique(params_); + CHECK_ERROR(cudaMalloc(&measurement_grid_, dogm_map_->grid_cell_count * sizeof(dogm::MeasurementCell))); + + robot_x_ = 0; + robot_y_ = 0; + is_first_measurement_ = true; + + publisher_ = node_->create_publisher("dogm_map", 10); + publisher_->on_activate(); +} + +DogmLayer::~DogmLayer() { + // TODO: Do this in a function for shutdown, because memory is allocated in function onInitialize() + CHECK_ERROR(cudaFree(measurement_grid_)); +} + +void DogmLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, + double* min_x, double* min_y, + double* max_x, double* max_y) { + *min_x = robot_x - dogm_map_->params.size / 2; + *min_y = robot_y - dogm_map_->params.size / 2; + *max_x = robot_x + dogm_map_->params.size / 2; + *max_y = robot_y + dogm_map_->params.size / 2; + robot_x_ = robot_x; + robot_y_ = robot_y; + return; +} + +void DogmLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_grid, + int min_i, int min_j, int max_i, int max_j) { + if (!enabled_) { + return; + } + + // TODO: get current time from map + auto time_stamp = node_->now(); + costMapToMeasurementGrid(master_grid, min_i, min_j, max_i, max_j); + float robot_x = 0.f; + float robot_y = 0.f; + if (motion_compensation_) { + robot_x = robot_x_; + robot_y = robot_y_; + } + if (!is_first_measurement_) { + float dt = (time_stamp - last_time_stamp_).seconds(); + dogm_map_->updateGrid(measurement_grid_, robot_x, robot_y, 0, dt); + } else { + dogm_map_->updateGrid(measurement_grid_, robot_x, robot_y, 0, 0); + is_first_measurement_ = false; + } + publishDynamicGrid(); + last_time_stamp_ = time_stamp; + + if (opencv_visualization_) { + cv::Mat occupancy_image = dogm_map_->getOccupancyImage(); + int vis_image_size_ = 600; + float vis_occupancy_threshold_ = 0.6; + float vis_mahalanobis_distance_ = 2.0; + dogm_map_->drawVelocities(occupancy_image, vis_image_size_, 1., vis_occupancy_threshold_, vis_mahalanobis_distance_); + cv::namedWindow("occupancy_image", cv::WINDOW_NORMAL); + cv::imshow("occupancy_image", occupancy_image); + cv::waitKey(1); + } +} + +cv::Mat DogmLayer::getTransformFromMeasurementGridToCostMap(nav2_costmap_2d::Costmap2D& master_grid, + int min_i, int min_j, int max_i, int max_j) { + float measurement_grid_resolution = dogm_map_->params.resolution; + float costmap_resolution = master_grid.getResolution(); + cv::Mat scale_measurement_grid(cv::Mat::eye(cv::Size(3, 3), CV_32F)); + float scale = costmap_resolution / measurement_grid_resolution; + scale_measurement_grid.at(0, 0) *= scale; + scale_measurement_grid.at(1, 1) *= scale; + + float measurement_grid_origin_x = robot_x_ - dogm_map_->params.size / 2; + float measurement_grid_origin_y = robot_y_ - dogm_map_->params.size / 2; + float costmap_origin_x = master_grid.getOriginX() + min_i * costmap_resolution; + float costmap_origin_y = master_grid.getOriginY() + min_j * costmap_resolution; + cv::Mat scaled_measurement_grid_to_costmap(cv::Mat::eye(cv::Size(3, 3), CV_32F)); + scaled_measurement_grid_to_costmap.at(0, 2) = (costmap_origin_x - measurement_grid_origin_x) / costmap_resolution; + scaled_measurement_grid_to_costmap.at(1, 2) = (costmap_origin_y - measurement_grid_origin_y) / costmap_resolution; + + cv::Mat measurement_grid_to_costmap = scale_measurement_grid * scaled_measurement_grid_to_costmap; + return measurement_grid_to_costmap; +} + +cv::cuda::GpuMat DogmLayer::getMasterArrayDevice(nav2_costmap_2d::Costmap2D& master_grid, + int min_i, int min_j, int max_i, int max_j) { + unsigned char* master_array = master_grid.getCharMap(); + cv::Mat master_array_host(cv::Size(max_i - min_i, max_j - min_j), CV_8U, + master_array + master_grid.getIndex(min_i, min_j), master_grid.getSizeInCellsX() * sizeof(unsigned char)); + cv::cuda::GpuMat master_array_device; + master_array_device.upload(master_array_host); + return master_array_device; +} + +void DogmLayer::setUnknownAsFree(cv::cuda::GpuMat master_array_device) { + dim3 blocks(1, 1); + dim3 threads(16, 16); + setUnknownAsFreeKernel<<>>(master_array_device); +} + +cv::cuda::GpuMat DogmLayer::transformCostMapToMeasurementGrid(cv::cuda::GpuMat master_array_device, cv::Mat measurement_grid_to_costmap) { + cv::cuda::GpuMat measurement_grid_device; + master_array_device.convertTo(master_array_device, CV_32S); + cv::cuda::warpAffine(master_array_device, measurement_grid_device, measurement_grid_to_costmap(cv::Range(0, 2), cv::Range(0, 3)), + cv::Size(dogm_map_->grid_size, dogm_map_->grid_size), cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar(nav2_costmap_2d::FREE_SPACE)); + return measurement_grid_device; +} + +void DogmLayer::fillMeasurementGrid(cv::cuda::GpuMat measurement_grid_device) { + dim3 blocks(1, 1); + dim3 threads(16, 16); + fillMeasurementGridKernel<<>>(measurement_grid_, measurement_grid_device, normalized_occupancy_threshold_); +} + +void DogmLayer::costMapToMeasurementGrid(nav2_costmap_2d::Costmap2D& master_grid, + int min_i, int min_j, int max_i, int max_j) { + unsigned int size_x = master_grid.getSizeInCellsX(); + unsigned int size_y = master_grid.getSizeInCellsY(); + min_i = std::max(0, min_i); + min_j = std::max(0, min_j); + max_i = std::min(static_cast(size_x), max_i); + max_j = std::min(static_cast(size_y), max_j); + + cv::Mat measurement_grid_to_costmap = getTransformFromMeasurementGridToCostMap(master_grid, min_i, min_j, max_i, max_j); + cv::cuda::GpuMat master_array_device = getMasterArrayDevice(master_grid, min_i, min_j, max_i, max_j); + setUnknownAsFree(master_array_device); + cv::cuda::GpuMat measurement_grid_device = transformCostMapToMeasurementGrid(master_array_device, measurement_grid_to_costmap); + fillMeasurementGrid(measurement_grid_device); + + CHECK_ERROR(cudaGetLastError()); + CHECK_ERROR(cudaDeviceSynchronize()); +} + +void DogmLayer::publishDynamicGrid() { + auto message = std::make_unique(); + message->header.stamp = node_->now(); + // TODO: set correct frame id + message->header.frame_id = "map"; + message->info.resolution = dogm_map_->getResolution(); + message->info.length = dogm_map_->getGridSize() * dogm_map_->getResolution(); + message->info.size = dogm_map_->getGridSize(); + message->info.pose.position.x = dogm_map_->getPositionX(); + message->info.pose.position.y = dogm_map_->getPositionY(); + message->info.pose.position.z = 0.0; + message->info.pose.orientation.x = 0.0; + message->info.pose.orientation.y = 0.0; + message->info.pose.orientation.z = 0.0; + message->info.pose.orientation.w = 1.0; + + message->data.clear(); + message->data.resize(dogm_map_->getGridSize() * dogm_map_->getGridSize()); + + std::vector grid_cell_array = dogm_map_->getGridCells(); + #pragma omp parallel for + for (int i = 0; i < message->data.size(); i++) { + const dogm::GridCell& cell = grid_cell_array[i]; + + message->data[i].free_mass = cell.free_mass; + message->data[i].occ_mass = cell.occ_mass; + + message->data[i].mean_x_vel = cell.mean_x_vel; + message->data[i].mean_y_vel = cell.mean_y_vel; + message->data[i].var_x_vel = cell.var_x_vel; + message->data[i].var_y_vel = cell.var_y_vel; + message->data[i].covar_xy_vel = cell.covar_xy_vel; + } + + publisher_->publish(std::move(message)); +} + +void DogmLayer::reset() { + return; +} + +__global__ void setUnknownAsFreeKernel(cv::cuda::PtrStepSzb master_array) +{ + int start_row = blockIdx.y * blockDim.y + threadIdx.y; + int start_col = blockIdx.x * blockDim.x + threadIdx.x; + int step_row = blockDim.y * gridDim.y; + int step_col = blockDim.x * gridDim.x; + for (int row = start_row; row < master_array.rows; row += step_row) + { + for (int col = start_col; col < master_array.cols; col += step_col) + { + if (master_array(row, col) == nav2_costmap_2d::NO_INFORMATION) + { + master_array(row, col) = nav2_costmap_2d::FREE_SPACE; + } + } + } +} + +__device__ float clip(float x, float min, float max) +{ + assert(min <= max); + if (x < min) return min; + if (x > max) return max; + return x; +} + +__global__ void fillMeasurementGridKernel(dogm::MeasurementCell* __restrict__ measurement_grid, const cv::cuda::PtrStepSzi source, + float occupancy_threshold) +{ + int start_row = blockIdx.y * blockDim.y + threadIdx.y; + int start_col = blockIdx.x * blockDim.x + threadIdx.x; + int step_row = blockDim.y * gridDim.y; + int step_col = blockDim.x * gridDim.x; + const float eps = 0.0001f; + for (int row = start_row; row < source.rows; row += step_row) + { + for (int col = start_col; col < source.cols; col += step_col) + { + int index = col + row * source.cols; + float occ = 1.0f * source(row, col) / nav2_costmap_2d::LETHAL_OBSTACLE; + if (occ < occupancy_threshold) + { + measurement_grid[index].free_mass = clip(1 - occ, eps, 1 - eps); + measurement_grid[index].occ_mass = eps; + } + else + { + measurement_grid[index].free_mass = eps; + measurement_grid[index].occ_mass = clip(occ, eps, 1 - eps); + } + measurement_grid[index].likelihood = 1.0f; + measurement_grid[index].p_A = 1.0f; + } + } +} + +} // namespace dogm_plugin + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(dogm_plugin::DogmLayer, nav2_costmap_2d::Layer) diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/CMakeLists.txt b/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/CMakeLists.txt new file mode 100644 index 0000000..0d6925c --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.0.2) +project(time_measurer) + +include_directories( + "./include" +) + +add_library(time_measurer SHARED + src/time_measurer.cc +) + +add_executable(time_measurer_test src/time_measurer_test.cc) + +target_link_libraries(time_measurer_test + time_measurer +) + +# Install package.xml for catkin +install(FILES package.xml DESTINATION share/time_measurer/) + +set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/include") +set(TIME_MEASURER_CMAKE_DIR share/time_measurer/cmake) +include(CMakePackageConfigHelpers) +configure_package_config_file( + time_measurer-config.cmake.in + ${PROJECT_BINARY_DIR}/cmake/time_measurer/time_measurer-config.cmake + PATH_VARS TIME_MEASURER_CMAKE_DIR + INSTALL_DESTINATION ${CMAKE_INSTALL_PREFIX}/share/time_measurer +) + +install( + FILES ${PROJECT_BINARY_DIR}/cmake/time_measurer/time_measurer-config.cmake + DESTINATION share/time_measurer/ +) + +install( + TARGETS time_measurer + LIBRARY DESTINATION lib +) diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/include/time_measurer.h b/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/include/time_measurer.h new file mode 100644 index 0000000..1b6c906 --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/include/time_measurer.h @@ -0,0 +1,52 @@ +#ifndef TIME_MEASURER_H_ +#define TIME_MEASURER_H_ + +#include +#include +#include +#include +#include +#include + +namespace time_measurer { + +double ToSeconds(std::chrono::steady_clock::duration duration); + +class TimeMeasurer { + public: + TimeMeasurer(std::string name, bool print_results_on_destruction); + ~TimeMeasurer(); + + void StartMeasurement(); + void StopMeasurement(); + + void AddMeasurement(double measured_time); + + private: + std::mutex mutex_; + std::string name_; + bool print_results_on_destruction_; + std::map> start_time_; + std::vector time_measurements_; +}; + +#define MEASURE_TIME_FROM_HERE(name) \ + static time_measurer::TimeMeasurer (time_measurer_ ## name)(#name, true); \ + (time_measurer_ ## name).StartMeasurement() + +#define STOP_TIME_MESUREMENT(name) \ + (time_measurer_ ## name).StopMeasurement() + +#define MEASURE_BLOCK_TIME(name) \ + static time_measurer::TimeMeasurer (time_measurer_ ## name)(#name, true); \ + class time_measurer_stop_trigger_class_ ## name { \ + public: \ + (time_measurer_stop_trigger_class_ ## name)() {}; \ + (~time_measurer_stop_trigger_class_ ## name)() {(time_measurer_ ## name).StopMeasurement();}; \ + }; \ + time_measurer_stop_trigger_class_ ## name time_measurer_stop_trigger_ ## name; \ + (time_measurer_ ## name).StartMeasurement() + +} // namespace time_measurer + +#endif // TIME_MEASURER_H_ diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/package.xml b/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/package.xml new file mode 100644 index 0000000..8182047 --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/package.xml @@ -0,0 +1,16 @@ + + + time_measurer + 0.0.0 + The time_measurer package + + cds-jetson-host + + TODO + + cmake + + + cmake + + diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/src/time_measurer.cc b/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/src/time_measurer.cc new file mode 100644 index 0000000..38ffebc --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/src/time_measurer.cc @@ -0,0 +1,58 @@ +#include "time_measurer.h" + +namespace time_measurer { + +double ToSeconds(const std::chrono::steady_clock::duration duration) { + return std::chrono::duration_cast>(duration) + .count(); +} + +TimeMeasurer::TimeMeasurer(std::string name="Time measurer", bool print_results_on_destruction=false) { + std::lock_guard lock(mutex_); + name_ = name; + print_results_on_destruction_ = print_results_on_destruction; +} + +void TimeMeasurer::StartMeasurement() { + std::lock_guard lock(mutex_); + start_time_[pthread_self()] = std::chrono::steady_clock::now(); +} + +void TimeMeasurer::StopMeasurement() { + std::lock_guard lock(mutex_); + auto stop_time = std::chrono::steady_clock::now(); + double measured_time = ToSeconds(stop_time - start_time_[pthread_self()]); + time_measurements_.push_back(measured_time); +} + +void TimeMeasurer::AddMeasurement(double measured_time) { + std::lock_guard lock(mutex_); + start_time_[pthread_self()]; + time_measurements_.push_back(measured_time); +} + +TimeMeasurer::~TimeMeasurer() { + std::lock_guard lock(mutex_); + if (print_results_on_destruction_ && time_measurements_.size()) { + double total_measured_time = 0.; + double avarage_time = 0.; + double max_time = 0; + for (auto measured_time : time_measurements_) { + total_measured_time += measured_time; + max_time = max_time > measured_time ? max_time : measured_time; + } + avarage_time = total_measured_time / time_measurements_.size(); + + std::string log_string; + log_string += name_ + ":\n"; + log_string += " Number of measurements: " + std::to_string(time_measurements_.size()) + "\n"; + log_string += " Total measured time: " + std::to_string(total_measured_time) + "\n"; + log_string += " Average time: " + std::to_string(avarage_time) + "\n"; + log_string += " Max time: " + std::to_string(max_time) + "\n"; + log_string += " Number of threads: " + std::to_string(start_time_.size()) + "\n"; + + std::cout << log_string; + } +} + +} // namespace time_measurer diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/src/time_measurer_test.cc b/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/src/time_measurer_test.cc new file mode 100644 index 0000000..1c3c4dd --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/src/time_measurer_test.cc @@ -0,0 +1,18 @@ +#include "time_measurer.h" +#include +#include + +void long_operation(unsigned int n) { + usleep(n); +} + +int main() { + time_measurer::TimeMeasurer long_operation_time_measurer("long_operation", true); + int num = 10; + unsigned int operation_duration = 100000; + for (int i = 0; i < num; i++) { + long_operation_time_measurer.StartMeasurement(); + long_operation(operation_duration); + long_operation_time_measurer.StopMeasurement(); + } +} diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/time_measurer-config.cmake.in b/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/time_measurer-config.cmake.in new file mode 100644 index 0000000..516413c --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/time_measurer-config.cmake.in @@ -0,0 +1,5 @@ +get_filename_component(time_measurer_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) +set(time_measurer_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@") + +find_library(time_measurer_LIBRARIES NAMES time_measurer NO_DEFAULT_PATH HINTS ${time_measurer_CMAKE_DIR}/../../lib/ REQUIRED) + diff --git a/ros2_ws/src/rosbot/rosbot_gazebo/config/local_planner_test_scan_sim.yaml b/ros2_ws/src/rosbot/rosbot_gazebo/config/local_planner_test_scan_sim.yaml index 6137bec..f28406c 100644 --- a/ros2_ws/src/rosbot/rosbot_gazebo/config/local_planner_test_scan_sim.yaml +++ b/ros2_ws/src/rosbot/rosbot_gazebo/config/local_planner_test_scan_sim.yaml @@ -97,11 +97,28 @@ local_costmap: robot_base_frame: base_link use_sim_time: True rolling_window: true - width: 3 - height: 3 + width: 5 + height: 5 resolution: 0.05 footprint: '[[0.14, 0.16], [0.14, -0.16], [-0.14, -0.16], [-0.14, 0.16]]' - plugins: ["voxel_layer", "inflation_layer"] + plugins: ["voxel_layer", "inflation_layer", "dogm_layer"] + + dogm_layer: + plugin: "dogm_plugin/DogmLayer" + enabled: True + opencv_visualization: True + motion_compensation: False + size: 5.0 + resolution: 0.2 + particle_count: 3000000 + new_born_particle_count: 300000 + persistence_prob: 0.99 + stddev_process_noise_position: 0.1 + stddev_process_noise_velocity: 1.0 + birth_prob: 0.02 + stddev_velocity: 30.0 + init_max_velocity: 30.0 + normalized_occupancy_threshold: 0.5 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer"