From 1356e8822c765f841298f320e0f40f9a53b439dd Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Mon, 18 Oct 2021 12:01:10 +0300 Subject: [PATCH 01/20] add empty plugin for dogm --- .../dogm_plugin/CMakeLists.txt | 46 +++++++++++++++++++ .../dogm_plugin/dogm_plugin.xml | 5 ++ .../include/dogm_plugin/dogm_layer.h | 28 +++++++++++ .../dynamic_obstacles/dogm_plugin/package.xml | 22 +++++++++ .../dogm_plugin/src/dogm_layer.cpp | 35 ++++++++++++++ .../config/local_planner_test_scan_sim.yaml | 5 ++ 6 files changed, 141 insertions(+) create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/CMakeLists.txt create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin.xml create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/include/dogm_plugin/dogm_layer.h create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/package.xml create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/src/dogm_layer.cpp diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/CMakeLists.txt b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/CMakeLists.txt new file mode 100644 index 0000000..9c64292 --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.5) +project(dogm_plugin) + +# 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(nav2_costmap_2d REQUIRED) +find_package(pluginlib REQUIRED) + +# build +add_library(${PROJECT_NAME} SHARED src/dogm_layer.cpp) +include_directories(include) +set(dep_pkgs + nav2_costmap_2d + pluginlib) + +#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_target_dependencies(${PROJECT_NAME} ${dep_pkgs}) +ament_package() diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin.xml b/ros2_ws/src/algorithms/dynamic_obstacles/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.xml @@ -0,0 +1,5 @@ + + + Dogm plugin + + \ No newline at end of file diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/include/dogm_plugin/dogm_layer.h b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/include/dogm_plugin/dogm_layer.h new file mode 100644 index 0000000..ec9738f --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/include/dogm_plugin/dogm_layer.h @@ -0,0 +1,28 @@ +#ifndef DOGM_LAYER_HPP_ +#define DOGM_LAYER_HPP_ + +#include "nav2_costmap_2d/layer.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" + +namespace dogm_plugin +{ + +class DogmLayer : public nav2_costmap_2d::Layer +{ +public: + 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); + virtual void reset(); + +private: +}; + +} // namespace dogm_plugin + +#endif // DOGM_LAYER_HPP_ \ No newline at end of file diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/package.xml b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/package.xml new file mode 100644 index 0000000..a996c65 --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/package.xml @@ -0,0 +1,22 @@ + + + + dogm_plugin + 0.0.0 + TODO: Package description + user + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + nav2_costmap_2d + pluginlib + + + + ament_cmake + + diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/src/dogm_layer.cpp b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/src/dogm_layer.cpp new file mode 100644 index 0000000..337055a --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/src/dogm_layer.cpp @@ -0,0 +1,35 @@ +#include "dogm_plugin/dogm_layer.h" +#include + +namespace dogm_plugin +{ + +DogmLayer::DogmLayer() {} + +void DogmLayer::onInitialize() { + return; +} + +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 = 0; + *min_y = 0; + *max_x = 0; + *max_y = 0; + return; +} + +void DogmLayer::updateCosts(nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, int max_i, int max_j) { + std::cout << " 123\n"; +} + +void DogmLayer::reset() { + return; +} + +} // 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/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..d77edf5 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 @@ -102,6 +102,11 @@ local_costmap: 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 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" From ac326b07eddc3e147b2d2203b84e325dd05ad135 Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Mon, 18 Oct 2021 12:16:02 +0300 Subject: [PATCH 02/20] add dogm as submodule; add time_measurer in this repo --- .gitmodules | 3 + ros2_ws/src/algorithms/dynamic_obstacles/dogm | 1 + .../dynamic_obstacles/dogm_plugin/package.xml | 1 + .../time_measurer/CMakeLists.txt | 39 +++++++++++++ .../time_measurer/include/time_measurer.h | 52 +++++++++++++++++ .../time_measurer/package.xml | 16 +++++ .../time_measurer/src/time_measurer.cc | 58 +++++++++++++++++++ .../time_measurer/src/time_measurer_test.cc | 18 ++++++ .../time_measurer-config.cmake.in | 5 ++ 9 files changed, 193 insertions(+) create mode 160000 ros2_ws/src/algorithms/dynamic_obstacles/dogm create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/CMakeLists.txt create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/include/time_measurer.h create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/package.xml create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/src/time_measurer.cc create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/src/time_measurer_test.cc create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/time_measurer/time_measurer-config.cmake.in 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/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/package.xml b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/package.xml index a996c65..349d6be 100644 --- a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/package.xml +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/package.xml @@ -14,6 +14,7 @@ nav2_costmap_2d pluginlib + dogm 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) + From ce75c0b3d76529748a3c82a1f89bd9fcb6f7f985 Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Mon, 25 Oct 2021 14:02:02 +0300 Subject: [PATCH 03/20] refactor; add dogm messages --- .../dogm_plugin/dogm_msgs/CMakeLists.txt | 43 +++++++++++++++++++ .../dogm_msgs/msg/DynamicOccupancyGrid.msg | 8 ++++ .../dogm_plugin/dogm_msgs/msg/GridCell.msg | 20 +++++++++ .../dogm_plugin/dogm_msgs/msg/GridMapInfo.msg | 11 +++++ .../dogm_plugin/dogm_msgs/package.xml | 22 ++++++++++ .../{ => dogm_plugin}/CMakeLists.txt | 11 ++++- .../{ => dogm_plugin}/dogm_plugin.xml | 0 .../include/dogm_plugin/dogm_layer.h | 8 +++- .../dogm_plugin/{ => dogm_plugin}/package.xml | 1 + .../{ => dogm_plugin}/src/dogm_layer.cpp | 16 ++++++- 10 files changed, 135 insertions(+), 5 deletions(-) create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/CMakeLists.txt create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/msg/DynamicOccupancyGrid.msg create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/msg/GridCell.msg create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/msg/GridMapInfo.msg create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/package.xml rename ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/{ => dogm_plugin}/CMakeLists.txt (80%) rename ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/{ => dogm_plugin}/dogm_plugin.xml (100%) rename ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/{ => dogm_plugin}/include/dogm_plugin/dogm_layer.h (72%) rename ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/{ => dogm_plugin}/package.xml (96%) rename ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/{ => dogm_plugin}/src/dogm_layer.cpp (54%) 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..11e3525 --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/CMakeLists.txt @@ -0,0 +1,43 @@ +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) +# 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" +) + +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..1d2ec7e --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_msgs/package.xml @@ -0,0 +1,22 @@ + + + + dogm_msgs + 0.0.0 + TODO: Package description + user + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/CMakeLists.txt b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/CMakeLists.txt similarity index 80% rename from ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/CMakeLists.txt rename to ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/CMakeLists.txt index 9c64292..1e59d2d 100644 --- a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/CMakeLists.txt +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/CMakeLists.txt @@ -19,13 +19,20 @@ endif() find_package(ament_cmake REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(pluginlib REQUIRED) +find_package(OpenCV REQUIRED) +find_package(rclcpp REQUIRED) +find_package(dogm REQUIRED) +find_package(dogm_msgs REQUIRED) # build add_library(${PROJECT_NAME} SHARED src/dogm_layer.cpp) -include_directories(include) +include_directories(include ${DOGM_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} ${DOGM_LIBRARIES} ${OpenCV_LIBS}) set(dep_pkgs nav2_costmap_2d - pluginlib) + pluginlib + rclcpp + dogm_msgs) #install install(TARGETS ${PROJECT_NAME} DESTINATION lib) diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin.xml b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/dogm_plugin.xml similarity index 100% rename from ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin.xml rename to ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/dogm_plugin.xml diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/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 similarity index 72% rename from ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/include/dogm_plugin/dogm_layer.h rename to ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/include/dogm_plugin/dogm_layer.h index ec9738f..b1f8627 100644 --- a/ros2_ws/src/algorithms/dynamic_obstacles/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 @@ -1,8 +1,12 @@ #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 namespace dogm_plugin { @@ -16,11 +20,13 @@ class DogmLayer : public nav2_costmap_2d::Layer 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, + virtual void updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); virtual void reset(); private: + dogm::DOGM::Params params_; + std::unique_ptr grid_map_; }; } // namespace dogm_plugin diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/package.xml b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/package.xml similarity index 96% rename from ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/package.xml rename to ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/package.xml index 349d6be..8352138 100644 --- a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/package.xml +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/package.xml @@ -15,6 +15,7 @@ nav2_costmap_2d pluginlib dogm + dogm_msgs diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/src/dogm_layer.cpp b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/src/dogm_layer.cpp similarity index 54% rename from ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/src/dogm_layer.cpp rename to ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/src/dogm_layer.cpp index 337055a..71e01f0 100644 --- a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/src/dogm_layer.cpp +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/src/dogm_layer.cpp @@ -1,4 +1,5 @@ #include "dogm_plugin/dogm_layer.h" +#include "dogm_msgs/msg/dynamic_occupancy_grid.hpp" #include namespace dogm_plugin @@ -7,7 +8,18 @@ namespace dogm_plugin DogmLayer::DogmLayer() {} void DogmLayer::onInitialize() { - return; + params_.size = 50.0f; + params_.resolution = 0.2f; + params_.particle_count = 3 * static_cast(1e6); + params_.new_born_particle_count = 3 * static_cast(1e5); + params_.persistence_prob = 0.99f; + params_.stddev_process_noise_position = 0.1f; + params_.stddev_process_noise_velocity = 1.0f; + params_.birth_prob = 0.02f; + params_.stddev_velocity = 30.0f; + params_.init_max_velocity = 30.0f; + + grid_map_ = std::make_unique(params_); } void DogmLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, @@ -20,7 +32,7 @@ void DogmLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, return; } -void DogmLayer::updateCosts(nav2_costmap_2d::Costmap2D & master_grid, +void DogmLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { std::cout << " 123\n"; } From d6f866feffb41b86516dc236cca27d111b4091ad Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Tue, 26 Oct 2021 17:57:09 +0300 Subject: [PATCH 04/20] integrate dogm; messages are not published; opencv visualization is used --- .../dogm_plugin/dogm_plugin/CMakeLists.txt | 13 +- .../include/dogm_plugin/dogm_layer.h | 20 ++- .../dogm_plugin/dogm_plugin/package.xml | 1 + .../dogm_plugin/src/dogm_layer.cpp | 157 ++++++++++++++++-- .../config/local_planner_test_scan_sim.yaml | 1 - 5 files changed, 167 insertions(+), 25 deletions(-) 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 index 1e59d2d..0915e86 100644 --- 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 @@ -17,22 +17,18 @@ 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 REQUIRED) -find_package(rclcpp REQUIRED) find_package(dogm REQUIRED) find_package(dogm_msgs REQUIRED) # build add_library(${PROJECT_NAME} SHARED src/dogm_layer.cpp) -include_directories(include ${DOGM_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} ${DOGM_LIBRARIES} ${OpenCV_LIBS}) -set(dep_pkgs - nav2_costmap_2d - pluginlib - rclcpp - dogm_msgs) +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) @@ -49,5 +45,4 @@ if(BUILD_TESTING) endif() pluginlib_export_plugin_description_file(nav2_costmap_2d dogm_plugin.xml) -ament_target_dependencies(${PROJECT_NAME} ${dep_pkgs}) ament_package() 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 index b1f8627..fb4a7b7 100644 --- 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 @@ -7,12 +7,12 @@ #include #include #include +#include "dogm_msgs/msg/dynamic_occupancy_grid.hpp" +#include "std_msgs/msg/string.hpp" -namespace dogm_plugin -{ +namespace dogm_plugin { -class DogmLayer : public nav2_costmap_2d::Layer -{ +class DogmLayer : public nav2_costmap_2d::Layer { public: DogmLayer(); @@ -22,11 +22,21 @@ class DogmLayer : public nav2_costmap_2d::Layer 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); + void costMapToMeasurementGrid(nav2_costmap_2d::Costmap2D& master_grid, + int min_i, int min_j, int max_i, int max_j); + void publishDynamicGrid(); virtual void reset(); private: dogm::DOGM::Params params_; - std::unique_ptr grid_map_; + std::unique_ptr dogm_map_; + std::vector measurement_grid_; + rclcpp::Publisher::SharedPtr publisher_; + + double robot_x_; + double robot_y_; + bool is_first_measurement_; + rclcpp::Time last_time_stamp_; }; } // namespace dogm_plugin 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 index 8352138..8e3f93f 100644 --- 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 @@ -12,6 +12,7 @@ ament_lint_auto ament_lint_common + rclcpp nav2_costmap_2d pluginlib dogm diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/src/dogm_layer.cpp b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/src/dogm_layer.cpp index 71e01f0..00cdfde 100644 --- a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/src/dogm_layer.cpp +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/src/dogm_layer.cpp @@ -1,14 +1,17 @@ #include "dogm_plugin/dogm_layer.h" -#include "dogm_msgs/msg/dynamic_occupancy_grid.hpp" + +#include +#include +#include + #include -namespace dogm_plugin -{ +namespace dogm_plugin { DogmLayer::DogmLayer() {} void DogmLayer::onInitialize() { - params_.size = 50.0f; + params_.size = 20.0f; params_.resolution = 0.2f; params_.particle_count = 3 * static_cast(1e6); params_.new_born_particle_count = 3 * static_cast(1e5); @@ -19,22 +22,156 @@ void DogmLayer::onInitialize() { params_.stddev_velocity = 30.0f; params_.init_max_velocity = 30.0f; - grid_map_ = std::make_unique(params_); + dogm_map_ = std::make_unique(params_); + measurement_grid_.resize(dogm_map_->grid_cell_count); + + is_first_measurement_ = true; + + // publisher_ = node_->create_publisher("dogm_map", 10); } 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 = 0; - *min_y = 0; - *max_x = 0; - *max_y = 0; + *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) { - std::cout << " 123\n"; + auto time_stamp = node_->now(); + costMapToMeasurementGrid(master_grid, min_i, min_j, max_i, max_j); + if (!is_first_measurement_) { + float dt = (time_stamp - last_time_stamp_).seconds(); + dogm_map_->updateGrid(measurement_grid_.data(), 0, 0, 0, dt, false); + } else { + dogm_map_->updateGrid(measurement_grid_.data(), 0, 0, 0, 0, false); + is_first_measurement_ = false; + } + last_time_stamp_ = time_stamp; + + 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); +} + +void DogmLayer::costMapToMeasurementGrid(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(); + unsigned int size_x = master_grid.getSizeInCellsX(), 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); + + 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(); + float costmap_origin_y = master_grid.getOriginY(); + std::cout << "robot_position\n"; + std::cout << robot_x_ << ' ' << robot_y_ << std::endl; + std::cout << "costmap_origin\n"; + std::cout << costmap_origin_x << ' ' << costmap_origin_y << std::endl; + std::cout << "costmap_size_in_meters\n"; + std::cout << master_grid.getSizeInMetersX() << ' ' << master_grid.getSizeInMetersY() << std::endl; + std::cout << std::endl; + 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; + + cv::Mat costmap(cv::Size(size_x, size_y), CV_8U, master_array); + costmap.convertTo(costmap, CV_32S); + cv::cuda::GpuMat costmap_device; + costmap_device.upload(costmap); + + cv::Mat measurement_grid; + cv::cuda::GpuMat measurement_grid_device; + cv::cuda::warpAffine(costmap_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(-1)); + measurement_grid_device.download(measurement_grid); + + const float eps = 0.0001; + float occupancy_threshold = 0.5; + for (int x = 0; x < dogm_map_->grid_size; x++) + { + for (int y = 0; y < dogm_map_->grid_size; y++) + { + int index = x + y * dogm_map_->grid_size; + float occ = measurement_grid.at(y, x) / 100.; + if (occ < 0) + { + measurement_grid_[index].free_mass = eps; + measurement_grid_[index].occ_mass = eps; + } + else if (occ < occupancy_threshold) + { + measurement_grid_[index].free_mass = std::max(eps, std::min(1 - eps, 1 - occ)); + measurement_grid_[index].occ_mass = eps; + } + else + { + measurement_grid_[index].free_mass = eps; + measurement_grid_[index].occ_mass = std::max(eps, std::min(1 - eps, occ)); + } + measurement_grid_[index].likelihood = 1.0f; + measurement_grid_[index].p_A = 1.0f; + } + } +} + +void DogmLayer::publishDynamicGrid() { + auto message = dogm_msgs::msg::DynamicOccupancyGrid(); + message.header.stamp = node_->now(); + message.header.frame_id = "abc"; + 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(message); } void DogmLayer::reset() { 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 d77edf5..514df25 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 @@ -101,7 +101,6 @@ local_costmap: height: 3 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: From b7a544f52fbc4f50f28ab4c108c9ac494850fe03 Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Wed, 27 Oct 2021 17:32:57 +0300 Subject: [PATCH 05/20] add parameters in yaml file; use GPU to fill measurement grid --- .../dogm_plugin/dogm_plugin/CMakeLists.txt | 4 +- .../include/dogm_plugin/dogm_layer.h | 7 +- .../dogm_plugin/src/dogm_layer.cpp | 184 ------------- .../dogm_plugin/dogm_plugin/src/dogm_layer.cu | 246 ++++++++++++++++++ .../config/local_planner_test_scan_sim.yaml | 15 +- 5 files changed, 266 insertions(+), 190 deletions(-) delete mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/src/dogm_layer.cpp create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/src/dogm_layer.cu 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 index 0915e86..b23d5e6 100644 --- 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 @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(dogm_plugin) +project(dogm_plugin LANGUAGES CXX CUDA) # Default to C99 if(NOT CMAKE_C_STANDARD) @@ -25,7 +25,7 @@ find_package(dogm REQUIRED) find_package(dogm_msgs REQUIRED) # build -add_library(${PROJECT_NAME} SHARED src/dogm_layer.cpp) +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) 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 index fb4a7b7..ce8fa3d 100644 --- 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 @@ -15,6 +15,7 @@ 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, @@ -23,14 +24,16 @@ class DogmLayer : public nav2_costmap_2d::Layer { virtual void updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); void costMapToMeasurementGrid(nav2_costmap_2d::Costmap2D& master_grid, - int min_i, int min_j, int max_i, int max_j); + int min_i, int min_j, int max_i, int max_j, + float occupancy_threshold); void publishDynamicGrid(); virtual void reset(); private: + bool motion_compensation_; dogm::DOGM::Params params_; std::unique_ptr dogm_map_; - std::vector measurement_grid_; + dogm::MeasurementCell* measurement_grid_; rclcpp::Publisher::SharedPtr publisher_; double robot_x_; diff --git a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/src/dogm_layer.cpp b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/src/dogm_layer.cpp deleted file mode 100644 index 00cdfde..0000000 --- a/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/src/dogm_layer.cpp +++ /dev/null @@ -1,184 +0,0 @@ -#include "dogm_plugin/dogm_layer.h" - -#include -#include -#include - -#include - -namespace dogm_plugin { - -DogmLayer::DogmLayer() {} - -void DogmLayer::onInitialize() { - params_.size = 20.0f; - params_.resolution = 0.2f; - params_.particle_count = 3 * static_cast(1e6); - params_.new_born_particle_count = 3 * static_cast(1e5); - params_.persistence_prob = 0.99f; - params_.stddev_process_noise_position = 0.1f; - params_.stddev_process_noise_velocity = 1.0f; - params_.birth_prob = 0.02f; - params_.stddev_velocity = 30.0f; - params_.init_max_velocity = 30.0f; - - dogm_map_ = std::make_unique(params_); - measurement_grid_.resize(dogm_map_->grid_cell_count); - - is_first_measurement_ = true; - - // publisher_ = node_->create_publisher("dogm_map", 10); -} - -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) { - auto time_stamp = node_->now(); - costMapToMeasurementGrid(master_grid, min_i, min_j, max_i, max_j); - if (!is_first_measurement_) { - float dt = (time_stamp - last_time_stamp_).seconds(); - dogm_map_->updateGrid(measurement_grid_.data(), 0, 0, 0, dt, false); - } else { - dogm_map_->updateGrid(measurement_grid_.data(), 0, 0, 0, 0, false); - is_first_measurement_ = false; - } - last_time_stamp_ = time_stamp; - - 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); -} - -void DogmLayer::costMapToMeasurementGrid(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(); - unsigned int size_x = master_grid.getSizeInCellsX(), 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); - - 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(); - float costmap_origin_y = master_grid.getOriginY(); - std::cout << "robot_position\n"; - std::cout << robot_x_ << ' ' << robot_y_ << std::endl; - std::cout << "costmap_origin\n"; - std::cout << costmap_origin_x << ' ' << costmap_origin_y << std::endl; - std::cout << "costmap_size_in_meters\n"; - std::cout << master_grid.getSizeInMetersX() << ' ' << master_grid.getSizeInMetersY() << std::endl; - std::cout << std::endl; - 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; - - cv::Mat costmap(cv::Size(size_x, size_y), CV_8U, master_array); - costmap.convertTo(costmap, CV_32S); - cv::cuda::GpuMat costmap_device; - costmap_device.upload(costmap); - - cv::Mat measurement_grid; - cv::cuda::GpuMat measurement_grid_device; - cv::cuda::warpAffine(costmap_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(-1)); - measurement_grid_device.download(measurement_grid); - - const float eps = 0.0001; - float occupancy_threshold = 0.5; - for (int x = 0; x < dogm_map_->grid_size; x++) - { - for (int y = 0; y < dogm_map_->grid_size; y++) - { - int index = x + y * dogm_map_->grid_size; - float occ = measurement_grid.at(y, x) / 100.; - if (occ < 0) - { - measurement_grid_[index].free_mass = eps; - measurement_grid_[index].occ_mass = eps; - } - else if (occ < occupancy_threshold) - { - measurement_grid_[index].free_mass = std::max(eps, std::min(1 - eps, 1 - occ)); - measurement_grid_[index].occ_mass = eps; - } - else - { - measurement_grid_[index].free_mass = eps; - measurement_grid_[index].occ_mass = std::max(eps, std::min(1 - eps, occ)); - } - measurement_grid_[index].likelihood = 1.0f; - measurement_grid_[index].p_A = 1.0f; - } - } -} - -void DogmLayer::publishDynamicGrid() { - auto message = dogm_msgs::msg::DynamicOccupancyGrid(); - message.header.stamp = node_->now(); - message.header.frame_id = "abc"; - 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(message); -} - -void DogmLayer::reset() { - return; -} - -} // 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/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..9ed1a44 --- /dev/null +++ b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/src/dogm_layer.cu @@ -0,0 +1,246 @@ +#include "dogm_plugin/dogm_layer.h" + +#include +#include +#include + +#include + +namespace dogm_plugin { + +__global__ void setUnknownAsFree(cv::cuda::PtrStepSzi occupancy_grid); +__global__ void fillMeasurementGrid(dogm::MeasurementCell* __restrict__ measurement_grid, const cv::cuda::PtrStepSzi source, + float occupancy_threshold); + +DogmLayer::DogmLayer() { + CHECK_ERROR(cudaMalloc(&measurement_grid_, dogm_map_->grid_cell_count * sizeof(dogm::MeasurementCell))); +} + +void DogmLayer::onInitialize() { + declareParameter("enabled", rclcpp::ParameterValue(true)); + node_->get_parameter(name_ + "." + "enabled", enabled_); + declareParameter("motion_compensation", rclcpp::ParameterValue(true)); + node_->get_parameter(name_ + "." + "motion_compensation", motion_compensation_); + declareParameter("size", rclcpp::ParameterValue(50.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); + + dogm_map_ = std::make_unique(params_); + + robot_x_ = 0; + robot_y_ = 0; + is_first_measurement_ = true; + + // publisher_ = node_->create_publisher("dogm_map", 10); +} + +DogmLayer::~DogmLayer() { + 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; + } + + auto time_stamp = node_->now(); + costMapToMeasurementGrid(master_grid, min_i, min_j, max_i, max_j, 0.5); + float robot_x = 0.f, 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; + } + last_time_stamp_ = time_stamp; + + 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); +} + +void DogmLayer::costMapToMeasurementGrid(nav2_costmap_2d::Costmap2D& master_grid, + int min_i, int min_j, int max_i, int max_j, + float occupancy_threshold) { + unsigned char* master_array = master_grid.getCharMap(); + unsigned int size_x = master_grid.getSizeInCellsX(), 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); + + 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(); + float costmap_origin_y = master_grid.getOriginY(); + 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; + + dim3 blocks(1, 1); + dim3 threads(16, 16); + cv::Mat costmap(cv::Size(size_x, size_y), CV_8U, master_array); + costmap.convertTo(costmap, CV_32S); + cv::cuda::GpuMat costmap_device; + costmap_device.upload(costmap); + setUnknownAsFree<<>>(costmap_device); + + cv::Mat measurement_grid; + cv::cuda::GpuMat measurement_grid_device; + cv::cuda::warpAffine(costmap_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(0)); + fillMeasurementGrid<<>>(measurement_grid_, measurement_grid_device, occupancy_threshold); + + CHECK_ERROR(cudaGetLastError()); + CHECK_ERROR(cudaDeviceSynchronize()); +} + +void DogmLayer::publishDynamicGrid() { + auto message = dogm_msgs::msg::DynamicOccupancyGrid(); + message.header.stamp = node_->now(); + message.header.frame_id = "abc"; + 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(message); +} + +void DogmLayer::reset() { + return; +} + +__global__ void setUnknownAsFree(cv::cuda::PtrStepSzi occupancy_grid) +{ + 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 < occupancy_grid.rows; row += step_row) + { + for (int col = start_col; col < occupancy_grid.cols; col += step_col) + { + if (occupancy_grid(row, col) < 0) + { + occupancy_grid(row, col) = 0; + } + } + } +} + +__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 fillMeasurementGrid(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 = source(row, col) / 100.f; + 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/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 514df25..6bb4e15 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,8 +97,8 @@ 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", "dogm_layer"] @@ -106,6 +106,17 @@ local_costmap: dogm_layer: plugin: "dogm_plugin/DogmLayer" enabled: True + motion_compensation: False + size: 20.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 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" From cf1695e55b0a5e2e4cae5774c5a9f001cf01117c Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Wed, 27 Oct 2021 18:30:34 +0300 Subject: [PATCH 06/20] fix bug --- .../dogm_plugin/dogm_plugin/src/dogm_layer.cu | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 index 9ed1a44..6114a11 100644 --- 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 @@ -12,9 +12,7 @@ __global__ void setUnknownAsFree(cv::cuda::PtrStepSzi occupancy_grid); __global__ void fillMeasurementGrid(dogm::MeasurementCell* __restrict__ measurement_grid, const cv::cuda::PtrStepSzi source, float occupancy_threshold); -DogmLayer::DogmLayer() { - CHECK_ERROR(cudaMalloc(&measurement_grid_, dogm_map_->grid_cell_count * sizeof(dogm::MeasurementCell))); -} +DogmLayer::DogmLayer() {} void DogmLayer::onInitialize() { declareParameter("enabled", rclcpp::ParameterValue(true)); @@ -43,6 +41,7 @@ void DogmLayer::onInitialize() { node_->get_parameter(name_ + "." + "init_max_velocity", params_.init_max_velocity); 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; From 0ff8514fb97999efc9c46ec907680a53c723f9a6 Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Wed, 27 Oct 2021 18:35:58 +0300 Subject: [PATCH 07/20] use nav2_costmap_2d constants to determine cells states --- .../dogm_plugin/dogm_plugin/src/dogm_layer.cu | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 index 6114a11..1a88f6f 100644 --- 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 @@ -193,9 +193,9 @@ __global__ void setUnknownAsFree(cv::cuda::PtrStepSzi occupancy_grid) { for (int col = start_col; col < occupancy_grid.cols; col += step_col) { - if (occupancy_grid(row, col) < 0) + if (occupancy_grid(row, col) == nav2_costmap_2d::NO_INFORMATION) { - occupancy_grid(row, col) = 0; + occupancy_grid(row, col) = nav2_costmap_2d::FREE_SPACE; } } } @@ -222,7 +222,7 @@ __global__ void fillMeasurementGrid(dogm::MeasurementCell* __restrict__ measurem for (int col = start_col; col < source.cols; col += step_col) { int index = col + row * source.cols; - float occ = source(row, col) / 100.f; + 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); From 5ec697e3edaf2839bab4f0384a3650da7507fb86 Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Thu, 4 Nov 2021 10:45:43 +0300 Subject: [PATCH 08/20] build with cuda-dev and opencv with cuda; build cuda and opencv by default --- docker/Dockerfile | 9 ++++++++- docker/env-gazebo.sh | 2 +- docker/env-robot.sh | 4 ++-- docker/env-universal.sh | 2 +- docker/scripts/cuda.sh | 11 +++++++++-- docker/scripts/opencv.sh | 41 +++++++++++++++++++++++++++++++--------- 6 files changed, 53 insertions(+), 16 deletions(-) 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..564334c 100755 --- a/docker/scripts/opencv.sh +++ b/docker/scripts/opencv.sh @@ -1,14 +1,37 @@ #!/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 +# 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 +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/user/activate_opencv4.5.bash + From c1a04b23b4fea1737d6a22e53cf7dedc1219396c Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Sat, 6 Nov 2021 14:58:27 +0300 Subject: [PATCH 09/20] add dependencies for opencv in opencv.sh --- docker/scripts/opencv.sh | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/docker/scripts/opencv.sh b/docker/scripts/opencv.sh index 564334c..abf3ec9 100755 --- a/docker/scripts/opencv.sh +++ b/docker/scripts/opencv.sh @@ -1,5 +1,13 @@ #!/bin/bash +# 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 @@ -30,6 +38,7 @@ sudo cmake \ sudo make -j4 sudo make install +# create bash file 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\ From 2bc2784a055f1df6afe626392fe71db1437e14a4 Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Sat, 6 Nov 2021 16:10:00 +0300 Subject: [PATCH 10/20] use part of costmap --- .../dogm_plugin/dogm_plugin/src/dogm_layer.cu | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) 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 index 1a88f6f..67bd88c 100644 --- 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 @@ -74,7 +74,8 @@ void DogmLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_grid, auto time_stamp = node_->now(); costMapToMeasurementGrid(master_grid, min_i, min_j, max_i, max_j, 0.5); - float robot_x = 0.f, robot_y = 0.f; + float robot_x = 0.f; + float robot_y = 0.f; if (motion_compensation_) { robot_x = robot_x_; robot_y = robot_y_; @@ -101,8 +102,8 @@ void DogmLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_grid, void DogmLayer::costMapToMeasurementGrid(nav2_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j, float occupancy_threshold) { - unsigned char* master_array = master_grid.getCharMap(); - unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY(); + 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); @@ -117,8 +118,8 @@ void DogmLayer::costMapToMeasurementGrid(nav2_costmap_2d::Costmap2D& master_grid 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(); - float costmap_origin_y = master_grid.getOriginY(); + 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; @@ -127,7 +128,8 @@ void DogmLayer::costMapToMeasurementGrid(nav2_costmap_2d::Costmap2D& master_grid dim3 blocks(1, 1); dim3 threads(16, 16); - cv::Mat costmap(cv::Size(size_x, size_y), CV_8U, master_array); + unsigned char* master_array = master_grid.getCharMap(); + cv::Mat costmap(cv::Size(max_i - min_i, max_j - min_j), CV_8U, master_array + master_grid.getIndex(min_i, min_j), size_x * sizeof(unsigned char)); costmap.convertTo(costmap, CV_32S); cv::cuda::GpuMat costmap_device; costmap_device.upload(costmap); @@ -136,7 +138,7 @@ void DogmLayer::costMapToMeasurementGrid(nav2_costmap_2d::Costmap2D& master_grid cv::Mat measurement_grid; cv::cuda::GpuMat measurement_grid_device; cv::cuda::warpAffine(costmap_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(0)); + cv::Size(dogm_map_->grid_size, dogm_map_->grid_size), cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar(nav2_costmap_2d::FREE_SPACE)); fillMeasurementGrid<<>>(measurement_grid_, measurement_grid_device, occupancy_threshold); CHECK_ERROR(cudaGetLastError()); From 0be0e79c38992f522fa8501b26f47315666988d3 Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Tue, 9 Nov 2021 14:10:29 +0300 Subject: [PATCH 11/20] fix dogm_msgs --- .../dynamic_obstacles/dogm_plugin/dogm_msgs/CMakeLists.txt | 3 +++ .../dynamic_obstacles/dogm_plugin/dogm_msgs/package.xml | 5 ++++- 2 files changed, 7 insertions(+), 1 deletion(-) 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 index 11e3525..4d730d1 100644 --- 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 @@ -17,6 +17,8 @@ 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) @@ -27,6 +29,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/GridCell.msg" "msg/GridMapInfo.msg" "msg/DynamicOccupancyGrid.msg" + DEPENDENCIES std_msgs geometry_msgs ) if(BUILD_TESTING) 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 index 1d2ec7e..a77ed07 100644 --- 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 @@ -11,7 +11,10 @@ ament_lint_auto ament_lint_common - + + std_msgs + geometry_msgs + rosidl_default_generators rosidl_default_runtime rosidl_interface_packages From 548424ec7e2baf4aa576b55e616d41d97626ec22 Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Tue, 9 Nov 2021 14:13:09 +0300 Subject: [PATCH 12/20] adjust dinamic map size --- .../rosbot_gazebo/config/local_planner_test_scan_sim.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 6bb4e15..9dd7dde 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 @@ -107,7 +107,7 @@ local_costmap: plugin: "dogm_plugin/DogmLayer" enabled: True motion_compensation: False - size: 20.0 + size: 5.0 resolution: 0.2 particle_count: 3000000 new_born_particle_count: 300000 From 15f4996881233bf300165f92710ef43e2e8f30c0 Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Thu, 11 Nov 2021 03:52:39 +0300 Subject: [PATCH 13/20] use unique pointer to create and send ros2 message --- .../dogm_plugin/dogm_plugin/src/dogm_layer.cu | 50 +++++++++---------- 1 file changed, 25 insertions(+), 25 deletions(-) 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 index 67bd88c..082785d 100644 --- 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 @@ -146,39 +146,39 @@ void DogmLayer::costMapToMeasurementGrid(nav2_costmap_2d::Costmap2D& master_grid } void DogmLayer::publishDynamicGrid() { - auto message = dogm_msgs::msg::DynamicOccupancyGrid(); - message.header.stamp = node_->now(); - message.header.frame_id = "abc"; - 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()); + auto message = std::make_unique(); + message->header.stamp = node_->now(); + message->header.frame_id = "abc"; + 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++) { + 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].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; + 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(message); + publisher_->publish(std::move(message)); } void DogmLayer::reset() { From fe7bb27de4c94c96d64a7531a07478aac5e00f50 Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Thu, 11 Nov 2021 04:10:23 +0300 Subject: [PATCH 14/20] minor changes --- .../dogm_plugin/dogm_plugin/src/dogm_layer.cu | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) 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 index 082785d..876843c 100644 --- 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 @@ -8,7 +8,7 @@ namespace dogm_plugin { -__global__ void setUnknownAsFree(cv::cuda::PtrStepSzi occupancy_grid); +__global__ void setUnknownAsFree(cv::cuda::PtrStepSzb master_array); __global__ void fillMeasurementGrid(dogm::MeasurementCell* __restrict__ measurement_grid, const cv::cuda::PtrStepSzi source, float occupancy_threshold); @@ -129,15 +129,15 @@ void DogmLayer::costMapToMeasurementGrid(nav2_costmap_2d::Costmap2D& master_grid dim3 blocks(1, 1); dim3 threads(16, 16); unsigned char* master_array = master_grid.getCharMap(); - cv::Mat costmap(cv::Size(max_i - min_i, max_j - min_j), CV_8U, master_array + master_grid.getIndex(min_i, min_j), size_x * sizeof(unsigned char)); - costmap.convertTo(costmap, CV_32S); - cv::cuda::GpuMat costmap_device; - costmap_device.upload(costmap); - setUnknownAsFree<<>>(costmap_device); + 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), size_x * sizeof(unsigned char)); + cv::cuda::GpuMat master_array_device; + master_array_device.upload(master_array_host); + setUnknownAsFree<<>>(master_array_device); + master_array_device.convertTo(master_array_device, CV_32S); cv::Mat measurement_grid; cv::cuda::GpuMat measurement_grid_device; - cv::cuda::warpAffine(costmap_device, measurement_grid_device, measurement_grid_to_costmap(cv::Range(0, 2), cv::Range(0, 3)), + 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)); fillMeasurementGrid<<>>(measurement_grid_, measurement_grid_device, occupancy_threshold); @@ -185,19 +185,19 @@ void DogmLayer::reset() { return; } -__global__ void setUnknownAsFree(cv::cuda::PtrStepSzi occupancy_grid) +__global__ void setUnknownAsFree(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 < occupancy_grid.rows; row += step_row) + for (int row = start_row; row < master_array.rows; row += step_row) { - for (int col = start_col; col < occupancy_grid.cols; col += step_col) + for (int col = start_col; col < master_array.cols; col += step_col) { - if (occupancy_grid(row, col) == nav2_costmap_2d::NO_INFORMATION) + if (master_array(row, col) == nav2_costmap_2d::NO_INFORMATION) { - occupancy_grid(row, col) = nav2_costmap_2d::FREE_SPACE; + master_array(row, col) = nav2_costmap_2d::FREE_SPACE; } } } From e365eb9fbd3f9759e6fb1eb124865816cb95bff2 Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Thu, 11 Nov 2021 04:14:19 +0300 Subject: [PATCH 15/20] add new parameters: opencv_visualization and normalized_threshold --- .../include/dogm_plugin/dogm_layer.h | 5 +-- .../dogm_plugin/dogm_plugin/src/dogm_layer.cu | 33 ++++++++++++------- .../config/local_planner_test_scan_sim.yaml | 2 ++ 3 files changed, 26 insertions(+), 14 deletions(-) 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 index ce8fa3d..cb9f1d0 100644 --- 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 @@ -24,14 +24,15 @@ class DogmLayer : public nav2_costmap_2d::Layer { virtual void updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); void costMapToMeasurementGrid(nav2_costmap_2d::Costmap2D& master_grid, - int min_i, int min_j, int max_i, int max_j, - float occupancy_threshold); + int min_i, int min_j, int max_i, int max_j); void publishDynamicGrid(); virtual void reset(); private: + bool opencv_visualization_; bool motion_compensation_; dogm::DOGM::Params params_; + float normalized_threshold_; std::unique_ptr dogm_map_; dogm::MeasurementCell* measurement_grid_; rclcpp::Publisher::SharedPtr publisher_; 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 index 876843c..e10cb88 100644 --- 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 @@ -17,8 +17,12 @@ 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(50.0f)); node_->get_parameter(name_ + "." + "size", params_.size); declareParameter("resolution", rclcpp::ParameterValue(0.2f)); @@ -40,6 +44,9 @@ void DogmLayer::onInitialize() { declareParameter("init_max_velocity", rclcpp::ParameterValue(30.0f)); node_->get_parameter(name_ + "." + "init_max_velocity", params_.init_max_velocity); + declareParameter("normalized_threshold", rclcpp::ParameterValue(0.5f)); + node_->get_parameter(name_ + "." + "normalized_threshold", normalized_threshold_); + dogm_map_ = std::make_unique(params_); CHECK_ERROR(cudaMalloc(&measurement_grid_, dogm_map_->grid_cell_count * sizeof(dogm::MeasurementCell))); @@ -72,8 +79,9 @@ void DogmLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_grid, return; } + // TODO: get current time from map auto time_stamp = node_->now(); - costMapToMeasurementGrid(master_grid, min_i, min_j, max_i, max_j, 0.5); + costMapToMeasurementGrid(master_grid, min_i, min_j, max_i, max_j); float robot_x = 0.f; float robot_y = 0.f; if (motion_compensation_) { @@ -89,19 +97,20 @@ void DogmLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_grid, } last_time_stamp_ = time_stamp; - 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); + 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); + } } void DogmLayer::costMapToMeasurementGrid(nav2_costmap_2d::Costmap2D& master_grid, - int min_i, int min_j, int max_i, int max_j, - float occupancy_threshold) { + 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); @@ -139,7 +148,7 @@ void DogmLayer::costMapToMeasurementGrid(nav2_costmap_2d::Costmap2D& master_grid cv::cuda::GpuMat measurement_grid_device; 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)); - fillMeasurementGrid<<>>(measurement_grid_, measurement_grid_device, occupancy_threshold); + fillMeasurementGrid<<>>(measurement_grid_, measurement_grid_device, normalized_threshold_); CHECK_ERROR(cudaGetLastError()); CHECK_ERROR(cudaDeviceSynchronize()); 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 9dd7dde..df0a811 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 @@ -106,6 +106,7 @@ local_costmap: dogm_layer: plugin: "dogm_plugin/DogmLayer" enabled: True + opencv_visualization: True motion_compensation: False size: 5.0 resolution: 0.2 @@ -117,6 +118,7 @@ local_costmap: birth_prob: 0.02 stddev_velocity: 30.0 init_max_velocity: 30.0 + normalized_threshold: 0.5 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" From d94b8f37b1c029ad7a68b85e782e8e16890e6af1 Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Fri, 12 Nov 2021 00:50:03 +0300 Subject: [PATCH 16/20] add opencv 4.5 libraries to ldconfig --- docker/scripts/opencv.sh | 21 ++++++++++++++----- .../dogm_plugin/dogm_plugin/CMakeLists.txt | 2 +- 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/docker/scripts/opencv.sh b/docker/scripts/opencv.sh index abf3ec9..5a51318 100755 --- a/docker/scripts/opencv.sh +++ b/docker/scripts/opencv.sh @@ -38,9 +38,20 @@ sudo cmake \ sudo make -j4 sudo make install -# create bash file 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/user/activate_opencv4.5.bash +# 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_plugin/dogm_plugin/CMakeLists.txt b/ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/CMakeLists.txt index b23d5e6..9e1ef76 100644 --- 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 @@ -20,7 +20,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(pluginlib REQUIRED) -find_package(OpenCV REQUIRED) +find_package(OpenCV 4.5.2 EXACT REQUIRED) find_package(dogm REQUIRED) find_package(dogm_msgs REQUIRED) From fcb25f1905ab21b417a1ecb7d4900508cdff3cd3 Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Sat, 27 Nov 2021 13:15:25 +0300 Subject: [PATCH 17/20] fix publisher --- .../dogm_plugin/include/dogm_plugin/dogm_layer.h | 2 +- .../dogm_plugin/dogm_plugin/src/dogm_layer.cu | 8 ++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) 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 index cb9f1d0..0f29c7c 100644 --- 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 @@ -35,7 +35,7 @@ class DogmLayer : public nav2_costmap_2d::Layer { float normalized_threshold_; std::unique_ptr dogm_map_; dogm::MeasurementCell* measurement_grid_; - rclcpp::Publisher::SharedPtr publisher_; + std::shared_ptr> publisher_; double robot_x_; double robot_y_; 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 index e10cb88..942d101 100644 --- 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 @@ -54,10 +54,12 @@ void DogmLayer::onInitialize() { robot_y_ = 0; is_first_measurement_ = true; - // publisher_ = node_->create_publisher("dogm_map", 10); + 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_)); } @@ -95,6 +97,7 @@ void DogmLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_grid, dogm_map_->updateGrid(measurement_grid_, robot_x, robot_y, 0, 0); is_first_measurement_ = false; } + publishDynamicGrid(); last_time_stamp_ = time_stamp; if (opencv_visualization_) { @@ -157,7 +160,8 @@ void DogmLayer::costMapToMeasurementGrid(nav2_costmap_2d::Costmap2D& master_grid void DogmLayer::publishDynamicGrid() { auto message = std::make_unique(); message->header.stamp = node_->now(); - message->header.frame_id = "abc"; + // 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(); From ea7f1ff1fa62e34ce761c62f6dab19c12d15de74 Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Thu, 2 Dec 2021 17:35:08 +0300 Subject: [PATCH 18/20] refactor and add documentation --- .../include/dogm_plugin/dogm_layer.h | 34 +++++++++ .../dogm_plugin/dogm_plugin/src/dogm_layer.cu | 69 ++++++++++++------- 2 files changed, 79 insertions(+), 24 deletions(-) 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 index 0f29c7c..f737cd7 100644 --- 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 @@ -5,6 +5,8 @@ #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" @@ -23,11 +25,43 @@ class DogmLayer : public nav2_costmap_2d::Layer { 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_; 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 index 942d101..7308fb6 100644 --- 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 @@ -1,15 +1,11 @@ #include "dogm_plugin/dogm_layer.h" - -#include -#include #include - #include namespace dogm_plugin { -__global__ void setUnknownAsFree(cv::cuda::PtrStepSzb master_array); -__global__ void fillMeasurementGrid(dogm::MeasurementCell* __restrict__ measurement_grid, const cv::cuda::PtrStepSzi source, +__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() {} @@ -112,15 +108,8 @@ void DogmLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_grid, } } -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 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)); @@ -137,22 +126,54 @@ void DogmLayer::costMapToMeasurementGrid(nav2_costmap_2d::Costmap2D& master_grid 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; +} - dim3 blocks(1, 1); - dim3 threads(16, 16); +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), size_x * sizeof(unsigned char)); + 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); - setUnknownAsFree<<>>(master_array_device); - master_array_device.convertTo(master_array_device, CV_32S); + 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::Mat measurement_grid; +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)); - fillMeasurementGrid<<>>(measurement_grid_, measurement_grid_device, normalized_threshold_); + 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_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()); } @@ -198,7 +219,7 @@ void DogmLayer::reset() { return; } -__global__ void setUnknownAsFree(cv::cuda::PtrStepSzb master_array) +__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; @@ -224,7 +245,7 @@ __device__ float clip(float x, float min, float max) return x; } -__global__ void fillMeasurementGrid(dogm::MeasurementCell* __restrict__ measurement_grid, const cv::cuda::PtrStepSzi source, +__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; From f7a8d925598dd3d31fdcaaab7702c8b9222ebabb Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Thu, 2 Dec 2021 19:09:30 +0300 Subject: [PATCH 19/20] remane parameter normalized_threshold -> normalized_occupancy_threshold --- .../dogm_plugin/include/dogm_plugin/dogm_layer.h | 2 +- .../dogm_plugin/dogm_plugin/src/dogm_layer.cu | 10 +++++----- .../config/local_planner_test_scan_sim.yaml | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) 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 index f737cd7..aa816a5 100644 --- 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 @@ -66,7 +66,7 @@ class DogmLayer : public nav2_costmap_2d::Layer { bool opencv_visualization_; bool motion_compensation_; dogm::DOGM::Params params_; - float normalized_threshold_; + float normalized_occupancy_threshold_; std::unique_ptr dogm_map_; dogm::MeasurementCell* measurement_grid_; std::shared_ptr> publisher_; 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 index 7308fb6..1a39a88 100644 --- 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 @@ -19,7 +19,7 @@ void DogmLayer::onInitialize() { declareParameter("motion_compensation", rclcpp::ParameterValue(true)); node_->get_parameter(name_ + "." + "motion_compensation", motion_compensation_); - declareParameter("size", rclcpp::ParameterValue(50.0f)); + 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); @@ -40,8 +40,8 @@ void DogmLayer::onInitialize() { declareParameter("init_max_velocity", rclcpp::ParameterValue(30.0f)); node_->get_parameter(name_ + "." + "init_max_velocity", params_.init_max_velocity); - declareParameter("normalized_threshold", rclcpp::ParameterValue(0.5f)); - node_->get_parameter(name_ + "." + "normalized_threshold", normalized_threshold_); + 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))); @@ -156,7 +156,7 @@ cv::cuda::GpuMat DogmLayer::transformCostMapToMeasurementGrid(cv::cuda::GpuMat m void DogmLayer::fillMeasurementGrid(cv::cuda::GpuMat measurement_grid_device) { dim3 blocks(1, 1); dim3 threads(16, 16); - fillMeasurementGridKernel<<>>(measurement_grid_, measurement_grid_device, normalized_threshold_); + fillMeasurementGridKernel<<>>(measurement_grid_, measurement_grid_device, normalized_occupancy_threshold_); } void DogmLayer::costMapToMeasurementGrid(nav2_costmap_2d::Costmap2D& master_grid, @@ -246,7 +246,7 @@ __device__ float clip(float x, float min, float max) } __global__ void fillMeasurementGridKernel(dogm::MeasurementCell* __restrict__ measurement_grid, const cv::cuda::PtrStepSzi source, - float occupancy_threshold) + float occupancy_threshold) { int start_row = blockIdx.y * blockDim.y + threadIdx.y; int start_col = blockIdx.x * blockDim.x + threadIdx.x; 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 df0a811..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 @@ -118,7 +118,7 @@ local_costmap: birth_prob: 0.02 stddev_velocity: 30.0 init_max_velocity: 30.0 - normalized_threshold: 0.5 + normalized_occupancy_threshold: 0.5 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" From 9471e5137725297774603eee9d3b40c2cf92c0a8 Mon Sep 17 00:00:00 2001 From: andrey1908 Date: Thu, 2 Dec 2021 19:09:53 +0300 Subject: [PATCH 20/20] add readme --- .../dogm_plugin/dogm_plugin/README.md | 104 ++++++++++++++++++ 1 file changed, 104 insertions(+) create mode 100644 ros2_ws/src/algorithms/dynamic_obstacles/dogm_plugin/dogm_plugin/README.md 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 |