diff --git a/mkdocs.yml b/mkdocs.yml index 17dc51f1f..99a592d91 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -100,6 +100,7 @@ nav: - Planning: - docs/robot/autonomy/4_global/planning/index.md - robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/README.md + - robot/ros_ws/src/autonomy/4_global/b_planners/exploration/README.md - Behavior: - docs/robot/autonomy/5_behavior/index.md - docs/robot/autonomy/5_behavior/behavior_tree.md diff --git a/robot/ros_ws/manual_exploration.sh b/robot/ros_ws/manual_exploration.sh new file mode 100755 index 000000000..8804c070a --- /dev/null +++ b/robot/ros_ws/manual_exploration.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Pair 2 (run in parallel) +ros2 service call /robot_1/behavior/global_plan_toggle \ + std_srvs/srv/Trigger "{}" & p1=$! + +ros2 service call /robot_1/trajectory_controller/set_trajectory_mode \ + airstack_msgs/srv/TrajectoryMode "{mode: 3}" & p2=$! + +wait $p1 $p2 +echo "exploration command sent" diff --git a/robot/ros_ws/manual_takeoff.sh b/robot/ros_ws/manual_takeoff.sh new file mode 100755 index 000000000..56a062d3e --- /dev/null +++ b/robot/ros_ws/manual_takeoff.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Pair 1 (run in parallel) +ros2 service call /robot_1/trajectory_controller/set_trajectory_mode \ + airstack_msgs/srv/TrajectoryMode "{mode: 2}" & p1=$! + +ros2 service call /robot_1/takeoff_landing_planner/set_takeoff_landing_command \ + airstack_msgs/srv/TakeoffLandingCommand "{command: 0}" & p2=$! + +wait $p1 $p2 +echo "takeoff command sent" diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/CMakeLists.txt b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/CMakeLists.txt new file mode 100644 index 000000000..2dfc80ff7 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/CMakeLists.txt @@ -0,0 +1,146 @@ +cmake_minimum_required(VERSION 3.5) +set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules" ${CMAKE_MODULE_PATH}) +project(exploration_planner) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +# set(OpenVDB_INCLUDE_DIR "/usr/local/include/openvdb" CACHE STRING "Path to OpenVDB installation") +find_package(OpenVDB REQUIRED) +# find_package(vdb_mapping REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(message_filters REQUIRED) + +################################### +## ament specific configuration ## +################################### + +ament_package() + +########### +## Build ## +########### + +# Specify additional locations of header files + +# Declare a C++ executable +add_executable(exploration_planner + src/exploration_node_run.cpp + src/exploration_node.cpp + src/exploration_logic.cpp + src/viewpoint_sampling.cpp + src/collision_checker.cpp + src/rrt_planner.cpp + src/utils.cpp) + +target_include_directories(exploration_planner + PRIVATE + ${PCL_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${CMAKE_CURRENT_SOURCE_DIR}/include) + +target_link_libraries(exploration_planner + # vdb_mapping::vdb_mapping + OpenVDB::openvdb + ${EIGEN3_LIBRARIES} + ${PCL_LIBRARIES}) + +add_executable(waypoint_routing_planner + src/waypoint_routing_node.cpp + src/exploration_node.cpp + src/exploration_logic.cpp + src/viewpoint_sampling.cpp + src/collision_checker.cpp + src/rrt_planner.cpp + src/utils.cpp) + +target_include_directories(waypoint_routing_planner + PRIVATE + ${PCL_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${CMAKE_CURRENT_SOURCE_DIR}/include) + +target_link_libraries(waypoint_routing_planner + # vdb_mapping::vdb_mapping + OpenVDB::openvdb + ${EIGEN3_LIBRARIES} + ${PCL_LIBRARIES}) + +# Link libraries +ament_target_dependencies(exploration_planner + rclcpp + rclcpp_action + std_msgs + std_srvs + geometry_msgs + nav_msgs + visualization_msgs + tf2 + tf2_ros + pcl_conversions + tf2_eigen + tf2_geometry_msgs +) + +ament_target_dependencies(waypoint_routing_planner + rclcpp + rclcpp_action + std_msgs + std_srvs + geometry_msgs + nav_msgs + visualization_msgs + tf2 + tf2_ros + pcl_conversions + tf2_eigen + tf2_geometry_msgs +) + +# Install executable +install(TARGETS + exploration_planner + waypoint_routing_planner + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}) + +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}) + +install(DIRECTORY + src + DESTINATION share/${PROJECT_NAME}) + + +############# +## Testing ## +############# + +# Add gtest based cpp test target and link libraries +# ament_add_gtest(${PROJECT_NAME}-test test/test_exploration_planner.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_node) +# endif() \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/CMakeModules/FindBlosc.cmake b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/CMakeModules/FindBlosc.cmake new file mode 100644 index 000000000..44a0f3f54 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/CMakeModules/FindBlosc.cmake @@ -0,0 +1,394 @@ +# Copyright Contributors to the OpenVDB Project +# SPDX-License-Identifier: MPL-2.0 +# +#[=======================================================================[.rst: + +FindBlosc +--------- + +Find Blosc include dirs and libraries + +Use this module by invoking find_package with the form:: + + find_package(Blosc + [version] [EXACT] # Minimum or EXACT version e.g. 1.5.0 + [REQUIRED] # Fail with error if Blosc is not found + ) + +IMPORTED Targets +^^^^^^^^^^^^^^^^ + +``Blosc::blosc`` + This module defines IMPORTED target Blosc::Blosc, if Blosc has been found. + +Result Variables +^^^^^^^^^^^^^^^^ + +This will define the following variables: + +``Blosc_FOUND`` + True if the system has the Blosc library. +``Blosc_VERSION`` + The version of the Blosc library which was found. +``Blosc_INCLUDE_DIRS`` + Include directories needed to use Blosc. +``Blosc_RELEASE_LIBRARIES`` + Libraries needed to link to the release version of Blosc. +``Blosc_RELEASE_LIBRARY_DIRS`` + Blosc release library directories. +``Blosc_DEBUG_LIBRARIES`` + Libraries needed to link to the debug version of Blosc. +``Blosc_DEBUG_LIBRARY_DIRS`` + Blosc debug library directories. + +Deprecated - use [RELEASE|DEBUG] variants: + +``Blosc_LIBRARIES`` + Libraries needed to link to Blosc. +``Blosc_LIBRARY_DIRS`` + Blosc library directories. + +Cache Variables +^^^^^^^^^^^^^^^ + +The following cache variables may also be set: + +``Blosc_INCLUDE_DIR`` + The directory containing ``blosc.h``. +``Blosc_LIBRARY`` + The path to the Blosc library. may include target_link_libraries() debug/optimized keywords +``Blosc_LIBRARY_RELEASE`` + The path to the Blosc release library. +``Blosc_LIBRARY_DEBUG`` + The path to the Blosc debug library. + +Hints +^^^^^ + +Instead of explicitly setting the cache variables, the following variables +may be provided to tell this module where to look. + +``Blosc_ROOT`` + Preferred installation prefix. +``BLOSC_INCLUDEDIR`` + Preferred include directory e.g. /include +``BLOSC_LIBRARYDIR`` + Preferred library directory e.g. /lib +``BLOSC_DEBUG_SUFFIX`` + Suffix of the debug version of blosc. Defaults to "_d", OR the empty string for VCPKG_TOOLCHAIN +``SYSTEM_LIBRARY_PATHS`` + Global list of library paths intended to be searched by and find_xxx call +``BLOSC_USE_STATIC_LIBS`` + Only search for static blosc libraries +``BLOSC_USE_EXTERNAL_SOURCES`` + Set to ON if Blosc has been built using external sources for LZ4, snappy, + zlib and zstd. Default is OFF. +``DISABLE_CMAKE_SEARCH_PATHS`` + Disable CMakes default search paths for find_xxx calls in this module + +#]=======================================================================] + +cmake_minimum_required(VERSION 3.15) +include(GNUInstallDirs) + +mark_as_advanced( + Blosc_INCLUDE_DIR + Blosc_LIBRARY +) + +set(_FIND_BLOSC_ADDITIONAL_OPTIONS "") +if(DISABLE_CMAKE_SEARCH_PATHS) + set(_FIND_BLOSC_ADDITIONAL_OPTIONS NO_DEFAULT_PATH) +endif() + +# Set _BLOSC_ROOT based on a user provided root var. Xxx_ROOT and ENV{Xxx_ROOT} +# are prioritised over the legacy capitalized XXX_ROOT variables for matching +# CMake 3.12 behaviour +# @todo deprecate -D and ENV BLOSC_ROOT from CMake 3.12 +if(Blosc_ROOT) + set(_BLOSC_ROOT ${Blosc_ROOT}) +elseif(DEFINED ENV{Blosc_ROOT}) + set(_BLOSC_ROOT $ENV{Blosc_ROOT}) +elseif(BLOSC_ROOT) + set(_BLOSC_ROOT ${BLOSC_ROOT}) +elseif(DEFINED ENV{BLOSC_ROOT}) + set(_BLOSC_ROOT $ENV{BLOSC_ROOT}) +endif() + +# Additionally try and use pkconfig to find blosc +if(USE_PKGCONFIG) + if(NOT DEFINED PKG_CONFIG_FOUND) + find_package(PkgConfig) + endif() + if(PKG_CONFIG_FOUND) + pkg_check_modules(PC_Blosc QUIET blosc) + endif() +endif() + +# ------------------------------------------------------------------------ +# Search for blosc include DIR +# ------------------------------------------------------------------------ + +set(_BLOSC_INCLUDE_SEARCH_DIRS "") +list(APPEND _BLOSC_INCLUDE_SEARCH_DIRS + ${BLOSC_INCLUDEDIR} + ${_BLOSC_ROOT} + ${PC_Blosc_INCLUDE_DIRS} + ${SYSTEM_LIBRARY_PATHS} +) + +# Look for a standard blosc header file. +find_path(Blosc_INCLUDE_DIR blosc.h + ${_FIND_BLOSC_ADDITIONAL_OPTIONS} + PATHS ${_BLOSC_INCLUDE_SEARCH_DIRS} + PATH_SUFFIXES ${CMAKE_INSTALL_INCLUDEDIR} include +) + +if(EXISTS "${Blosc_INCLUDE_DIR}/blosc.h") + file(STRINGS "${Blosc_INCLUDE_DIR}/blosc.h" + _blosc_version_major_string REGEX "#define BLOSC_VERSION_MAJOR +[0-9]+ " + ) + string(REGEX REPLACE "#define BLOSC_VERSION_MAJOR +([0-9]+).*$" "\\1" + _blosc_version_major_string "${_blosc_version_major_string}" + ) + string(STRIP "${_blosc_version_major_string}" Blosc_VERSION_MAJOR) + + file(STRINGS "${Blosc_INCLUDE_DIR}/blosc.h" + _blosc_version_minor_string REGEX "#define BLOSC_VERSION_MINOR +[0-9]+ " + ) + string(REGEX REPLACE "#define BLOSC_VERSION_MINOR +([0-9]+).*$" "\\1" + _blosc_version_minor_string "${_blosc_version_minor_string}" + ) + string(STRIP "${_blosc_version_minor_string}" Blosc_VERSION_MINOR) + + file(STRINGS "${Blosc_INCLUDE_DIR}/blosc.h" + _blosc_version_release_string REGEX "#define BLOSC_VERSION_RELEASE +[0-9]+ " + ) + string(REGEX REPLACE "#define BLOSC_VERSION_RELEASE +([0-9]+).*$" "\\1" + _blosc_version_release_string "${_blosc_version_release_string}" + ) + string(STRIP "${_blosc_version_release_string}" Blosc_VERSION_RELEASE) + + unset(_blosc_version_major_string) + unset(_blosc_version_minor_string) + unset(_blosc_version_release_string) + + set(Blosc_VERSION ${Blosc_VERSION_MAJOR}.${Blosc_VERSION_MINOR}.${Blosc_VERSION_RELEASE}) +endif() + +# ------------------------------------------------------------------------ +# Search for blosc lib DIR +# ------------------------------------------------------------------------ + +set(_BLOSC_LIBRARYDIR_SEARCH_DIRS "") +list(APPEND _BLOSC_LIBRARYDIR_SEARCH_DIRS + ${BLOSC_LIBRARYDIR} + ${_BLOSC_ROOT} + ${PC_Blosc_LIBRARY_DIRS} + ${SYSTEM_LIBRARY_PATHS} +) + +# Library suffix handling + +set(_BLOSC_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES ${CMAKE_FIND_LIBRARY_SUFFIXES}) + +if(WIN32) + if(BLOSC_USE_STATIC_LIBS) + set(CMAKE_FIND_LIBRARY_SUFFIXES ".lib") + endif() +else() + if(BLOSC_USE_STATIC_LIBS) + set(CMAKE_FIND_LIBRARY_SUFFIXES ".a") + endif() +endif() + +set(Blosc_LIB_COMPONENTS "") +# NOTE: Search for debug version first (see vcpkg hack) +list(APPEND BLOSC_BUILD_TYPES DEBUG RELEASE) + +foreach(BUILD_TYPE ${BLOSC_BUILD_TYPES}) + set(_BLOSC_LIB_NAME blosc) + + set(_BLOSC_CMAKE_IGNORE_PATH ${CMAKE_IGNORE_PATH}) + if(VCPKG_TOOLCHAIN) + # Blosc is installed very strangely in VCPKG (debug/release libs have the + # same name, static build uses external deps, dll doesn't) and blosc itself + # comes with almost zero downstream CMake support for us to detect settings. + # We should not support external package managers in our own modules like + # this, but there doesn't seem to be a work around + if(NOT DEFINED BLOSC_DEBUG_SUFFIX) + set(BLOSC_DEBUG_SUFFIX "") + endif() + if(BUILD_TYPE STREQUAL RELEASE) + if(EXISTS ${Blosc_LIBRARY_DEBUG}) + get_filename_component(_BLOSC_DEBUG_DIR ${Blosc_LIBRARY_DEBUG} DIRECTORY) + list(APPEND CMAKE_IGNORE_PATH ${_BLOSC_DEBUG_DIR}) + endif() + endif() + endif() + + if(BUILD_TYPE STREQUAL DEBUG) + if(NOT DEFINED BLOSC_DEBUG_SUFFIX) + set(BLOSC_DEBUG_SUFFIX _d) + endif() + set(_BLOSC_LIB_NAME "${_BLOSC_LIB_NAME}${BLOSC_DEBUG_SUFFIX}") + endif() + + find_library(Blosc_LIBRARY_${BUILD_TYPE} ${_BLOSC_LIB_NAME} + ${_FIND_BLOSC_ADDITIONAL_OPTIONS} + PATHS ${_BLOSC_LIBRARYDIR_SEARCH_DIRS} + PATH_SUFFIXES ${CMAKE_INSTALL_LIBDIR} lib64 lib + ) + + list(APPEND Blosc_LIB_COMPONENTS ${Blosc_LIBRARY_${BUILD_TYPE}}) + set(CMAKE_IGNORE_PATH ${_BLOSC_CMAKE_IGNORE_PATH}) +endforeach() + +# Reset library suffix + +set(CMAKE_FIND_LIBRARY_SUFFIXES ${_BLOSC_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES}) +unset(_BLOSC_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES) + +if(Blosc_LIBRARY_DEBUG AND Blosc_LIBRARY_RELEASE) + # if the generator is multi-config or if CMAKE_BUILD_TYPE is set for + # single-config generators, set optimized and debug libraries + get_property(_isMultiConfig GLOBAL PROPERTY GENERATOR_IS_MULTI_CONFIG) + if(_isMultiConfig OR CMAKE_BUILD_TYPE) + set(Blosc_LIBRARY optimized ${Blosc_LIBRARY_RELEASE} debug ${Blosc_LIBRARY_DEBUG}) + else() + # For single-config generators where CMAKE_BUILD_TYPE has no value, + # just use the release libraries + set(Blosc_LIBRARY ${Blosc_LIBRARY_RELEASE}) + endif() + # FIXME: This probably should be set for both cases + set(Blosc_LIBRARIES optimized ${Blosc_LIBRARY_RELEASE} debug ${Blosc_LIBRARY_DEBUG}) +endif() + +# if only the release version was found, set the debug variable also to the release version +if(Blosc_LIBRARY_RELEASE AND NOT Blosc_LIBRARY_DEBUG) + set(Blosc_LIBRARY_DEBUG ${Blosc_LIBRARY_RELEASE}) + set(Blosc_LIBRARY ${Blosc_LIBRARY_RELEASE}) + set(Blosc_LIBRARIES ${Blosc_LIBRARY_RELEASE}) +endif() + +# if only the debug version was found, set the release variable also to the debug version +if(Blosc_LIBRARY_DEBUG AND NOT Blosc_LIBRARY_RELEASE) + set(Blosc_LIBRARY_RELEASE ${Blosc_LIBRARY_DEBUG}) + set(Blosc_LIBRARY ${Blosc_LIBRARY_DEBUG}) + set(Blosc_LIBRARIES ${Blosc_LIBRARY_DEBUG}) +endif() + +# If the debug & release library ends up being the same, omit the keywords +if("${Blosc_LIBRARY_RELEASE}" STREQUAL "${Blosc_LIBRARY_DEBUG}") + set(Blosc_LIBRARY ${Blosc_LIBRARY_RELEASE} ) + set(Blosc_LIBRARIES ${Blosc_LIBRARY_RELEASE} ) +endif() + +if(Blosc_LIBRARY) + set(Blosc_FOUND TRUE) +else() + set(Blosc_FOUND FALSE) +endif() + +# ------------------------------------------------------------------------ +# Cache and set Blosc_FOUND +# ------------------------------------------------------------------------ + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Blosc + FOUND_VAR Blosc_FOUND + REQUIRED_VARS + Blosc_LIBRARY + Blosc_INCLUDE_DIR + VERSION_VAR Blosc_VERSION +) + +if(NOT Blosc_FOUND) + if(Blosc_FIND_REQUIRED) + message(FATAL_ERROR "Unable to find Blosc") + endif() + return() +endif() + +# Partition release/debug lib vars + +set(Blosc_RELEASE_LIBRARIES ${Blosc_LIBRARY_RELEASE}) +get_filename_component(Blosc_RELEASE_LIBRARY_DIRS ${Blosc_RELEASE_LIBRARIES} DIRECTORY) +set(Blosc_DEBUG_LIBRARIES ${Blosc_LIBRARY_DEBUG}) +get_filename_component(Blosc_DEBUG_LIBRARY_DIRS ${Blosc_DEBUG_LIBRARIES} DIRECTORY) +set(Blosc_LIBRARIES ${Blosc_RELEASE_LIBRARIES}) +set(Blosc_LIBRARY_DIRS ${Blosc_RELEASE_LIBRARY_DIRS}) +set(Blosc_INCLUDE_DIRS ${Blosc_INCLUDE_DIR}) +set(Blosc_INCLUDE_DIRS ${Blosc_INCLUDE_DIR}) + +# Configure lib type. If XXX_USE_STATIC_LIBS, we always assume a static +# lib is in use. If win32, we can't mark the import .libs as shared, so +# these are always marked as UNKNOWN. Otherwise, infer from extension. +set(BLOSC_LIB_TYPE UNKNOWN) +if(BLOSC_USE_STATIC_LIBS) + set(BLOSC_LIB_TYPE STATIC) +elseif(UNIX) + get_filename_component(_BLOSC_EXT ${Blosc_LIBRARY_RELEASE} EXT) + if(_BLOSC_EXT STREQUAL ".a") + set(BLOSC_LIB_TYPE STATIC) + elseif(_BLOSC_EXT STREQUAL ".so" OR + _BLOSC_EXT STREQUAL ".dylib") + set(BLOSC_LIB_TYPE SHARED) + endif() +endif() + +get_filename_component(Blosc_LIBRARY_DIRS ${Blosc_LIBRARY_RELEASE} DIRECTORY) + +if(NOT TARGET Blosc::blosc) + add_library(Blosc::blosc ${BLOSC_LIB_TYPE} IMPORTED) + set_target_properties(Blosc::blosc PROPERTIES + INTERFACE_COMPILE_OPTIONS "${PC_Blosc_CFLAGS_OTHER}" + INTERFACE_INCLUDE_DIRECTORIES "${Blosc_INCLUDE_DIRS}") + + # Standard location + set_target_properties(Blosc::blosc PROPERTIES + IMPORTED_LINK_INTERFACE_LANGUAGES "CXX" + IMPORTED_LOCATION "${Blosc_LIBRARY}") + + # Release location + if(EXISTS "${Blosc_LIBRARY_RELEASE}") + set_property(TARGET Blosc::blosc APPEND PROPERTY + IMPORTED_CONFIGURATIONS RELEASE) + set_target_properties(Blosc::blosc PROPERTIES + IMPORTED_LINK_INTERFACE_LANGUAGES_RELEASE "CXX" + IMPORTED_LOCATION_RELEASE "${Blosc_LIBRARY_RELEASE}") + endif() + + # Debug location + if(EXISTS "${Blosc_LIBRARY_DEBUG}") + set_property(TARGET Blosc::blosc APPEND PROPERTY + IMPORTED_CONFIGURATIONS DEBUG) + set_target_properties(Blosc::blosc PROPERTIES + IMPORTED_LINK_INTERFACE_LANGUAGES_DEBUG "CXX" + IMPORTED_LOCATION_DEBUG "${Blosc_LIBRARY_DEBUG}") + endif() + + # Blosc may optionally be compiled with external sources for + # lz4, snappy and zlib . Add them as interface libs if requested + # (there doesn't seem to be a way to figure this out automatically). + # We assume they live along side blosc + if(BLOSC_USE_EXTERNAL_SOURCES) + set_target_properties(Blosc::blosc PROPERTIES + INTERFACE_LINK_DIRECTORIES + "\$<\$:${Blosc_RELEASE_LIBRARY_DIRS}>;\$<\$:${Blosc_DEBUG_LIBRARY_DIRS}>") + target_link_libraries(Blosc::blosc INTERFACE + $<$:lz4;snappy;zlib> + $<$:lz4d;snappyd;zlibd>) + + if(BLOSC_USE_STATIC_LIBS) + target_link_libraries(Blosc::blosc INTERFACE + $<$:zstd_static> + $<$:zstd_staticd>) + else() + target_link_libraries(Blosc::blosc INTERFACE + $<$:zstd> + $<$:zstdd>) + endif() + endif() +endif() + diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/CMakeModules/FindOpenVDB.cmake b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/CMakeModules/FindOpenVDB.cmake new file mode 100644 index 000000000..0dac1de40 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/CMakeModules/FindOpenVDB.cmake @@ -0,0 +1,838 @@ +# Copyright Contributors to the OpenVDB Project +# SPDX-License-Identifier: MPL-2.0 +# +#[=======================================================================[.rst: + +FindOpenVDB +----------- + +Find OpenVDB include dirs, libraries and settings + +Use this module by invoking find_package with the form:: + + find_package(OpenVDB + [version] [EXACT] # Minimum or EXACT version + [REQUIRED] # Fail with error if OpenVDB is not found + [COMPONENTS ...] # OpenVDB libraries by their canonical name + # e.g. "openvdb" for "libopenvdb", + # "pyopenvdb" for the python plugin + # "openvdb_ax" for the OpenVDB AX extension + # "openvdb_houdini" for the houdini plugin + # "nanovdb" for the nanovdb extension + ) + +IMPORTED Targets +^^^^^^^^^^^^^^^^ + +``OpenVDB::openvdb`` + The core openvdb library target. +``OpenVDB::openvdb_je`` + The core openvdb library target with jemalloc. +``OpenVDB::pyopenvdb`` + The openvdb python library target. +``OpenVDB::openvdb_houdini`` + The openvdb houdini library target. +``OpenVDB::openvdb_ax`` + The openvdb_ax library target. +``OpenVDB::nanovdb`` + The nanovdb library target. + +Result Variables +^^^^^^^^^^^^^^^^ + +This will define the following variables: + +``OpenVDB_FOUND`` + True if the system has the OpenVDB library. +``OpenVDB_VERSION`` + The version of the OpenVDB library which was found. +``OpenVDB_INCLUDE_DIRS`` + Include directories needed to use OpenVDB. +``OpenVDB_LIBRARIES`` + Libraries needed to link to OpenVDB. +``OpenVDB_LIBRARY_DIRS`` + OpenVDB library directories. +``OpenVDB_DEFINITIONS`` + Definitions to use when compiling code that uses OpenVDB. +``OpenVDB_${COMPONENT}_FOUND`` + True if the system has the named OpenVDB component. +``OpenVDB_USES_BLOSC`` + True if the OpenVDB Library has been built with blosc support +``OpenVDB_USES_ZLIB`` + True if the OpenVDB Library has been built with zlib support +``OpenVDB_USES_LOG4CPLUS`` + True if the OpenVDB Library has been built with log4cplus support +``OpenVDB_USES_IMATH_HALF`` + True if the OpenVDB Library has been built with Imath half support +``OpenVDB_ABI`` + Set if this module was able to determine the ABI number the located + OpenVDB Library was built against. Unset otherwise. + +Cache Variables +^^^^^^^^^^^^^^^ + +The following cache variables may also be set: + +``OpenVDB_INCLUDE_DIR`` + The directory containing ``openvdb/version.h``. +``OpenVDB_${COMPONENT}_INCLUDE_DIR`` + Individual component include directories for OpenVDB +``OpenVDB_${COMPONENT}_LIBRARY`` + Individual component libraries for OpenVDB + +Hints +^^^^^ + +Instead of explicitly setting the cache variables, the following variables +may be provided to tell this module where to look. + +``OpenVDB_ROOT`` + Preferred installation prefix. +``OPENVDB_INCLUDEDIR`` + Preferred include directory e.g. /include +``OPENVDB_LIBRARYDIR`` + Preferred library directory e.g. /lib +``OPENVDB_${COMPONENT}_ROOT`` + Preferred installation prefix of a specific component. +``OPENVDB_${COMPONENT}_INCLUDEDIR`` + Preferred include directory of a specific component e.g. /include +``OPENVDB_${COMPONENT}_LIBRARYDIR`` + Preferred library directory of a specific component e.g. /lib +``SYSTEM_LIBRARY_PATHS`` + Global list of library paths intended to be searched by and find_xxx call +``OPENVDB_USE_STATIC_LIBS`` + Only search for static openvdb libraries +``DISABLE_CMAKE_SEARCH_PATHS`` + Disable CMakes default search paths for find_xxx calls in this module + +#]=======================================================================] + +cmake_minimum_required(VERSION 3.15) +include(GNUInstallDirs) + + +# Include utility functions for version information +include(${CMAKE_CURRENT_LIST_DIR}/OpenVDBUtils.cmake) + +mark_as_advanced( + OpenVDB_INCLUDE_DIR + OpenVDB_LIBRARY +) + +set(_FIND_OPENVDB_ADDITIONAL_OPTIONS "") +if(DISABLE_CMAKE_SEARCH_PATHS) + set(_FIND_OPENVDB_ADDITIONAL_OPTIONS NO_DEFAULT_PATH) +endif() + +set(_OPENVDB_COMPONENT_LIST + openvdb + openvdb_je + pyopenvdb + openvdb_ax + openvdb_houdini + nanovdb +) + +if(OpenVDB_FIND_COMPONENTS) + set(OPENVDB_COMPONENTS_PROVIDED TRUE) + set(_IGNORED_COMPONENTS "") + foreach(COMPONENT ${OpenVDB_FIND_COMPONENTS}) + if(NOT ${COMPONENT} IN_LIST _OPENVDB_COMPONENT_LIST) + list(APPEND _IGNORED_COMPONENTS ${COMPONENT}) + endif() + endforeach() + + if(_IGNORED_COMPONENTS) + message(STATUS "Ignoring unknown components of OpenVDB:") + foreach(COMPONENT ${_IGNORED_COMPONENTS}) + message(STATUS " ${COMPONENT}") + endforeach() + list(REMOVE_ITEM OpenVDB_FIND_COMPONENTS ${_IGNORED_COMPONENTS}) + endif() +else() + set(OPENVDB_COMPONENTS_PROVIDED FALSE) + set(OpenVDB_FIND_COMPONENTS openvdb) +endif() + +# always make sure openvdb is picked up as a component i.e. +# find_package(OpenVDB COMPONENTS pyopenvdb) results in both +# openvdb and pyopenvdb targets. Also make sure it appears +# first in the component lists. +list(INSERT OpenVDB_FIND_COMPONENTS 0 openvdb) +list(REMOVE_DUPLICATES OpenVDB_FIND_COMPONENTS) + +# Set _OPENVDB_ROOT based on a user provided root var. Xxx_ROOT and ENV{Xxx_ROOT} +# are prioritised over the legacy capitalized XXX_ROOT variables for matching +# CMake 3.12 behaviour +# @todo deprecate -D and ENV OPENVDB_ROOT from CMake 3.12 +if(OpenVDB_ROOT) + set(_OPENVDB_ROOT ${OpenVDB_ROOT}) +elseif(DEFINED ENV{OpenVDB_ROOT}) + set(_OPENVDB_ROOT $ENV{OpenVDB_ROOT}) +elseif(OPENVDB_ROOT) + set(_OPENVDB_ROOT ${OPENVDB_ROOT}) +elseif(DEFINED ENV{OPENVDB_ROOT}) + set(_OPENVDB_ROOT $ENV{OPENVDB_ROOT}) +endif() + +# Additionally try and use pkconfig to find OpenVDB +if(USE_PKGCONFIG) + if(NOT DEFINED PKG_CONFIG_FOUND) + find_package(PkgConfig) + endif() + pkg_check_modules(PC_OpenVDB QUIET OpenVDB) +endif() + +# This CMake module supports being called from external packages AND from +# within the OpenVDB repository for building openvdb components with the +# core library build disabled. Determine where we are being called from: +# +# (repo structure = /cmake/FindOpenVDB.cmake) +# (inst structure = /lib/cmake/OpenVDB/FindOpenVDB.cmake) + +get_filename_component(_DIR_NAME ${CMAKE_CURRENT_LIST_DIR} NAME) + +if(${_DIR_NAME} STREQUAL "cmake") + # Called from root repo for openvdb components +elseif(${_DIR_NAME} STREQUAL "OpenVDB") + # Set the install variable to track directories if this is being called from + # an installed location and from another package. The expected installation + # directory structure is: + # /lib/cmake/OpenVDB/FindOpenVDB.cmake + # /include + # /bin + get_filename_component(_IMPORT_PREFIX ${CMAKE_CURRENT_LIST_DIR} DIRECTORY) + get_filename_component(_IMPORT_PREFIX ${_IMPORT_PREFIX} DIRECTORY) + get_filename_component(_IMPORT_PREFIX ${_IMPORT_PREFIX} DIRECTORY) + set(_OPENVDB_INSTALL ${_IMPORT_PREFIX}) + list(APPEND _OPENVDB_ROOT ${_OPENVDB_INSTALL}) +endif() + +unset(_DIR_NAME) +unset(_IMPORT_PREFIX) + +# ------------------------------------------------------------------------ +# Search for OpenVDB include DIR +# ------------------------------------------------------------------------ + +set(_OPENVDB_INCLUDE_SEARCH_DIRS "") +list(APPEND _OPENVDB_INCLUDE_SEARCH_DIRS + ${OPENVDB_INCLUDEDIR} + ${_OPENVDB_ROOT} + ${PC_OpenVDB_INCLUDE_DIRS} + ${SYSTEM_LIBRARY_PATHS} +) + +foreach(COMPONENT ${OpenVDB_FIND_COMPONENTS}) + # Add in extra component paths + set(_VDB_COMPONENT_SEARCH_DIRS ${_OPENVDB_INCLUDE_SEARCH_DIRS}) + list(APPEND _VDB_COMPONENT_SEARCH_DIRS + ${OPENVDB_${COMPONENT}_ROOT} + ${OPENVDB_${COMPONENT}_INCLUDEDIR} + ) + if(_VDB_COMPONENT_SEARCH_DIRS) + list(REMOVE_DUPLICATES _VDB_COMPONENT_SEARCH_DIRS) + endif() + + # Look for a standard header files. + if(${COMPONENT} STREQUAL "openvdb") + # Look for a standard OpenVDB header file. + find_path(OpenVDB_${COMPONENT}_INCLUDE_DIR openvdb/version.h + ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} + PATHS ${_VDB_COMPONENT_SEARCH_DIRS} + PATH_SUFFIXES + ${CMAKE_INSTALL_INCLUDEDIR} + include + ) + elseif(${COMPONENT} STREQUAL "pyopenvdb") + find_path(OpenVDB_${COMPONENT}_INCLUDE_DIR pyopenvdb.h + ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} + PATHS ${_VDB_COMPONENT_SEARCH_DIRS} + PATH_SUFFIXES + ${CMAKE_INSTALL_INCLUDEDIR}/openvdb/python + ${CMAKE_INSTALL_INCLUDEDIR}/openvdb + ${CMAKE_INSTALL_INCLUDEDIR} + include + ) + elseif(${COMPONENT} STREQUAL "openvdb_ax") + # Look for a standard OpenVDB header file. + find_path(OpenVDB_${COMPONENT}_INCLUDE_DIR compiler/Compiler.h + ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} + PATHS ${_VDB_COMPONENT_SEARCH_DIRS} + PATH_SUFFIXES + ${CMAKE_INSTALL_INCLUDEDIR}/openvdb/openvdb_ax + ${CMAKE_INSTALL_INCLUDEDIR}/openvdb_ax + ${CMAKE_INSTALL_INCLUDEDIR} + include + ) + elseif(${COMPONENT} STREQUAL "openvdb_houdini") + # @note Expects both houdini_utils and openvdb_houdini folders + # to be located in the same place + find_path(OpenVDB_${COMPONENT}_INCLUDE_DIR openvdb_houdini/SOP_NodeVDB.h + ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} + PATHS ${_VDB_COMPONENT_SEARCH_DIRS} + PATH_SUFFIXES + ${CMAKE_INSTALL_INCLUDEDIR}/openvdb + ${CMAKE_INSTALL_INCLUDEDIR} + include + ) + elseif(${COMPONENT} STREQUAL "nanovdb") + # Look for NanoVDB.h + find_path(OpenVDB_${COMPONENT}_INCLUDE_DIR NanoVDB.h + ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} + PATHS ${_VDB_COMPONENT_SEARCH_DIRS} + PATH_SUFFIXES + ${CMAKE_INSTALL_INCLUDEDIR}/nanovdb + ${CMAKE_INSTALL_INCLUDEDIR} + include + ) + endif() + unset(_VDB_COMPONENT_SEARCH_DIRS) +endforeach() + +set(OpenVDB_INCLUDE_DIR ${OpenVDB_openvdb_INCLUDE_DIR} + CACHE PATH "The OpenVDB core include directory") + +set(_OPENVDB_VERSION_HEADER "${OpenVDB_INCLUDE_DIR}/openvdb/version.h") +OPENVDB_VERSION_FROM_HEADER("${_OPENVDB_VERSION_HEADER}" + VERSION OpenVDB_VERSION + MAJOR OpenVDB_MAJOR_VERSION + MINOR OpenVDB_MINOR_VERSION + PATCH OpenVDB_PATCH_VERSION + ABI OpenVDB_ABI_FROM_HEADER # will be OpenVDB_MAJOR_VERSION prior to 8.1.0 +) + +if(OpenVDB_VERSION VERSION_LESS 8.1.0) + set(_OPENVDB_HAS_NEW_VERSION_HEADER FALSE) + # ABI gets computed later +else() + set(_OPENVDB_HAS_NEW_VERSION_HEADER TRUE) + set(OpenVDB_ABI ${OpenVDB_ABI_FROM_HEADER}) +endif() +unset(OpenVDB_ABI_FROM_HEADER) + +# ------------------------------------------------------------------------ +# Search for OPENVDB lib DIR +# ------------------------------------------------------------------------ + +set(_OPENVDB_LIBRARYDIR_SEARCH_DIRS "") + +# Append to _OPENVDB_LIBRARYDIR_SEARCH_DIRS in priority order + +list(APPEND _OPENVDB_LIBRARYDIR_SEARCH_DIRS + ${OPENVDB_LIBRARYDIR} + ${_OPENVDB_ROOT} + ${PC_OpenVDB_LIBRARY_DIRS} + ${SYSTEM_LIBRARY_PATHS} +) + +# Library suffix handling + +set(_OPENVDB_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES ${CMAKE_FIND_LIBRARY_SUFFIXES}) + +set(OPENVDB_PYTHON_PATH_SUFFIXES + lib64/python + lib64/python2.7 + lib64/python3 + lib/python + lib/python2.7 + lib/python3 +) + +# Recurse through all the site-packages and dist-packages on the file system +file(GLOB PYTHON_SITE_PACKAGES ${CMAKE_INSTALL_FULL_LIBDIR}/python**/*) +foreach(_site_package_full_dir ${PYTHON_SITE_PACKAGES}) + string(REPLACE ${CMAKE_INSTALL_FULL_LIBDIR} "${CMAKE_INSTALL_LIBDIR}" + _site_package_dir ${_site_package_full_dir}) + list(APPEND OPENVDB_PYTHON_PATH_SUFFIXES ${_site_package_dir}) +endforeach() + +set(OPENVDB_LIB_PATH_SUFFIXES + ${CMAKE_INSTALL_LIBDIR} + lib64 + lib +) + +list(REMOVE_DUPLICATES OPENVDB_PYTHON_PATH_SUFFIXES) +list(REMOVE_DUPLICATES OPENVDB_LIB_PATH_SUFFIXES) + +# Static library setup +if(WIN32) + if(OPENVDB_USE_STATIC_LIBS) + set(CMAKE_FIND_LIBRARY_SUFFIXES ".lib") + endif() +else() + if(OPENVDB_USE_STATIC_LIBS) + set(CMAKE_FIND_LIBRARY_SUFFIXES ".a") + endif() +endif() + +set(OpenVDB_LIB_COMPONENTS "") + +foreach(COMPONENT ${OpenVDB_FIND_COMPONENTS}) + message("COMPONENT = " ${COMPONENT}) + set(LIB_NAME ${COMPONENT}) + + # Add in extra component paths + set(_VDB_COMPONENT_SEARCH_DIRS ${_OPENVDB_LIBRARYDIR_SEARCH_DIRS}) + list(APPEND _VDB_COMPONENT_SEARCH_DIRS + ${OPENVDB_${COMPONENT}_ROOT} + ${OPENVDB_${COMPONENT}_LIBRARYDIR} + ) + + if(${COMPONENT} STREQUAL "pyopenvdb") + set(_OPENVDB_ORIG_CMAKE_FIND_LIBRARY_PREFIXES ${CMAKE_FIND_LIBRARY_PREFIXES}) + set(CMAKE_FIND_LIBRARY_PREFIXES ";lib") # find non-prefixed + find_library(OpenVDB_${COMPONENT}_LIBRARY ${LIB_NAME} + ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} + PATHS ${_VDB_COMPONENT_SEARCH_DIRS} + PATH_SUFFIXES ${OPENVDB_PYTHON_PATH_SUFFIXES} + ) + set(CMAKE_FIND_LIBRARY_PREFIXES ${_OPENVDB_ORIG_CMAKE_FIND_LIBRARY_PREFIXES}) + elseif(${COMPONENT} STREQUAL "openvdb_je") + # alias to the result of openvdb which should be handled first + set(OpenVDB_${COMPONENT}_LIBRARY ${OpenVDB_openvdb_LIBRARY}) + else() + find_library(OpenVDB_${COMPONENT}_LIBRARY ${LIB_NAME} + ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} + PATHS ${_VDB_COMPONENT_SEARCH_DIRS} + PATH_SUFFIXES ${OPENVDB_LIB_PATH_SUFFIXES} + ) + endif() + + list(APPEND OpenVDB_LIB_COMPONENTS ${OpenVDB_${COMPONENT}_LIBRARY}) + if(${COMPONENT} STREQUAL "nanovdb") + # nanovdb is headers-only, no lib component + if(OpenVDB_${COMPONENT}_INCLUDE_DIR) + set(OpenVDB_${COMPONENT}_FOUND TRUE) + else() + set(OpenVDB_${COMPONENT}_FOUND FALSE) + endif() + else() + if(OpenVDB_${COMPONENT}_LIBRARY) + set(OpenVDB_${COMPONENT}_FOUND TRUE) + else() + set(OpenVDB_${COMPONENT}_FOUND FALSE) + endif() + endif() + unset(_VDB_COMPONENT_SEARCH_DIRS) +endforeach() + +unset(OPENVDB_PYTHON_PATH_SUFFIXES) +unset(OPENVDB_LIB_PATH_SUFFIXES) + +# Reset library suffix + +set(CMAKE_FIND_LIBRARY_SUFFIXES ${_OPENVDB_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES}) +unset(_OPENVDB_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES) + +# ------------------------------------------------------------------------ +# Cache and set OPENVDB_FOUND +# ------------------------------------------------------------------------ +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(OpenVDB + FOUND_VAR OpenVDB_FOUND + REQUIRED_VARS + OpenVDB_INCLUDE_DIR + OpenVDB_LIB_COMPONENTS + VERSION_VAR OpenVDB_VERSION + HANDLE_COMPONENTS +) + +# ------------------------------------------------------------------------ +# Determine ABI number +# ------------------------------------------------------------------------ + +# Set the ABI number the library was built against. The old system, +# which didn't define the ABI in the build config, uses vdb_print + +if(NOT _OPENVDB_HAS_NEW_VERSION_HEADER) + if(_OPENVDB_INSTALL) + OPENVDB_ABI_VERSION_FROM_PRINT( + "${_OPENVDB_INSTALL}/bin/vdb_print" + ABI OpenVDB_ABI + ) + else() + # Try and find vdb_print from the include path + OPENVDB_ABI_VERSION_FROM_PRINT( + "${OpenVDB_INCLUDE_DIR}/../bin/vdb_print" + ABI OpenVDB_ABI + ) + endif() +endif() + +if(NOT OpenVDB_FIND_QUIET) + if(NOT OpenVDB_ABI) + message(WARNING "Unable to determine OpenVDB ABI version from OpenVDB " + "installation. The library major version \"${OpenVDB_MAJOR_VERSION}\" " + "will be inferred. If this is not correct, use " + "add_definitions(-DOPENVDB_ABI_VERSION_NUMBER=N)" + ) + else() + message(STATUS "OpenVDB ABI Version: ${OpenVDB_ABI}") + endif() +endif() + +# ------------------------------------------------------------------------ +# Handle OpenVDB dependencies and interface settings +# ------------------------------------------------------------------------ + +# Handle openvdb_houdini first to configure search paths + +if(openvdb_houdini IN_LIST OpenVDB_FIND_COMPONENTS) + include(OpenVDBHoudiniSetup) +endif() + +# Add standard dependencies + +find_package(TBB REQUIRED COMPONENTS tbb) + +if(NOT OPENVDB_USE_STATIC_LIBS AND NOT Boost_USE_STATIC_LIBS) + # @note Both of these must be set for Boost 1.70 (VFX2020) to link against + # boost shared libraries (more specifically libraries built with -fPIC). + # http://boost.2283326.n4.nabble.com/CMake-config-scripts-broken-in-1-70-td4708957.html + # https://github.com/boostorg/boost_install/commit/160c7cb2b2c720e74463865ef0454d4c4cd9ae7c + set(BUILD_SHARED_LIBS ON) + set(Boost_USE_STATIC_LIBS OFF) +endif() + +find_package(Boost REQUIRED COMPONENTS iostreams system) + +# Add deps for pyopenvdb +# @todo track for numpy + +if(pyopenvdb IN_LIST OpenVDB_FIND_COMPONENTS) + find_package(PythonLibs REQUIRED) + + # Boost python handling - try and find both python and pythonXx (version suffixed). + # Prioritize the version suffixed library, failing if neither exist. + + find_package(Boost ${MINIMUM_BOOST_VERSION} + QUIET COMPONENTS python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR} + ) + + if(TARGET Boost::python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}) + set(BOOST_PYTHON_LIB "python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}") + message(STATUS "Found boost_python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}") + else() + find_package(Boost ${MINIMUM_BOOST_VERSION} QUIET COMPONENTS python) + if(TARGET Boost::python) + set(BOOST_PYTHON_LIB "python") + message(STATUS "Found non-suffixed boost_python, assuming to be python version " + "\"${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}\" compatible" + ) + else() + message(FATAL_ERROR "Unable to find boost_python or " + "boost_python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}." + ) + endif() + endif() +endif() + +# Add deps for openvdb_ax + +if(openvdb_ax IN_LIST OpenVDB_FIND_COMPONENTS) + find_package(LLVM REQUIRED) + find_library(found_LLVM LLVM HINTS ${LLVM_LIBRARY_DIRS}) + + if(found_LLVM) + set(LLVM_LIBS "LLVM") + else() + llvm_map_components_to_libnames(_llvm_libs + native core executionengine support mcjit passes objcarcopts) + set(LLVM_LIBS "${_llvm_libs}") + endif() + + if(NOT OpenVDB_FIND_QUIET) + message(STATUS "Found LLVM: ${LLVM_DIR} (found version \"${LLVM_PACKAGE_VERSION}\")") + endif() +endif() + +# As the way we resolve optional libraries relies on library file names, use +# the configuration options from the main CMakeLists.txt to allow users +# to manually identify the requirements of OpenVDB builds if they know them. +set(OpenVDB_USES_BLOSC ${USE_BLOSC}) +set(OpenVDB_USES_ZLIB ${USE_ZLIB}) +set(OpenVDB_USES_LOG4CPLUS ${USE_LOG4CPLUS}) +set(OpenVDB_USES_IMATH_HALF ${USE_IMATH_HALF}) +set(OpenVDB_DEFINITIONS) + +if(WIN32) + if(OPENVDB_USE_STATIC_LIBS) + list(APPEND OpenVDB_DEFINITIONS OPENVDB_STATICLIB) + else() + list(APPEND OpenVDB_DEFINITIONS OPENVDB_DLL) + endif() + # Newer version of OpenVDB define these in Platform.h, but they are also + # provided here to maintain backwards compatibility with header include + # others + list(APPEND OpenVDB_DEFINITIONS _WIN32) + list(APPEND OpenVDB_DEFINITIONS NOMINMAX) +endif() + +if(OpenVDB_ABI) + # Newer version of OpenVDB defines this in version.h, but it is are also + # provided here to maintain backwards compatibility with header include + # others + list(APPEND OpenVDB_DEFINITIONS OPENVDB_ABI_VERSION_NUMBER=${OpenVDB_ABI}) +endif() + +# Configure deps + +if(_OPENVDB_HAS_NEW_VERSION_HEADER) + OPENVDB_GET_VERSION_DEFINE(${_OPENVDB_VERSION_HEADER} "OPENVDB_USE_IMATH_HALF" OpenVDB_USES_IMATH_HALF) + OPENVDB_GET_VERSION_DEFINE(${_OPENVDB_VERSION_HEADER} "OPENVDB_USE_BLOSC" OpenVDB_USES_BLOSC) + OPENVDB_GET_VERSION_DEFINE(${_OPENVDB_VERSION_HEADER} "OPENVDB_USE_ZLIB" OpenVDB_USES_ZLIB) +elseif(NOT OPENVDB_USE_STATIC_LIBS) + # Use GetPrerequisites to see which libraries this OpenVDB lib has linked to + # which we can query for optional deps. This basically runs ldd/otoll/objdump + # etc to track deps. We could use a vdb_config binary tools here to improve + # this process + include(GetPrerequisites) + + set(_EXCLUDE_SYSTEM_PREREQUISITES 1) + set(_RECURSE_PREREQUISITES 0) + set(_OPENVDB_PREREQUISITE_LIST) + + get_prerequisites(${OpenVDB_openvdb_LIBRARY} + _OPENVDB_PREREQUISITE_LIST + ${_EXCLUDE_SYSTEM_PREREQUISITES} + ${_RECURSE_PREREQUISITES} + "" + "${SYSTEM_LIBRARY_PATHS}" + ) + + unset(_EXCLUDE_SYSTEM_PREREQUISITES) + unset(_RECURSE_PREREQUISITES) + + # Search for optional dependencies + foreach(PREREQUISITE ${_OPENVDB_PREREQUISITE_LIST}) + set(_HAS_DEP) + get_filename_component(PREREQUISITE ${PREREQUISITE} NAME) + + string(FIND ${PREREQUISITE} "blosc" _HAS_DEP) + if(NOT ${_HAS_DEP} EQUAL -1) + set(OpenVDB_USES_BLOSC ON) + endif() + + string(FIND ${PREREQUISITE} "zlib" _HAS_DEP) + if(NOT ${_HAS_DEP} EQUAL -1) + set(OpenVDB_USES_ZLIB ON) + endif() + + string(FIND ${PREREQUISITE} "log4cplus" _HAS_DEP) + if(NOT ${_HAS_DEP} EQUAL -1) + set(OpenVDB_USES_LOG4CPLUS ON) + endif() + + string(FIND ${PREREQUISITE} "Half" _HAS_DEP) + if(NOT ${_HAS_DEP} EQUAL -1) + set(OpenVDB_USES_IMATH_HALF ON) + endif() + endforeach() + + unset(_OPENVDB_PREREQUISITE_LIST) +endif() + +if(OpenVDB_USES_BLOSC) + find_package(Blosc REQUIRED) +endif() + +if(OpenVDB_USES_ZLIB) + find_package(ZLIB REQUIRED) +endif() + +if(OpenVDB_USES_LOG4CPLUS) + find_package(Log4cplus REQUIRED) +endif() + +if(OpenVDB_USES_IMATH_HALF) + find_package(Imath CONFIG) + if (NOT TARGET Imath::Imath) + find_package(IlmBase REQUIRED COMPONENTS Half) + endif() + + if(WIN32) + # @note OPENVDB_OPENEXR_STATICLIB is old functionality and should be removed + if(OPENEXR_USE_STATIC_LIBS OR + (${ILMBASE_LIB_TYPE} STREQUAL STATIC_LIBRARY) OR + (${IMATH_LIB_TYPE} STREQUAL STATIC_LIBRARY)) + list(APPEND OpenVDB_DEFINITIONS OPENVDB_OPENEXR_STATICLIB) + endif() + endif() +endif() + +if(UNIX) + find_package(Threads REQUIRED) +endif() + +# Set deps. Note that the order here is important. If we're building against +# Houdini 17.5 we must include IlmBase deps first to ensure the users chosen +# namespaced headers are correctly prioritized. Otherwise other include paths +# from shared installs (including houdini) may pull in the wrong headers + +set(_OPENVDB_VISIBLE_DEPENDENCIES + Boost::iostreams + Boost::system +) + +if(OpenVDB_USES_IMATH_HALF) + list(APPEND _OPENVDB_VISIBLE_DEPENDENCIES $ $) +endif() + +if(OpenVDB_USES_LOG4CPLUS) + list(APPEND _OPENVDB_VISIBLE_DEPENDENCIES Log4cplus::log4cplus) + list(APPEND OpenVDB_DEFINITIONS OPENVDB_USE_LOG4CPLUS) +endif() + +list(APPEND _OPENVDB_VISIBLE_DEPENDENCIES + TBB::tbb +) +if(UNIX) + list(APPEND _OPENVDB_VISIBLE_DEPENDENCIES + Threads::Threads + ) +endif() + +set(_OPENVDB_HIDDEN_DEPENDENCIES) + +if(NOT OPENVDB_USE_STATIC_LIBS) + if(OpenVDB_USES_BLOSC) + list(APPEND _OPENVDB_HIDDEN_DEPENDENCIES Blosc::blosc) + endif() + if(OpenVDB_USES_ZLIB) + list(APPEND _OPENVDB_HIDDEN_DEPENDENCIES ZLIB::ZLIB) + endif() +endif() + +if(openvdb_je IN_LIST OpenVDB_FIND_COMPONENTS) + find_package(Jemalloc REQUIRED) +endif() + +# ------------------------------------------------------------------------ +# Configure imported targets +# ------------------------------------------------------------------------ + +set(OpenVDB_LIBRARIES ${OpenVDB_LIB_COMPONENTS}) +set(OpenVDB_INCLUDE_DIRS ${OpenVDB_INCLUDE_DIR}) + +set(OpenVDB_LIBRARY_DIRS "") +foreach(LIB ${OpenVDB_LIB_COMPONENTS}) + get_filename_component(_OPENVDB_LIBDIR ${LIB} DIRECTORY) + list(APPEND OpenVDB_LIBRARY_DIRS ${_OPENVDB_LIBDIR}) +endforeach() +list(REMOVE_DUPLICATES OpenVDB_LIBRARY_DIRS) + +# OpenVDB::openvdb + +if(NOT TARGET OpenVDB::openvdb) + set(OPENVDB_openvdb_LIB_TYPE UNKNOWN) + if(OPENVDB_USE_STATIC_LIBS) + set(OPENVDB_openvdb_LIB_TYPE STATIC) + elseif(UNIX) + get_filename_component(_OPENVDB_openvdb_EXT + ${OpenVDB_openvdb_LIBRARY} EXT) + if(_OPENVDB_openvdb_EXT STREQUAL ".a") + set(OPENVDB_openvdb_LIB_TYPE STATIC) + elseif(_OPENVDB_openvdb_EXT STREQUAL ".so" OR + _OPENVDB_openvdb_EXT STREQUAL ".dylib") + set(OPENVDB_openvdb_LIB_TYPE SHARED) + endif() + endif() + + add_library(OpenVDB::openvdb ${OPENVDB_openvdb_LIB_TYPE} IMPORTED) + set_target_properties(OpenVDB::openvdb PROPERTIES + IMPORTED_LOCATION "${OpenVDB_openvdb_LIBRARY}" + INTERFACE_COMPILE_OPTIONS "${PC_OpenVDB_CFLAGS_OTHER}" + INTERFACE_COMPILE_DEFINITIONS "${OpenVDB_DEFINITIONS}" + INTERFACE_INCLUDE_DIRECTORIES "${OpenVDB_INCLUDE_DIR}" + IMPORTED_LINK_DEPENDENT_LIBRARIES "${_OPENVDB_HIDDEN_DEPENDENCIES}" # non visible deps + INTERFACE_LINK_LIBRARIES "${_OPENVDB_VISIBLE_DEPENDENCIES}" # visible deps (headers) + INTERFACE_COMPILE_FEATURES cxx_std_14 + ) +endif() + +# OpenVDB::openvdb_je + +if(OpenVDB_openvdb_je_LIBRARY) + if(NOT TARGET OpenVDB::openvdb_je) + add_library(OpenVDB::openvdb_je INTERFACE IMPORTED) + target_link_libraries(OpenVDB::openvdb_je INTERFACE OpenVDB::openvdb) + target_link_libraries(OpenVDB::openvdb_je INTERFACE Jemalloc::jemalloc) + endif() +endif() + +# OpenVDB::pyopenvdb + +if(OpenVDB_pyopenvdb_LIBRARY) + if(NOT TARGET OpenVDB::pyopenvdb) + add_library(OpenVDB::pyopenvdb MODULE IMPORTED) + set_target_properties(OpenVDB::pyopenvdb PROPERTIES + IMPORTED_LOCATION "${OpenVDB_pyopenvdb_LIBRARY}" + INTERFACE_INCLUDE_DIRECTORIES "${OpenVDB_pyopenvdb_INCLUDE_DIR};${PYTHON_INCLUDE_DIR}" + INTERFACE_LINK_LIBRARIES "OpenVDB::openvdb;Boost::${BOOST_PYTHON_LIB};${PYTHON_LIBRARIES}" + INTERFACE_COMPILE_FEATURES cxx_std_14 + ) + endif() +endif() + +# OpenVDB::openvdb_houdini + +if(OpenVDB_openvdb_houdini_LIBRARY) + if(NOT TARGET OpenVDB::openvdb_houdini) + add_library(OpenVDB::openvdb_houdini SHARED IMPORTED) + set_target_properties(OpenVDB::openvdb_houdini PROPERTIES + IMPORTED_LOCATION "${OpenVDB_openvdb_houdini_LIBRARY}" + INTERFACE_INCLUDE_DIRECTORIES "${OpenVDB_openvdb_houdini_INCLUDE_DIR}" + INTERFACE_LINK_LIBRARIES "OpenVDB::openvdb;Houdini" + INTERFACE_COMPILE_FEATURES cxx_std_14 + ) + endif() +endif() + +# OpenVDB::openvdb_ax + +if(OpenVDB_openvdb_ax_LIBRARY) + set(OPENVDB_openvdb_ax_LIB_TYPE UNKNOWN) + if(OPENVDB_USE_STATIC_LIBS) + set(OPENVDB_openvdb_ax_LIB_TYPE STATIC) + elseif(UNIX) + get_filename_component(_OPENVDB_openvdb_ax_EXT + ${OpenVDB_openvdb_ax_LIBRARY} EXT) + if(_OPENVDB_openvdb_ax_EXT STREQUAL ".a") + set(OPENVDB_openvdb_ax_LIB_TYPE STATIC) + elseif(_OPENVDB_openvdb_ax_EXT STREQUAL ".so" OR + _OPENVDB_openvdb_ax_EXT STREQUAL ".dylib") + set(OPENVDB_openvdb_ax_LIB_TYPE SHARED) + endif() + endif() + + + if(NOT TARGET OpenVDB::openvdb_ax) + add_library(OpenVDB::openvdb_ax UNKNOWN IMPORTED) + set_target_properties(OpenVDB::openvdb_ax PROPERTIES + IMPORTED_LOCATION "${OpenVDB_openvdb_ax_LIBRARY}" + INTERFACE_INCLUDE_DIRECTORIES "${OpenVDB_openvdb_ax_INCLUDE_DIR}" + INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${LLVM_INCLUDE_DIRS}" + INTERFACE_LINK_LIBRARIES "OpenVDB::openvdb;${LLVM_LIBS}" + INTERFACE_COMPILE_FEATURES cxx_std_14 + ) + endif() +endif() + +# OpenVDB::nanovdb + +if(OpenVDB_nanovdb_LIBRARY) + if(NOT TARGET OpenVDB::nanovdb) + add_library(OpenVDB::nanovdb INTERFACE IMPORTED) + set_target_properties(OpenVDB::nanovdb PROPERTIES + IMPORTED_LOCATION "${OpenVDB_nanovdb_LIBRARY}" + INTERFACE_INCLUDE_DIRECTORIES "${OpenVDB_nanovdb_INCLUDE_DIR}" + INTERFACE_LINK_LIBRARIES "OpenVDB::openvdb;" + INTERFACE_COMPILE_FEATURES cxx_std_14 + ) + endif() +endif() + +unset(_OPENVDB_VISIBLE_DEPENDENCIES) +unset(_OPENVDB_HIDDEN_DEPENDENCIES) diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/CMakeModules/OpenVDBUtils.cmake b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/CMakeModules/OpenVDBUtils.cmake new file mode 100644 index 000000000..084f27d71 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/CMakeModules/OpenVDBUtils.cmake @@ -0,0 +1,164 @@ +# Copyright Contributors to the OpenVDB Project +# SPDX-License-Identifier: MPL-2.0 +# +#[=======================================================================[.rst: + +OpenVDBUtils.cmake +------------------ + +A utility CMake file which provides helper functions for configuring an +OpenVDB installation. + +Use this module by invoking include with the form:: + + include ( OpenVDBUtils ) + + +The following functions are provided: + +``OPENVDB_GET_VERSION_DEFINE`` + + OPENVDB_GET_VERSION_DEFINE ( ) + + Parse the provided version file to retrieve the a given OpenVDB define + specified by and store the result in . If the define has a + value, the value is stored. If the define has no value but exists, ON is + stored. Else, OFF is stored. + + If the file does not exist, is unmodified. + +``OPENVDB_VERSION_FROM_HEADER`` + + OPENVDB_VERSION_FROM_HEADER ( + VERSION [] + MAJOR [] + MINOR [] + PATCH [] ) + + Parse the provided version file to retrieve the current OpenVDB + version information. The file is expected to be a version.h file + as found in the following path of an OpenVDB repository: + openvdb/version.h + + If the file does not exist, variables are unmodified. + +``OPENVDB_ABI_VERSION_FROM_PRINT`` + + OPENVDB_ABI_VERSION_FROM_PRINT ( + [QUIET] + ABI [] ) + + Retrieve the ABI version that an installation of OpenVDB was compiled + for using the provided vdb_print binary. Parses the result of: + vdb_print --version + + If the binary does not exist or fails to launch, variables are + unmodified. + +#]=======================================================================] + +cmake_minimum_required(VERSION 3.15) + + +function(OPENVDB_GET_VERSION_DEFINE HEADER KEY VALUE) + if(NOT EXISTS ${HEADER}) + return() + endif() + + file(STRINGS "${HEADER}" details REGEX "^#define[\t ]+${KEY}[\t ]+.*") + if(details) + # define has a value, extract it and return + string(REGEX REPLACE "^.*${KEY}[\t ]+([0-9]*).*$" "\\1" details "${details}") + set(${VALUE} ${details} PARENT_SCOPE) + return() + endif() + + # try re-searching for single defines + file(STRINGS "${HEADER}" details REGEX "^#define[\t ]+${KEY}.*") + if(details) + set(${VALUE} ON PARENT_SCOPE) + else() + set(${VALUE} OFF PARENT_SCOPE) + endif() +endfunction() + + +######################################################################## +######################################################################## + + +function(OPENVDB_VERSION_FROM_HEADER OPENVDB_VERSION_FILE) + cmake_parse_arguments(_VDB "" "VERSION;MAJOR;MINOR;PATCH;ABI" "" ${ARGN}) + + if(NOT EXISTS ${OPENVDB_VERSION_FILE}) + return() + endif() + + OPENVDB_GET_VERSION_DEFINE(${OPENVDB_VERSION_FILE} "OPENVDB_LIBRARY_MAJOR_VERSION_NUMBER" _OpenVDB_MAJOR_VERSION) + OPENVDB_GET_VERSION_DEFINE(${OPENVDB_VERSION_FILE} "OPENVDB_LIBRARY_MINOR_VERSION_NUMBER" _OpenVDB_MINOR_VERSION) + OPENVDB_GET_VERSION_DEFINE(${OPENVDB_VERSION_FILE} "OPENVDB_LIBRARY_PATCH_VERSION_NUMBER" _OpenVDB_PATCH_VERSION) + OPENVDB_GET_VERSION_DEFINE(${OPENVDB_VERSION_FILE} "OPENVDB_ABI_VERSION_NUMBER" _OpenVDB_ABI_VERSION) + + if(_VDB_VERSION) + set(${_VDB_VERSION} + ${_OpenVDB_MAJOR_VERSION}.${_OpenVDB_MINOR_VERSION}.${_OpenVDB_PATCH_VERSION} + PARENT_SCOPE + ) + endif() + if(_VDB_MAJOR) + set(${_VDB_MAJOR} ${_OpenVDB_MAJOR_VERSION} PARENT_SCOPE) + endif() + if(_VDB_MINOR) + set(${_VDB_MINOR} ${_OpenVDB_MINOR_VERSION} PARENT_SCOPE) + endif() + if(_VDB_PATCH) + set(${_VDB_PATCH} ${_OpenVDB_PATCH_VERSION} PARENT_SCOPE) + endif() + if(_VDB_ABI) + set(${_VDB_ABI} ${_OpenVDB_ABI_VERSION} PARENT_SCOPE) + endif() +endfunction() + + +######################################################################## +######################################################################## + + +function(OPENVDB_ABI_VERSION_FROM_PRINT OPENVDB_PRINT) + cmake_parse_arguments(_VDB "QUIET" "ABI" "" ${ARGN}) + + if(NOT EXISTS ${OPENVDB_PRINT}) + return() + endif() + + set(_VDB_PRINT_VERSION_STRING "") + set(_VDB_PRINT_RETURN_STATUS "") + + if(${_VDB_QUIET}) + execute_process(COMMAND ${OPENVDB_PRINT} "--version" + RESULT_VARIABLE _VDB_PRINT_RETURN_STATUS + OUTPUT_VARIABLE _VDB_PRINT_VERSION_STRING + ERROR_QUIET + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + else() + execute_process(COMMAND ${OPENVDB_PRINT} "--version" + RESULT_VARIABLE _VDB_PRINT_RETURN_STATUS + OUTPUT_VARIABLE _VDB_PRINT_VERSION_STRING + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + endif() + + if(${_VDB_PRINT_RETURN_STATUS}) + return() + endif() + + set(_OpenVDB_ABI) + string(REGEX REPLACE ".*abi([0-9]*).*" "\\1" _OpenVDB_ABI ${_VDB_PRINT_VERSION_STRING}) + unset(_VDB_PRINT_RETURN_STATUS) + unset(_VDB_PRINT_VERSION_STRING) + + if(_VDB_ABI) + set(${_VDB_ABI} ${_OpenVDB_ABI} PARENT_SCOPE) + endif() +endfunction() diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/README.md b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/README.md new file mode 100644 index 000000000..402b6d62e --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/README.md @@ -0,0 +1,52 @@ +# Exploration Planner + +The Exploration Planner is an optional global planner for autonomous flight. It combines the maintaining of an openvdb based voxel occupancy grid map, extract frontier from the map, and generates trajectories that enables the drone to explore previously undiscovered areas. With VDB mapping integrated, you can turn off the vdb_mapping node in the launch xml, and the planner will generate and publish multiple linked straight-line trajectories as a shortened RRT path to a selected viewpoint with collision check. + +## Functionality + +You can comment out the world model and random walk planning module in `global.launch.xml` and add the following line: + +``: + +Then when running the robot stack, after taking off, click the `Global Plan` and the exploration planner will work in the place of previous random walk planner. The working process is: + +1. Create and maintain a voxel grid map with odometry and laser scan, to visualize, check topic `"~/vdb_viz"`, where `~` is the namespace. +2. Extract frontier and select viewpoints for exploration. +3. Generate RRT path, shorten and publish as global plan. +4. Continuously monitor the robot's progress along the published path. +5. Once the robot completes the current path, a new exploration path will be generated. + +This loop continues, enabling the drone to keep explore in the entire space. + +We're still cleaning old params of random walk planner, based on which this exploration is developed. We'll update parameter documentation later. + +## Parameters +|
Parameter
| Description +|----------------------------|--------------------------------------------------------------- +| `num_paths_to_generate` | Number of straight-line paths to concatenate into a complete trajectory.| +| `max_start_to_goal_dist_m` | Maximum distance (in meters) from the start point to the goal point for each straight-line segment.| +| `checking_point_cnt` | Number of points along each straight-line segment to check for collisions.| +| `max_z_change_m` | Maximum allowed change in height (z-axis) between the start and goal points.| +| `collision_padding_m` | Extra padding (in meters) added around a voxel's dimensions when checking for collisions.| +| `path_end_threshold_m` | Distance threshold (in meters) for considering the current path completed and generating a new one.| +| `max_yaw_change_degrees` | Maximum allowed change in angle (in radians) between consecutive straight-line segments to ensure a relatively consistent direction.| +| `robot_frame_id` | The frame name for the robot's base frame to look up the transform from the robot position to the world.| + +## Services +|
Parameter
| Type | Description +|----------------------------|----------------------------------------|-----------------------| +| `~/global_plan_toggle` | std_srvs/Trigger | A toggle switch to turn on and off the random walk planner.| + +## Subscriptions +|
Parameter
| Type | Description +|----------------------------|----------------------------------------|-----------------------| +| `~/sub_map_topic` | visualization_msgs/Marker | Stores the map representation that is output from the world or local map topic; currently using vdb local map.| +| `~/tf` | geometry_msgs/TransformStamped | Stores the transform from the robot to the world.| + +## Publications +|
Parameter
| Type | Description +|----------------------------|----------------------------------------|-----------------------| +| `~/pub_global_plan_topic` | nav_msgs/Path | Outputs the global plan that is generated from the random walk planner.| + + + diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/config/exploration_config.yaml b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/config/exploration_config.yaml new file mode 100644 index 000000000..d642eb556 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/config/exploration_config.yaml @@ -0,0 +1,91 @@ +/**: + ros__parameters: + robot_frame_id: "base_link" + lidar_frame_id: "ouster" + map_frame_id: "map" + pub_global_plan_topic: "~/global_plan" + pub_goal_point_viz_topic: "~/goal_point_viz" + pub_trajectory_viz_topic: "~/traj_viz" + pub_grid_viz_topic: "~/vdb_viz" + pub_frontier_viz_topic: "~/frontier_viz" + pub_clustered_frontier_viz_topic: "~/clustered_frontier_viz" + sub_lidar_topic: "sensor_pointcloud" + sub_map_topic: "vdb_map_visualization" + sub_vdb_topic: "vdb_grid" + sub_robot_tf_topic: "/tf" + sub_target_path_topic: "/perspective_goals" + srv_exploration_toggle_topic: "/robot_1/behavior/global_plan_toggle" + + publish_visualizations: false # should trajectory visualizations be published + num_paths_to_generate: 5 # how many paths to string together + + # Planner Parameters + max_start_to_goal_dist_m: 10.0 # how max distance the goal can be from the current robot position + checking_point_cnt: 100 # how many points to collision check between the start and goal points + max_z_change_m: 2.0 # max height that the goal can deviate from the current height + collision_padding_m: 0.5 # how much space should the path have to any obstacles + path_end_threshold_m: 0.2 # how close to the goal will the path be considered complete + max_yaw_change_degrees: 180.0 # how much the goal can deviate from the current orientation + + # Mapping Parameters + map_voxel_resolution: 0.25 # voxel grid size + l_occ: 1.0 + cluster_cube_dim: 5.0 + voxel_cluster_count_thresh: 50 + + # Stall detection parameters + position_change_threshold: 0.2 # Minimum distance (meters) to consider as movement + stall_timeout_seconds: 20.0 # Time without movement before clearing plan + + # view sampling + bbox_length : 1.5 + bbox_breadth : 1.5 + bbox_height : 1.5 + viewing_distance : 1.5 # not used + num_viewpts_per_cluster : 6 + kViewingDistanceInnerR : 1.5 + kViewingDistanceOuterR : 2.0 + kViewingDistanceZOffset : 1.0 + viewp_bbox_unknown_frac_thresh : 1.0 # In a box around a voxel, all unknown as free, all are allowed to be unknown + viewp_bbox_known_occ_thresh : 0.0 # No occupation allowed + coll_check_endpoint_offset : 0.4 + robot_model_bbox_fraction : 0.1 + + # Traj Planning + too_close_distance: 4.0 + too_close_penalty: 5.0 + odometry_match_distance: 3.0 + odometry_match_angle: 3.14159 + odometry_match_penalty: 1000.0 + momentum_collision_check_distance: 15.0 + momentum_collision_check_distance_min: 4.0 + momentum_change_max_reward: 6.0 + momentum_change_collision_reward: 2.0 + viewpoint_neighborhood_too_close_radius: 10.0 + viewpoint_neighborhood_sphere_radius: 6.0 + viewpoint_neighborhood_count: 5 + momentum_minimum_distance: 0.5 + momentum_time: 6.0 + momentum_collision_check_step_size: 0.4 + + # Voxxel Planner RRT params + planner_norm_limit: 1.0 + dense_step: 0.1 + max_explore_dist: 100.0 + planner_max_iter: 20000 + traj_smooth_horizon: 50 + max_connection_iter: 1000 + + # RRT force adding nodes around the start and goal + rrt_region_nodes_count: 16 + rrt_region_nodes_radius: 0.4 + rrt_region_nodes_z_layers_count_up: 1 + rrt_region_nodes_z_layers_step: 0.4 + + priority_level_thred: 10 + + # Collision checker (Safe) + robot_model_safer_cube_dim: 1.8 + + # Collision checker (Risky) + robot_model_cube_dim: 1.4 #0.8 \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/exploration_vis_path.png b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/exploration_vis_path.png new file mode 100644 index 000000000..029e9323f Binary files /dev/null and b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/exploration_vis_path.png differ diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/exploration_vis_vdb.png b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/exploration_vis_vdb.png new file mode 100644 index 000000000..023090bb8 Binary files /dev/null and b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/exploration_vis_vdb.png differ diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/exploration_logic.hpp b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/exploration_logic.hpp new file mode 100644 index 000000000..962357d93 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/exploration_logic.hpp @@ -0,0 +1,222 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "utils/utils.hpp" +#include "utils/rrt_planner.h" +#include "viewpoint_sampling.hpp" +#include "utils/viewpoint.hpp" +#include "utils/collision_checker.h" +#include "rclcpp/rclcpp.hpp" + +typedef std::vector> Path; // x, y, z, yaw +using GridT = openvdb::Grid::Type>; + +struct TimedViewPoint +{ + ViewPoint point; + rclcpp::Time time; +}; + +struct init_params +{ + float max_start_to_goal_dist_m; + int checking_point_cnt; + float max_z_change_m; + float collision_padding_m; + float path_end_threshold_m; + float max_yaw_change_degrees; + + std::tuple voxel_size_m; + + // viewpoint sampling params + double kLOcc_; + double kVoxSize_; + double kViewingDistance_; + double kBboxLength_, kBboxBreadth_, kBboxHeight_; + int kNumViewpointsPerCluster_; + double kViewingDistanceInnerR_, kViewingDistanceOuterR_, kViewingDistanceZOffset_; + double kCollCheckEndpointOffset_; + double kRobotModelBBoxFraction_; + double kViewpBBoxUnknownFracThresh_, kViewpBBoxOccupiedFracThresh_; + + // in init_params + double too_close_distance, too_close_penalty; + double odometry_match_distance, odometry_match_angle, odometry_match_penalty; + double momentum_collision_check_distance, momentum_collision_check_distance_min; + double momentum_change_max_reward, momentum_change_collision_reward; + double viewpoint_neighborhood_too_close_radius, viewpoint_neighborhood_sphere_radius; + int viewpoint_neighborhood_count; + double momentum_minimum_distance; + double momentum_time; + double momentum_collision_check_step_size; + + // RRT + double planner_norm_limit_; + double max_explore_dist_; + int planner_max_iter_; + int max_connection_iter_; + int traj_smooth_horizon_; + int rrt_region_nodes_count_; + double rrt_region_nodes_radius_; + int rrt_region_nodes_z_layers_count_up_; + double rrt_region_nodes_z_layers_step_; + + int priority_level_thred_; + double dense_step_; +}; + +class ExplorationPlanner +{ +public: + ExplorationPlanner() = default; + ExplorationPlanner(init_params params); + ~ExplorationPlanner() = default; + + std::optional generate_straight_rand_path( + std::tuple start_point, + float timeout_duration); // x, y, z, yaw + + std::optional select_viewpoint_and_plan(const ViewPoint& start_point, float timeout_duration); + + std::optional plan_to_given_waypoint(const ViewPoint& start_point, const ViewPoint& goal_point); + + float path_end_threshold_m; + + std::vector> voxel_points; + + bool first_grid_received = false; + bool grid_ready = false; + + GridT::Ptr vdb_grid; + + openvdb::FloatGrid::Ptr occ_grid = openvdb::FloatGrid::create(0.0); + openvdb::FloatGrid::Ptr prev_grid = openvdb::FloatGrid::create(0.0); // to save time for frontier extraction, only check diff for new free voxels + openvdb::BoolGrid::Ptr frontier_grid = openvdb::BoolGrid::create(false); + + std::vector> clustered_points_; + + std::shared_ptr viewp_sample_; + + collision_checker_ns::CollisionChecker collision_checker_; + + std::vector executed_trajectory_; + std::vector momentum_executed_trajectory_; + + RRT_Planner rrt_planner_; + double dense_step_; + +private: + // Numerical constants + float max_start_to_goal_dist_m_; + int checking_point_cnt; + float max_z_change_m_; + float collision_padding_m; + float max_yaw_change_degrees; + + double kLOcc_; + double kVoxSize_; + double kViewingDistance_; + double kBboxLength_, kBboxBreadth_, kBboxHeight_; + int kNumViewpointsPerCluster_; + double kViewingDistanceInnerR_, kViewingDistanceOuterR_, kViewingDistanceZOffset_; + double kCollCheckEndpointOffset_; + double kRobotModelBBoxFraction_; + double kViewpBBoxUnknownFracThresh_, kViewpBBoxOccupiedFracThresh_; + + double too_close_distance_, too_close_penalty_; + double odometry_match_distance_, odometry_match_angle_, odometry_match_penalty_; + double momentum_collision_check_distance_, momentum_collision_check_distance_min_; + double momentum_change_max_reward_, momentum_change_collision_reward_; + double viewpoint_neighborhood_too_close_radius_, viewpoint_neighborhood_sphere_radius_; + int viewpoint_neighborhood_count_; + double momentum_minimum_distance_; + double momentum_time_; + double momentum_collision_check_step_size_; + + double planner_norm_limit_; + double max_explore_dist_; + int planner_max_iter_; + int max_connection_iter_; + int traj_smooth_horizon_; + int rrt_region_nodes_count_; + double rrt_region_nodes_radius_; + int rrt_region_nodes_z_layers_count_up_; + double rrt_region_nodes_z_layers_step_; + + int priority_level_thred_; + + ViewPoint robot_pos_, explore_goal_; + + bool explore_goal_cleared_by_other_robot_; + + // Variables + std::tuple voxel_size_m; + + std::mutex mutex; + + std::vector sampled_viewpoints; + + // Functions + bool check_if_collided_single_voxel(const std::tuple &point, + const std::tuple &voxel_center); + + bool check_if_collided(const std::tuple &point); + + double angleBetweenHeadingAndMomentum(const ViewPoint &point); + + bool computeMomentumVector(double &momentum_direction_x, double &momentum_direction_y, double &momentum_direction_z); + + float getCylinderReward(const int &viewpoint_index, const ViewPoint &planning_point); + + bool extractCylinderViewpointDistances(const int &viewpoint_index, const ViewPoint &planning_point, std::vector &viewpoint_distances); + + bool doesPointMatchTrajectory(const ViewPoint &point); + + void goalSelector(int priority_level); + + static bool compareFrontier(const ViewPoint &p1, const ViewPoint &p2); + + std::tuple generate_goal_point(std::tuple start_point); +}; + +double get_point_distance(const std::tuple &point1, + const std::tuple &point2); + +double deg2rad(double deg); + +double rad2deg(double rad); + +bool lineSegDistances(const ViewPoint &point, + const ViewPoint &line_segment_start, + const ViewPoint &line_segment_end, + double ¶llel_distance, + double &perpendicular_distance); \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/exploration_node.hpp b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/exploration_node.hpp new file mode 100644 index 000000000..7b1f0c16e --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/exploration_node.hpp @@ -0,0 +1,158 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "exploration_logic.hpp" +#include "utils/utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include + +class ExplorationNode : public rclcpp::Node +{ +protected: + // Planner + // ExplorationPlanner exploration_planner; + std::unique_ptr exploration_planner; + + // String constants + std::string world_frame_id_; + std::string robot_frame_id_; + std::string lidar_frame_id_; + std::string map_frame_id_; + std::string pub_global_plan_topic_; + std::string pub_goal_point_viz_topic_; + std::string pub_trajectory_viz_topic_; + std::string pub_grid_viz_topic_; + std::string pub_frontier_viz_topic_; + std::string pub_clustered_frontier_viz_topic_; + std::string sub_lidar_topic_; + std::string sub_map_topic_; + std::string sub_vdb_topic_; + std::string sub_robot_tf_topic_; + std::string srv_exploration_toggle_topic_; + + // Variables + init_params params; + // nav_msgs::msg::Path generated_path; + double momentum_time_; + int num_paths_to_generate_; + std::vector generated_paths; + bool publish_visualizations = false; + bool received_first_map = false; + bool received_first_robot_tf = false; + bool enable_exploration = false; + bool is_path_executing = false; + + geometry_msgs::msg::Transform current_location; // x, y, z, yaw + geometry_msgs::msg::Transform current_goal_location; // x, y, z, yaw + geometry_msgs::msg::Transform last_location; // Last recorded position + rclcpp::Time last_position_change; // Time of last position change + double position_change_threshold = 0.1; // Minimum distance (meters) to consider as movement + double stall_timeout_seconds = 5.0; // Time without movement before clearing plan + + double cluster_cube_dim; + int voxel_cluster_count_thresh; + + double map_voxel_resolution; // map voxel grid size + + // Callbacks + // void lidarCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + + void lidarCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + + void mapCallback(const visualization_msgs::msg::Marker::SharedPtr msg); + + void tfCallback(const tf2_msgs::msg::TFMessage::SharedPtr msg); + + void vdbCallback(const std_msgs::msg::ByteMultiArray::SharedPtr msg); + + void ExplorationToggleCallback(const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + + virtual void timerCallback(); + + virtual void generate_plan(); + + void publish_plan(); + + // Other functions + std::optional readParameters(); + +public: + // explicit ExplorationNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ExplorationNode(); + ~ExplorationNode() = default; + + virtual void initialize(); + // TF buffer + std::shared_ptr tf_buffer; + std::shared_ptr tf_listener; + + std::shared_ptr> tf_filter_; + std::shared_ptr> lidar_filter_sub_; + + // ROS subscribers + rclcpp::Subscription::SharedPtr sub_lidar; + rclcpp::Subscription::SharedPtr sub_map; + rclcpp::Subscription::SharedPtr sub_vdb; + // rclcpp::Subscription::SharedPtr sub_robot_tf; + + // ROS publishers + rclcpp::Publisher::SharedPtr pub_global_plan; + rclcpp::Publisher::SharedPtr pub_goal_point; + rclcpp::Publisher::SharedPtr pub_trajectory_lines; + rclcpp::Publisher::SharedPtr pub_vdb; + rclcpp::Publisher::SharedPtr pub_frontier_vis; + rclcpp::Publisher::SharedPtr pub_clustered_frontier_vis; + rclcpp::Publisher::SharedPtr planning_debug_vis; + + // ROS services + rclcpp::Service::SharedPtr srv_exploration_toggle; + + // ROS timers + rclcpp::TimerBase::SharedPtr timer; +}; diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/utils/collision_checker.h b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/utils/collision_checker.h new file mode 100644 index 000000000..aff0bb6f7 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/utils/collision_checker.h @@ -0,0 +1,149 @@ + +#ifndef COLLISION_CHECKER_H +#define COLLISION_CHECKER_H + +#include "utils/utils.hpp" +#include "utils/viewpoint.hpp" +#include "rclcpp/rclcpp.hpp" + +#include +#include + +// #include +// #include +// #include + +namespace collision_checker_ns +{ + // struct VirtualObstacle + // { + // ViewPoint point; + // float radius; + // rclcpp::Time expiry_time; + // }; + + class CollisionChecker + { + private: + // Constants + float kVoxSize_; + float kBboxLength_, kBboxBreadth_, kBboxHeight_; + float kL_occ_, kUnknown_fraction_thresh_, kOccupied_fraction_thresh_; + // bool kConsider_unknown_vox_occupied_; + // float kVirtual_obstacle_radius_, kVirtual_obstacle_expiry_time_; + // float kRobot_pos_clear_radius_; + // bool kEnable_gate_obstacle_; + // float kGate_cube_size_, kGate_wall_size_; + // std::string shared_frame_tf_topic_; + + // Stats counters + int count_hits_ = 0, count_misses_ = 0; + + openvdb::BoolGrid::Ptr collision_grid_ = NULL; + openvdb::FloatGrid::Ptr grid_map_ = NULL; + + // ros::Publisher debug_map_pub_, debug_virtual_obstacles_pub_, debug_virtual_gate_inner_pub_, debug_virtual_gate_outer_pub_; + // pcl::PointCloud::Ptr debug_map_; + // pcl::PointCloud::Ptr debug_virtual_obstacles_; + // pcl::PointCloud::Ptr debug_gate_obstacles_; + + // openvdb::math::Transform::Ptr darpa_vdb_tf_; + // tf::Transform darpa_tf_; + // bool darpa_tf_set_ = false; + + // openvdb::BoolGrid::Ptr gate_obstacle_inner_ = NULL; + // openvdb::BoolGrid::Ptr gate_obstacle_outer_ = NULL; + // bool inner_gate_obstacle_enabled_; + + // std::vector virtual_obstacles_; + + // ViewPoint robot_pos_; + // bool robot_pos_set_ = false; + + void initGrids(); + openvdb::Vec3d point2vdb(const ViewPoint &p_in); + void clearCache(); + // bool isNearOdometry(const ViewPoint &p); + // bool isInGateObstacle(const ViewPoint &p); + // void checkOdomInInnerGateObstacle(); + + static inline ViewPoint subtract(const ViewPoint &p1, const ViewPoint &p2) + { + ViewPoint p_return; + p_return.x = p1.x - p2.x; + p_return.y = p1.y - p2.y; + p_return.z = p1.z - p2.z; + return p_return; + } + + static inline ViewPoint addPoint(const ViewPoint &p1, const ViewPoint &p2) + { + ViewPoint p_return; + p_return.x = p1.x + p2.x; + p_return.y = p1.y + p2.y; + p_return.z = p1.z + p2.z; + return p_return; + } + + static inline ViewPoint divide(const ViewPoint &p, double factor) + { + ViewPoint p_return; + p_return.x = p.x / factor; + p_return.y = p.y / factor; + p_return.z = p.z / factor; + return p_return; + } + + static inline double norm(const ViewPoint &p) + { + return sqrt(p.x * p.x + p.y * p.y + p.z * p.z); + } + + static inline ViewPoint multiply(const ViewPoint &p, double factor) + { + ViewPoint p_return; + p_return.x = p.x * factor; + p_return.y = p.y * factor; + p_return.z = p.z * factor; + return p_return; + } + + public: + CollisionChecker(); + + void init(const float kVoxSize, + const float length, + const float breadth, + const float height, + const float l_occ, + const float unknown_fraction_thresh, + const float occupied_fraction_thresh); + + // void callbackDarpaTF(const nav_msgs::Odometry::ConstPtr &odom_msg); + // void markStartGate(); + + void updateGridPtr(const openvdb::FloatGrid::Ptr &grid); + void setOdometryForVirtualObstacles(const ViewPoint &robot_pos); + + bool estimateSurfaceNormalAtHit(const openvdb::Vec3d &hit_point_world, double inflation_factor, openvdb::Vec3d &normal_out); + bool collisionCheckRayStrict(const ViewPoint &p_start, const ViewPoint &p_end, openvdb::Vec3d& hit_point); + bool collisionCheckInFreeSpace(const ViewPoint &p); + bool collisionCheckInFreeSpaceVector(const ViewPoint &p_start, const ViewPoint &p_end); + bool collisionCheckInFreeSpaceVectorStepSize(const ViewPoint &p_start, const ViewPoint &p_end, const float step_size); + + // void publishCollisionCheckerViz(); + // void publishGateOuterObstacleViz(); + // void publishGateInnerObstacleViz(); + + // void addVirtualObstacles(const PointSet &obstacle_locations); + // void clearExpiredVirtualObstacles(); + // bool isInVirtualObstacle(const ViewPoint &p); + // void publishVirtualObstacleViz(); + + // bool gate_obstacle_created_ = false; + + }; // class CollisionChecker + +} // namespace collision_checker_ns + +#endif \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/utils/rrt_planner.h b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/utils/rrt_planner.h new file mode 100644 index 000000000..4a6362363 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/utils/rrt_planner.h @@ -0,0 +1,187 @@ + +#ifndef RRT_PLANNER_H +#define RRT_PLANNER_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "utils/utils.hpp" +#include "utils/viewpoint.hpp" +#include "utils/collision_checker.h" + +// Define a Tree +struct Tree +{ + PointSet vertices; + std::map edges; // point, parent + ViewPoint goal; + + void clear() + { + vertices.clear(); + edges.clear(); + } + void insertPoint(const ViewPoint &new_point, const ViewPoint &parent) + { + vertices.push_back(new_point); + edges[new_point] = parent; + } +}; + +class RRT_Planner +{ +public: + RRT_Planner(); + bool RRT_Init( // const float robot_model_cube_dim, + // const openvdb::BoolGrid::Ptr& grid_map, + // const float l_occ, + // const float voxel_dim, + const float norm_limit, + const float max_explore_dist, + const int max_iter, + const int max_connection_iter, + const int smooth_horizon, + // const bool consider_unknown_vox_occupied, + const int rrt_region_nodes_count, + const float rrt_region_nodes_radius, + const int rrt_region_nodes_z_layers_count_up, + const float rrt_region_nodes_z_layers_step, + const float dense_step); + + bool build_RRT(const ViewPoint &start_point, const ViewPoint &end_point, + collision_checker_ns::CollisionChecker &collision_checker, const bool reset_trees = true); + PointSet getPath(); + PointSet getCoarsePath(); + // void updateGridPtr(const openvdb::BoolGrid::Ptr& grid); + bool visualizeTrees(); + // void setRobotModelCubeDim(const float robot_model_cube_dim){kRobot_model_cube_dim_ = robot_model_cube_dim;} + + // Tree Structure + Tree start_tree_; + Tree end_tree_; + + /* ---------------------------------------------------------------------------- */ +private: + // Debugging publishers + // ros::Publisher viz_tree_start_pub_; + // ros::Publisher viz_tree_end_pub_; + + // RRT constants + float dense_step_; + float kNorm_limit_, kNorm_limit_sq_; + float kMax_explore_dist_; + // float kRobot_model_cube_dim_; + // float kVoxel_dim_; + int kMax_iter_; + int kMax_connection_iter_; + int kSmooth_horizon_; + bool kConsider_unknown_vox_occupied_; + int kRRT_region_nodes_count_; + float kRRT_region_nodes_radius_; + int kRRT_region_nodes_z_layers_count_up_; + float kRRT_region_nodes_z_layers_step_; + + // Random number generator + std::uniform_real_distribution distribution_uniform_; + std::default_random_engine generator_; + + // RRT variables + ViewPoint start_point_, end_point_; + ViewPoint last_point_; + bool last_point_found_ = false; + + // Solution + PointSet path_; + PointSet coarse_path_; + + // Mapping variables + // openvdb::BoolGrid::Ptr grid_map_ = NULL; + // float kL_occ_; + + // Main functions + ViewPoint randomPoint(ViewPoint goal); + void interpolate(const ViewPoint &p1, const ViewPoint &p2, PointSet &path); + void pathSmooth(PointSet &path, collision_checker_ns::CollisionChecker &collision_checker); + ViewPoint nearestPoint(const ViewPoint &p_rand, const PointSet &vertices); + void extend(const ViewPoint &p_rand, const ViewPoint &p_near, ViewPoint &p_new); + bool validEdge(const ViewPoint &p_new, const ViewPoint &p_near, collision_checker_ns::CollisionChecker &collision_checker); + bool extractPathFromGraph(collision_checker_ns::CollisionChecker &collision_checker); + bool nearGoal(const ViewPoint &p_new, const ViewPoint &goal); + void addRegionVertices(Tree &tree); + // bool collisionCheckInFreeSpace(const ViewPoint& p); + void swapTreePointers(Tree **a, Tree **b); + void reversePath(PointSet &path); + + // ViewPoint auxillary functions + static inline ViewPoint subtract(const ViewPoint &p1, const ViewPoint &p2) + { + ViewPoint p_return; + p_return.x = p1.x - p2.x; + p_return.y = p1.y - p2.y; + p_return.z = p1.z - p2.z; + return p_return; + } + + static inline ViewPoint addPoint(const ViewPoint &p1, const ViewPoint &p2) + { + ViewPoint p_return; + p_return.x = p1.x + p2.x; + p_return.y = p1.y + p2.y; + p_return.z = p1.z + p2.z; + return p_return; + } + + static inline ViewPoint divide(const ViewPoint &p, double factor) + { + ViewPoint p_return; + p_return.x = p.x / factor; + p_return.y = p.y / factor; + p_return.z = p.z / factor; + return p_return; + } + + static inline double norm(const ViewPoint &p) + { + return sqrt(p.x * p.x + p.y * p.y + p.z * p.z); + } + + static inline double normSq(const ViewPoint &p) + { + // use instead of norm for efficiency and when you don't need the sqrt + return (p.x * p.x + p.y * p.y + p.z * p.z); + } + + static inline ViewPoint multiply(const ViewPoint &p, double factor) + { + ViewPoint p_return; + p_return.x = p.x * factor; + p_return.y = p.y * factor; + p_return.z = p.z * factor; + return p_return; + } + + static inline bool equal(const ViewPoint &p1, const ViewPoint &p2) + { + if (p1.x != p2.x) + { + return false; + } + if (p1.y != p2.y) + { + return false; + } + if (p1.z != p2.z) + { + return false; + } + return true; + } +}; + +#endif \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/utils/utils.hpp b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/utils/utils.hpp new file mode 100644 index 000000000..66ff4406b --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/utils/utils.hpp @@ -0,0 +1,138 @@ +#ifndef _OLE_3D_UTIL_H_ +#define _OLE_3D_UTIL_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +using GridT = openvdb::Grid::Type>; + +// void generateVDBMarker(const GridT::Ptr& grid, +// const std::string& frame_id, +// visualization_msgs::msg::Marker& marker_msg); + +void generateVDBMarker(const openvdb::FloatGrid::Ptr &grid, + const std::string &frame_id, + visualization_msgs::msg::Marker &marker_msg); + +void generateFrontierMarker(const openvdb::BoolGrid::Ptr &grid, + const std::string &frame_id, + visualization_msgs::msg::Marker &marker_msg); + +void generateClusterMarker(const openvdb::FloatGrid::Ptr &grid, + const std::string &frame_id, + visualization_msgs::msg::Marker &marker_msg); + +bool surface_edge_frontier(openvdb::FloatGrid::Accessor accessor, openvdb::Coord ijk); + +void create_clustering_coordbbox(const openvdb::BoolGrid::Ptr grid, + openvdb::math::CoordBBox &coord_bbox_out, + const openvdb::Vec3d ¢er, + const float frontier_count_cube_dim); + +void point_clustering_vis(openvdb::BoolGrid::Ptr grid, + const float cluster_cube_dim, + pcl::PointCloud::Ptr cluster_cloud, + const int voxel_cluster_count_thresh, + std::vector> &clustered_points, + openvdb::FloatGrid::Ptr visualized_clusters); + +void mark_voxels_bool(const std::vector &point_list, + openvdb::BoolGrid::Ptr grid, + const bool voxel_value); + +class VDBUtil +{ +public: + static void updateOccMapFromNdArray(openvdb::FloatGrid::Ptr grid_logocc, + pcl::PointCloud::Ptr xyz, + const tf2::Vector3 &origin, + float l_free = -0.5f, // - a value if not hit + float l_occ = 1.0f, // + a value if hit + float l_min = -10.0f, // lower bound of free + float l_max = 10.0f, // upper bound of occ + int hit_thickness = 1, + double max_range = 50, + float visited_cleared_logocc_min_thresh = 8.0f); + + static void setVoxelSize(openvdb::GridBase &grid, double vs); + + static Eigen::Vector3d computeCentroid(const std::vector &point_list); + + static bool collCheckPointInPartialFreeSpace(const openvdb::FloatGrid::Ptr grid, + const Eigen::Vector3d &point_xyz, + const float length, + const float breadth, + const float height, + const float l_occ, + const float unknown_fraction_thresh, + const float occupied_fraction_thresh); + + static bool checkCollisionAlongRay(const openvdb::FloatGrid::Ptr& grid, + const openvdb::Vec3d &p1_xyz, + const openvdb::Vec3d &p2_xyz, + const float bbox_length, + const float bbox_breadth, + const float bbox_height, + const float l_occ, + openvdb::Vec3d &hit_point, + float dist_offset_from_cluster, + bool consider_unknown_occupied); + + static long int countVoxelsInsideBbox(const openvdb::FloatGrid::Ptr &grid, + const Eigen::Vector3d &bbox_center, + const float length, + const float breadth, + const float height); + + static void createCoordBBox(const openvdb::FloatGrid::Ptr grid, + openvdb::math::CoordBBox &coord_bbox_out, + const openvdb::Vec3d ¢er, + const float bbox_length, + const float bbox_breadth, + const float bbox_height); + + static bool checkIntersectWithCoordBBox(const openvdb::math::CoordBBox &search_bbox, + const openvdb::FloatGrid::Ptr grid, + float occupancy_threshold, + bool consider_unknown_occupied); + + static openvdb::Vec3d convertEigenVecToVec3D(const Eigen::Vector3d &point); + +}; // class VDBUtil +#endif \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/utils/viewpoint.hpp b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/utils/viewpoint.hpp new file mode 100644 index 000000000..15041d4ab --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/utils/viewpoint.hpp @@ -0,0 +1,318 @@ +// Modified from cmusubt drone_traj_planner + +#ifndef CUSTOM_POINT_H +#define CUSTOM_POINT_H + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#define PI 3.14159265359 +#define TWO_PI 6.28318530718 + +// Define a Point +struct ViewPoint +{ + // Position + double x; + double y; + double z; + + bool orientation_set = false; + geometry_msgs::msg::Quaternion orientation; + double orientation_yaw; // equivalent to the yaw within orientation + + // Scoring + bool score_initialized = false; + double score; + bool cleared_by_other_robot = false; + + // Viewpoint score (from map processor) + bool viewpoint_score_initialized = false; + double viewpoint_score; +}; + +// Define a set of Points +typedef std::vector PointSet; + +// Define operators (for map to work) +static inline bool operator>(const ViewPoint &i, const ViewPoint &j) +{ + if (i.x > j.x) + { + return true; + } + else if (i.x == j.x) + { + if (i.y > j.y) + { + return true; + } + else if (i.y == j.y) + { + if (i.z > j.z) + { + return true; + } + else + { + return false; + } + } + else + { + return false; + } + } + else + { + return false; + } +} + +static inline bool operator<(const ViewPoint &i, const ViewPoint &j) +{ + if (i.x < j.x) + { + return true; + } + else if (i.x == j.x) + { + if (i.y < j.y) + { + return true; + } + else if (i.y == j.y) + { + if (i.z < j.z) + { + return true; + } + else + { + return false; + } + } + else + { + return false; + } + } + else + { + return false; + } +} + +static inline void printPoint(const ViewPoint &p) +{ + std::cout << "(" << p.x << ", " << p.y << ", " << p.z << ") {" << p.orientation.x << ", " << p.orientation.y << ", " << p.orientation.z << ", " << p.orientation.w << "}" << std::endl; +} + +static inline double distancePoint2Point(const ViewPoint &p1, const ViewPoint &p2) +{ + // Euclidean distance + double x_diff = p1.x - p2.x; + double y_diff = p1.y - p2.y; + double z_diff = p1.z - p2.z; + + return sqrt(x_diff * x_diff + y_diff * y_diff + z_diff * z_diff); +} + +static inline double angleDiffPoint2Point(const ViewPoint &p1, const ViewPoint &p2) +{ + if (!p1.orientation_set || !p2.orientation_set) + { + return 0.0; + } + else + { + double angle_diff = p2.orientation_yaw - p1.orientation_yaw; + + // wrap around + if (angle_diff > PI) + { + angle_diff -= TWO_PI; + } + else if (angle_diff < -PI) + { + angle_diff += TWO_PI; + } + + return angle_diff; + } +} + +///////////////////// +// PCL +typedef pcl::PointXYZI PCLPointType; +typedef pcl::PointCloud PCLCloudType; + +static inline Eigen::Vector3d point2eigen(const ViewPoint& p) +{ + return Eigen::Vector3d(p.x, p.y, p.z); +} + +static inline openvdb::Vec3d point2vdb(const ViewPoint& p) +{ + return openvdb::Vec3d(p.x, p.y, p.z); +} + +static inline PCLPointType point2PCL(const ViewPoint &p) +{ + PCLPointType point; + point.x = p.x; + point.y = p.y; + point.z = p.z; + // point.intensity = p.score; + point.intensity = 0.0; + return point; +} + +static inline ViewPoint pcl2Point(const PCLPointType &pcl_point) +{ + ViewPoint point; + point.x = pcl_point.x; + point.y = pcl_point.y; + point.z = pcl_point.z; + return point; +} + +static inline PCLCloudType pointSet2PCL(const PointSet &ps) +{ + PCLCloudType pcl_cloud; + for (const auto &p : ps) + { + PCLPointType pcl_point = point2PCL(p); + pcl_cloud.push_back(pcl_point); + } + return pcl_cloud; +} + +///////////////////// +// Geometry msgs +static inline ViewPoint geometryMsgs2Point(const geometry_msgs::msg::Point &geom_point) +{ + ViewPoint point; + point.x = geom_point.x; + point.y = geom_point.y; + point.z = geom_point.z; + return point; +} + +static inline ViewPoint geometryMsgs2Point(const geometry_msgs::msg::Pose& geom_pose) +{ + ViewPoint point; + point.x = geom_pose.position.x; + point.y = geom_pose.position.y; + point.z = geom_pose.position.z; + + // tf2 (ROS 2-friendly) + double roll, pitch, yaw; + tf2::Quaternion quat_tf; + tf2::fromMsg(geom_pose.orientation, quat_tf); + tf2::Matrix3x3(quat_tf).getRPY(roll, pitch, yaw); + + point.orientation = geom_pose.orientation; + point.orientation_set = true; + point.orientation_yaw = yaw; + return point; +} + +static inline geometry_msgs::msg::Point point2GeometryMsgs(const ViewPoint &point) +{ + geometry_msgs::msg::Point geom_point; + geom_point.x = point.x; + geom_point.y = point.y; + geom_point.z = point.z; + return geom_point; +} + +static inline geometry_msgs::msg::Pose point2GeometryMsgsPose(const ViewPoint &point) +{ + geometry_msgs::msg::Pose geom_pose; + geom_pose.position.x = point.x; + geom_pose.position.y = point.y; + geom_pose.position.z = point.z; + if (point.orientation_set) + { + geom_pose.orientation = point.orientation; + } + else + { + geom_pose.orientation.x = 0.0; + geom_pose.orientation.y = 0.0; + geom_pose.orientation.z = 0.0; + geom_pose.orientation.w = 1.0; + } + return geom_pose; +} + +static inline geometry_msgs::msg::PoseArray pointSet2GeometryMsgsPoseArray(const PointSet &point_set) +{ + geometry_msgs::msg::PoseArray msg; + for (const auto &point : point_set) + { + msg.poses.push_back(point2GeometryMsgsPose(point)); + } + return msg; +} + +static inline nav_msgs::msg::Path pointSet2NavMsgsPath(const PointSet &point_set, const std_msgs::msg::Header &header) +{ + nav_msgs::msg::Path msg; + msg.header = header; + for (const auto &point : point_set) + { + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header = header; + pose_stamped.pose = point2GeometryMsgsPose(point); + msg.poses.push_back(pose_stamped); + } + return msg; +} + +/////////////////// +// Viewpoints +static inline ViewPoint pclHSVtoPoint(const pcl::PointXYZHSV &pcl_point) +{ + ViewPoint point; + + // Position + point.x = pcl_point.x; + point.y = pcl_point.y; + point.z = pcl_point.z; + + // Score (from HSV 's' field) + point.viewpoint_score = pcl_point.s; + point.viewpoint_score_initialized = true; + + // Orientation from heading (HSV 'h' field) + tf2::Quaternion q; + q.setRPY(0.0, 0.0, pcl_point.h); // roll=0, pitch=0, yaw=heading + point.orientation = tf2::toMsg(q); + point.orientation_set = true; + + // Store yaw separately + point.orientation_yaw = pcl_point.h; + + return point; +} + +#endif \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/viewpoint_sampling.hpp b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/viewpoint_sampling.hpp new file mode 100644 index 000000000..97a03b3a0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/include/viewpoint_sampling.hpp @@ -0,0 +1,121 @@ +#ifndef _VIEWPOINT_SAMPLING_H_ +#define _VIEWPOINT_SAMPLING_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include "utils/utils.hpp" + +namespace viewpoint_sampling_ns +{ + class ViewpointSampling; +} // namespace viewpoint_sampling_ns + +class viewpoint_sampling_ns::ViewpointSampling +{ + +public: + using ListOfClustersT = std::vector>; + pcl::PointCloud::Ptr viewpoint_list_; + +private: + int cluster_idx_; + + // params + double viewing_distance_; + float bbox_length_, bbox_breadth_, bbox_height_; + float l_occ_, vox_size_; + uint16_t num_viewp_to_sample_; + double viewing_distance_inner_r_, viewing_distance_outer_r_, viewing_distance_z_offset_; + + // offset the endpoint for collision checking along the ray connecting the viewp to cluster centroid + // fraction of the robot model bbox so that the collision checking above is done for a small bbox + double coll_check_endpoint_offset_, bbox_fraction_viewp_centroid_coll_check_; + + // flags + uint8_t solver_type_; + + // viewpoint attributes + double current_cluster_score_, heading_; + double viewp_bbox_unknown_frac_thresh_, viewp_bbox_occ_frac_thresh_; + + // lists + std::shared_ptr cluster_list_; + std::shared_ptr> cluster_score_list_; + std::vector xy_points_; + pcl::PointCloud::Ptr centroid_list_; + std::vector line_y_points_, line_x_points_; + std::vector cluster_x_points_, cluster_y_points_; + + // current geometric entities + Eigen::Vector3d current_cluster_centroid_; + Eigen::Matrix opposing_viewpoints_; + Eigen::Vector3d direction_vector_; + Eigen::Vector2d fitted_line_; + Eigen::MatrixXd A_mat_; + Eigen::VectorXd b_vec_; + Eigen::Vector3d selected_viewpoint_; + openvdb::FloatGrid::Ptr grid_map_; + +public: + ViewpointSampling(ListOfClustersT &cluster_list, + double viewing_distance, + float bbox_length, + float bbox_breadth, + float bbox_height, + float l_occ, + float vox_size, + uint16_t num_viewp_sample_per_cluster, + double viewing_distance_inner_r, + double viewing_distance_outer_r, + double viewing_distance_z_offset, + double coll_check_endpoint_offset, + double bbox_fraction_viewp_centroid_coll_check, + double viewp_bbox_unknown_frac_thresh, + double viewp_bbox_occ_frac_thresh); + + // data handling + void updateGridPtr(const openvdb::FloatGrid::Ptr &grid); + void updateCentroidListPtr(const pcl::PointCloud::Ptr ¢roid_list); + void reset(ListOfClustersT &cluster_list); + + // utility methods + void sampleViewpoints(); + void projectToXYPlane(); + void computePointsPerpToLine(); + void selectOnePoint(); + float computeYawFromDirVector(); + bool fitLineToPoints(int solver_flag); + void useSVDDecomposition(); + void useQRDecomposition(); + void useNormalEquations(); + void createAMatrix(); + void createBMatrix(); + Eigen::Vector3d computeMean(); + pcl::PointXYZHSV constructViewpoint(); + void sampleViewpointsAroundCentroid(); + int sampleViewpointsAroundCentroidParamaterized(const double viewing_distance, + const double z_offset, + const double angle_spacing); + + // logging and plotting + std::string setupFilePath(int file_index, std::string file_name); + void writeToCSVfile(std::string file_name, Eigen::MatrixXd &matrix); + void writeVecToCSVfile(std::string file_name, + std::vector &points); + void logDataToFile(); + void plotPointsAndLine(); + void drawPointsAlongLine(float resolution, float length); + void toXVecYVec(); + +}; // class ViewpointSampling +#endif \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/exploration_launch.xml b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/exploration_launch.xml new file mode 100644 index 000000000..459ba14c0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/exploration_launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/random_walk_launch.xml b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/random_walk_launch.xml new file mode 100644 index 000000000..57c5b1aa3 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/random_walk_launch.xml @@ -0,0 +1,11 @@ + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo.xml b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo.xml new file mode 100644 index 000000000..eb8eb3ea1 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gazebo_vis.rviz b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gazebo_vis.rviz new file mode 100644 index 000000000..d4e39c4b2 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gazebo_vis.rviz @@ -0,0 +1,696 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /Sensors1 + - /Perception1 + - /Perception1/MACVO PointCloud1 + - /Local1 + - /Local1/DROAN1/Trimmed Global Plan for DROAN1/Topic1 + - /Global1 + - /Global1/VDB Mapping Marker1 + Splitter Ratio: 0.590062141418457 + Tree Height: 677 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: true + base_link_stabilized: + Value: false + look_ahead_point: + Value: true + look_ahead_point_stabilized: + Value: false + map: + Value: true + ouster: + Value: false + rmf_owl: + Value: false + rmf_owl/base_link: + Value: false + rmf_owl/base_link/air_pressure: + Value: false + rmf_owl/base_link/base_link_inertia_collision: + Value: false + rmf_owl/base_link/base_link_inertia_visual: + Value: false + rmf_owl/base_link/camera_front: + Value: false + rmf_owl/base_link/imu_sensor: + Value: false + rmf_owl/base_link/magnetometer: + Value: false + rmf_owl/camera_link: + Value: false + rmf_owl/camera_link/camera_collision: + Value: false + rmf_owl/camera_link/camera_visual: + Value: false + rmf_owl/camera_link/depth_camera_front: + Value: false + rmf_owl/camera_link/segmentation_camera: + Value: false + rmf_owl/laser_link: + Value: false + rmf_owl/laser_link/gpu_lidar: + Value: false + rmf_owl/laser_link/laser_collision: + Value: false + rmf_owl/laser_link/laser_visual: + Value: false + rmf_owl/rotor_0: + Value: false + rmf_owl/rotor_0/rotor_0_collision: + Value: false + rmf_owl/rotor_0/rotor_0_visual: + Value: false + rmf_owl/rotor_1: + Value: false + rmf_owl/rotor_1/rotor_1_collision: + Value: false + rmf_owl/rotor_1/rotor_1_visual: + Value: false + rmf_owl/rotor_2: + Value: false + rmf_owl/rotor_2/rotor_2_collision: + Value: false + rmf_owl/rotor_2/rotor_2_visual: + Value: false + rmf_owl/rotor_3: + Value: false + rmf_owl/rotor_3/rotor_3_collision: + Value: false + rmf_owl/rotor_3/rotor_3_visual: + Value: false + tracking_point: + Value: true + tracking_point_stabilized: + Value: false + world: + Value: false + Marker Scale: 2 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + map: + base_link_stabilized: + {} + look_ahead_point: + {} + look_ahead_point_stabilized: + {} + tracking_point: + {} + tracking_point_stabilized: + {} + rmf_owl: + rmf_owl/base_link: + base_link: + {} + rmf_owl/base_link/air_pressure: + {} + rmf_owl/base_link/base_link_inertia_collision: + {} + rmf_owl/base_link/base_link_inertia_visual: + {} + rmf_owl/base_link/camera_front: + {} + rmf_owl/base_link/imu_sensor: + {} + rmf_owl/base_link/magnetometer: + {} + rmf_owl/camera_link: + rmf_owl/camera_link/camera_collision: + {} + rmf_owl/camera_link/camera_visual: + {} + rmf_owl/camera_link/depth_camera_front: + {} + rmf_owl/camera_link/segmentation_camera: + {} + rmf_owl/laser_link: + rmf_owl/laser_link/gpu_lidar: + ouster: + {} + rmf_owl/laser_link/laser_collision: + {} + rmf_owl/laser_link/laser_visual: + {} + rmf_owl/rotor_0: + rmf_owl/rotor_0/rotor_0_collision: + {} + rmf_owl/rotor_0/rotor_0_visual: + {} + rmf_owl/rotor_1: + rmf_owl/rotor_1/rotor_1_collision: + {} + rmf_owl/rotor_1/rotor_1_visual: + {} + rmf_owl/rotor_2: + rmf_owl/rotor_2/rotor_2_collision: + {} + rmf_owl/rotor_2/rotor_2_visual: + {} + rmf_owl/rotor_3: + rmf_owl/rotor_3/rotor_3_collision: + {} + rmf_owl/rotor_3/rotor_3_visual: + {} + Update Interval: 0 + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Front Left RGB + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: sensors/front_stereo/left/image_rect + Value: false + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 100 + Median window: 5 + Min Value: 0 + Name: Front Left Depth + Normalize Range: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: sensors/front_stereo/left/depth + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 6.571824073791504 + Min Value: -0.5682187080383301 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 170; 170; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Lidar + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: sensors/ouster/point_cloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Angle Tolerance: 0 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 1 + Name: Odometry + Position Tolerance: 0 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: odometry_conversion/odometry + Value: false + Enabled: true + Name: Sensors + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: MACVO Disparity + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/macvo/disparity + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud + Color: 255; 255; 255 + Color Transformer: RGBF32 + Decay Time: 5 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: MACVO PointCloud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/macvo/point_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 100 + Name: MACVO Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/macvo/odometry + Value: true + Enabled: true + Name: Perception + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Marker + Enabled: false + Name: Disparity Frustum + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/droan/frustum + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Disparity Map Collision Checking + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/droan/disparity_map_debug + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Disparity Graph Poses + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/droan/disparity_graph + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Trimmed Global Plan for DROAN + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/local_planner_global_plan_vis + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: ExpansionPoly + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/expansion_poly + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 220 + Min Color: 0; 0; 0 + Min Intensity: 120 + Name: Expansion Cloud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/expansion_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Traj Library + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/trajectory_library_vis + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Virtual Obstacles + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/virtual_obstacles + Value: true + Enabled: true + Name: DROAN + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Traj Vis + Namespaces: + traj_controller: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: trajectory_controller/trajectory_vis + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Traj Debug + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: trajectory_controller/trajectory_controller_debug_markers + Value: false + Enabled: true + Name: Trajectory Controller + Enabled: true + Name: Local + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Marker + Enabled: true + Name: VDB Mapping Marker + Namespaces: + vdb_grid: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/exploration_planner/vdb_viz + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 255; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: Global Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/global_plan + Value: true + Enabled: true + Name: Global + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 15.553143501281738 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.2159808725118637 + Y: -0.7331764101982117 + Z: -1.5793094635009766 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5253989100456238 + Target Frame: base_link + Value: Orbit (rviz) + Yaw: 2.190396785736084 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Front Left Depth: + collapsed: false + Front Left RGB: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + MACVO Disparity: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 1272 + Y: 621 diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_autonomy_launch.xml b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_autonomy_launch.xml new file mode 100644 index 000000000..e9a133f38 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_autonomy_launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_behavior_launch.xml b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_behavior_launch.xml new file mode 100644 index 000000000..549a166ca --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_behavior_launch.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_domain_bridge.yaml b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_domain_bridge.yaml new file mode 100644 index 000000000..13ffbb91d --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_domain_bridge.yaml @@ -0,0 +1,77 @@ +name: my_domain_bridge + +topics: + # Bridge "/foo/chatter" topic from doman ID 2 to domain ID 3 + # Automatically detect QoS settings and default to 'keep_last' history with depth 10 + + /tf: + type: tf2_msgs/msg/TFMessage + from_domain: 0 + to_domain: 1 + qos: + subscription: # on domain 0 side: match gz_parameter_bridge (RELIABLE) + reliability: reliable + durability: volatile + history: keep_last + depth: 100 + publisher: # on domain 1 side: match RViz TF listener (RELIABLE) + reliability: reliable + durability: volatile + history: keep_last + depth: 100 + + /tf_static: + type: tf2_msgs/msg/TFMessage + from_domain: 0 + to_domain: 1 + qos: + reliability: reliable + durability: transient_local + history: keep_last + depth: 1 + + /clock: + type: rosgraph_msgs/msg/Clock + from_domain: 0 + to_domain: 1 + qos: + reliability: best_effort + durability: volatile + history: keep_last + depth: 1 + + /odom: + from_domain: 0 + to_domain: 1 + type: nav_msgs/msg/Odometry + + /robot_1/sensors/ouster/point_cloud: + from_domain: 0 + to_domain: 1 + type: sensor_msgs/msg/PointCloud2 + + /robot_1/interface/cmd_velocity: + from_domain: 1 + to_domain: 0 + type: geometry_msgs/msg/TwistStamped + + # /robot_1/odometry_conversion/odometry: + # from_domain: 1 + # to_domain: 0 + # type: nav_msgs/msg/Odometry + + # /robot_1/behavior/behavior_tree_graphviz: + # type: std_msgs/msg/String + # from_domain: 1 + # to_domain: 0 + +# GPS related topics ----------- + # /robot_1/interface/mavros/global_position/raw/fix: + # type: sensor_msgs/msg/NavSatFix + # from_domain: 1 + # to_domain: 0 + + # /robot_1/interface/mavros/global_position/global: + # type: sensor_msgs/msg/NavSatFix + # from_domain: 1 + # to_domain: 0 diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_global_launch.xml b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_global_launch.xml new file mode 100644 index 000000000..8d5b60582 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_global_launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_interface_launch.xml b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_interface_launch.xml new file mode 100644 index 000000000..f42a5d1fd --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_interface_launch.xml @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_local_launch.xml b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_local_launch.xml new file mode 100644 index 000000000..0e0c9dd76 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_local_launch.xml @@ -0,0 +1,90 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_static_transforms.launch.xml b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_static_transforms.launch.xml new file mode 100644 index 000000000..b1946584a --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_static_transforms.launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/package.xml b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/package.xml new file mode 100644 index 000000000..a66c09a37 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/package.xml @@ -0,0 +1,47 @@ + + + exploration_planner + 0.0.0 + The exploration planner package + + + + + todo + + + + + + TODO + + + + + + + + + + + + + + ament_cmake + rclcpp + rclcpp_action + std_msgs + std_srvs + base + geometry_msgs + nav_msgs + visualization_msgs + message_generation + tf2 + tf2_ros + message_filters + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/collision_checker.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/collision_checker.cpp new file mode 100644 index 000000000..440b85690 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/collision_checker.cpp @@ -0,0 +1,255 @@ + +#include "utils/collision_checker.h" + +namespace collision_checker_ns +{ + + void CollisionChecker::init(const float kVoxSize, + const float length, + const float breadth, + const float height, + const float l_occ, + const float unknown_fraction_thresh, + const float occupied_fraction_thresh) + + { + kVoxSize_ = kVoxSize; + kBboxLength_ = length; + kBboxBreadth_ = breadth; + kBboxHeight_ = height; + kL_occ_ = l_occ; + kUnknown_fraction_thresh_ = unknown_fraction_thresh; + kOccupied_fraction_thresh_ = occupied_fraction_thresh; + + initGrids(); + } + + void CollisionChecker::initGrids() + { + // Grid used to cache collisions + collision_grid_ = openvdb::BoolGrid::create(false); + VDBUtil::setVoxelSize(*collision_grid_, kVoxSize_); + } + + inline openvdb::Vec3d CollisionChecker::point2vdb(const ViewPoint &p_in) + { + openvdb::Vec3d p_out(p_in.x, p_in.y, p_in.z); + return p_out; + } + + void CollisionChecker::updateGridPtr(const openvdb::FloatGrid::Ptr &grid) + { + grid_map_ = grid; + // clearExpiredVirtualObstacles(); + clearCache(); + } + + void CollisionChecker::clearCache() + { + collision_grid_->clear(); + // ROS_INFO_STREAM("hits: " << count_hits_ << " misses: " << count_misses_); + } + + bool CollisionChecker::collisionCheckRayStrict(const ViewPoint &p_start, const ViewPoint &p_end, openvdb::Vec3d &hit_point) + { + openvdb::Vec3d vec_start = point2vdb(p_start); + openvdb::Vec3d vec_end = point2vdb(p_end); + + return VDBUtil::checkCollisionAlongRay(grid_map_, vec_start, vec_end, + kBboxLength_, kBboxBreadth_, kBboxHeight_, + kL_occ_, hit_point, 0, false); + } + + bool CollisionChecker::collisionCheckInFreeSpace(const ViewPoint &p) + { + + // Convert to PCL point + Eigen::Vector3d query_pt = point2eigen(p); + + // Map does not exist + if (grid_map_ == NULL) + { + return false; + } + + // Setup cache accessor + openvdb::BoolGrid::Accessor collision_grid_acc = collision_grid_->getAccessor(); + const openvdb::math::Transform &collision_grid_tf(collision_grid_->transform()); + + openvdb::Vec3d cache_xyz = point2vdb(p); + openvdb::Coord cache_ijk = collision_grid_tf.worldToIndexCellCentered(cache_xyz); + + // Check cache + bool cache_hit = false; + bool cache_value; // only valid if cache_hit=true + if (!collision_grid_->empty()) + { + cache_hit = collision_grid_acc.probeValue(cache_ijk, cache_value); + } + + // Do collision checking using cache or explicit check + bool is_in_collision; + + if (cache_hit) + { + // Cache hit -- use stored value + count_hits_++; + is_in_collision = cache_value; + } + else + { + + // Cache miss -- do collision check + count_misses_++; + is_in_collision = !VDBUtil::collCheckPointInPartialFreeSpace(grid_map_, query_pt, + kBboxLength_, kBboxBreadth_, kBboxHeight_, + kL_occ_, kUnknown_fraction_thresh_, kOccupied_fraction_thresh_); + + // Add result to cache for next time + collision_grid_acc.setValueOn(cache_ijk, is_in_collision); + } + + // Ship it! + return is_in_collision; + } + + bool CollisionChecker::collisionCheckInFreeSpaceVector(const ViewPoint &p_start, const ViewPoint &p_end) + { + + // Map does not exist + if (grid_map_ == NULL) + { + return false; + } + + // Setup vector + ViewPoint check_point = p_start; + ViewPoint direction = subtract(p_end, p_start); + double diff = norm(direction); + direction = divide(direction, diff); + ViewPoint step = multiply(direction, double(kVoxSize_)); + + int num_steps = ceil(diff / kVoxSize_); + + // Step along the vector + for (int i = 0; i < num_steps; i++) + { + // Get next point on vector + check_point = addPoint(check_point, step); + + // Collision check + if (collisionCheckInFreeSpace(check_point)) + { + // Collision found + return true; + } + } + return false; + } + + // same as collisionCheckInFreeSpaceVector but with a custom step size + bool CollisionChecker::collisionCheckInFreeSpaceVectorStepSize(const ViewPoint &p_start, const ViewPoint &p_end, const float step_size) + { + + // Map does not exist + if (grid_map_ == NULL) + { + return false; + } + + // Setup vector + ViewPoint check_point = p_start; + ViewPoint direction = subtract(p_end, p_start); + double diff = norm(direction); + direction = divide(direction, diff); + ViewPoint step = multiply(direction, double(step_size)); + + int num_steps = ceil(diff / step_size); + + // Step along the vector + for (int i = 0; i < num_steps; i++) + { + + // Get next point on vector + check_point = addPoint(check_point, step); + + // Collision check + if (collisionCheckInFreeSpace(check_point)) + { + + // Collision found + return true; + } + } + return false; + } + + bool CollisionChecker::estimateSurfaceNormalAtHit(const openvdb::Vec3d &hit_point_world, + double inflation_factor, + openvdb::Vec3d &normal_out) + { + if (!grid_map_ || grid_map_->empty()) + { + return false; + } + + // Clamp inflation to non-negative. + const double infl = std::max(0.0, inflation_factor); + + // Build an inflated world-space AABB around the hit using your helper. + // We inflate the robot bbox uniformly by (1 + infl). + const float len = std::max(kVoxSize_, kBboxLength_ * static_cast(1.0 + infl)); + const float brd = std::max(kVoxSize_, kBboxBreadth_ * static_cast(1.0 + infl)); + const float hgt = std::max(kVoxSize_, kBboxHeight_ * static_cast(1.0 + infl)); + + openvdb::math::CoordBBox search_bbox; + VDBUtil::createCoordBBox(grid_map_, search_bbox, hit_point_world, len, brd, hgt); + + // Accessor & transform + openvdb::FloatGrid::Accessor acc = grid_map_->getAccessor(); + const openvdb::math::Transform &tf = grid_map_->transform(); + + // Accumulate centroid of OCCUPIED voxels (value > kL_occ_) inside the bbox. + openvdb::Vec3d sum_world(0.0, 0.0, 0.0); + std::size_t count = 0; + + for (auto it = search_bbox.beginXYZ(); it; ++it) + { + const openvdb::Coord ijk = *it; + + float v = 0.0f; + const bool active = acc.probeValue(ijk, v); + if (active && v > kL_occ_) + { + // Use voxel center in world space + sum_world += tf.indexToWorld(ijk); + ++count; + } + } + + if (count == 0) + { + // No occupied mass found in the (inflated) neighborhood — cannot infer a normal. + return false; + } + + const openvdb::Vec3d centroid_world = sum_world / static_cast(count); + + // Outward direction: from obstacle mass centroid -> hit point. + openvdb::Vec3d n = hit_point_world - centroid_world; + const double nlen = n.length(); + if (nlen <= 1e-9) + { + // Degenerate (e.g., centroid == hit); no stable direction. + return false; + } + + normal_out = n / nlen; // unit vector, world coordinates + return true; + } + + CollisionChecker::CollisionChecker() + { + } + +} // namespace collision_checker_ns \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/exploration_logic.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/exploration_logic.cpp new file mode 100644 index 000000000..4aa6da6e9 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/exploration_logic.cpp @@ -0,0 +1,762 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "../include/exploration_logic.hpp" + +ExplorationPlanner::ExplorationPlanner(init_params params) +{ + this->max_start_to_goal_dist_m_ = params.max_start_to_goal_dist_m; + this->checking_point_cnt = params.checking_point_cnt; + this->max_z_change_m_ = params.max_z_change_m; + this->voxel_size_m = params.voxel_size_m; + this->collision_padding_m = params.collision_padding_m; + this->path_end_threshold_m = params.path_end_threshold_m; + this->max_yaw_change_degrees = params.max_yaw_change_degrees; + + this->kLOcc_ = params.kLOcc_; + this->kVoxSize_ = params.kVoxSize_; + this->kViewingDistance_ = params.kViewingDistance_; + this->kBboxLength_ = params.kBboxLength_; + this->kBboxBreadth_ = params.kBboxBreadth_; + this->kBboxHeight_ = params.kBboxHeight_; + this->kNumViewpointsPerCluster_ = params.kNumViewpointsPerCluster_; + this->kViewingDistanceInnerR_ = params.kViewingDistanceInnerR_; + this->kViewingDistanceOuterR_ = params.kViewingDistanceOuterR_; + this->kViewingDistanceZOffset_ = params.kViewingDistanceZOffset_; + this->kCollCheckEndpointOffset_ = params.kCollCheckEndpointOffset_; + this->kRobotModelBBoxFraction_ = params.kRobotModelBBoxFraction_; + this->kViewpBBoxUnknownFracThresh_ = params.kViewpBBoxUnknownFracThresh_; + this->kViewpBBoxOccupiedFracThresh_ = params.kViewpBBoxOccupiedFracThresh_; + + this->too_close_distance_ = params.too_close_distance; + this->too_close_penalty_ = params.too_close_penalty; + this->odometry_match_distance_ = params.odometry_match_distance; + this->odometry_match_angle_ = params.odometry_match_angle; + this->odometry_match_penalty_ = params.odometry_match_penalty; + this->momentum_collision_check_distance_ = params.momentum_collision_check_distance; + this->momentum_collision_check_distance_min_ = params.momentum_collision_check_distance_min; + this->momentum_change_max_reward_ = params.momentum_change_max_reward; + this->momentum_change_collision_reward_ = params.momentum_change_collision_reward; + this->viewpoint_neighborhood_too_close_radius_ = params.viewpoint_neighborhood_too_close_radius; + this->viewpoint_neighborhood_sphere_radius_ = params.viewpoint_neighborhood_sphere_radius; + this->viewpoint_neighborhood_count_ = params.viewpoint_neighborhood_count; + this->momentum_minimum_distance_ = params.momentum_minimum_distance; + this->momentum_time_ = params.momentum_time; + this->momentum_collision_check_step_size_ = params.momentum_collision_check_step_size; + + this->planner_norm_limit_ = params.planner_norm_limit_; + this->max_explore_dist_ = params.max_explore_dist_; + this->planner_max_iter_ = params.planner_max_iter_; + this->max_connection_iter_ = params.max_connection_iter_; + this->traj_smooth_horizon_ = params.traj_smooth_horizon_; + this->rrt_region_nodes_count_ = params.rrt_region_nodes_count_; + this->rrt_region_nodes_radius_ = params.rrt_region_nodes_radius_; + this->rrt_region_nodes_z_layers_count_up_ = params.rrt_region_nodes_z_layers_count_up_; + this->rrt_region_nodes_z_layers_step_ = params.rrt_region_nodes_z_layers_step_; + + this->priority_level_thred_ = params.priority_level_thred_; + this->dense_step_ = params.dense_step_; + + viewp_sample_ = std::make_shared(clustered_points_, + kViewingDistance_, + kBboxLength_, + kBboxBreadth_, + kBboxHeight_, + kLOcc_, + kVoxSize_, + uint16_t(kNumViewpointsPerCluster_), + kViewingDistanceInnerR_, + kViewingDistanceOuterR_, + kViewingDistanceZOffset_, + kCollCheckEndpointOffset_, + kRobotModelBBoxFraction_, + kViewpBBoxUnknownFracThresh_, + kViewpBBoxOccupiedFracThresh_); + + collision_checker_.init(kVoxSize_, + kBboxLength_, + kBboxBreadth_, + kBboxHeight_, + kLOcc_, + kViewpBBoxUnknownFracThresh_, + kViewpBBoxOccupiedFracThresh_); + + rrt_planner_.RRT_Init(planner_norm_limit_, + max_explore_dist_, + planner_max_iter_, + max_connection_iter_, + traj_smooth_horizon_, + rrt_region_nodes_count_, + rrt_region_nodes_radius_, + rrt_region_nodes_z_layers_count_up_, + rrt_region_nodes_z_layers_step_, + dense_step_); +} + +std::optional ExplorationPlanner::plan_to_given_waypoint(const ViewPoint& start_point, const ViewPoint& goal_point) +{ + Path output_path; + bool is_traj = false; + + is_traj = rrt_planner_.build_RRT(start_point, goal_point, collision_checker_, true); + + // success + if (is_traj) + { + PointSet traj = rrt_planner_.getPath(); + output_path.reserve(traj.size()); + + // convert to our path format + for (const auto &vp : traj) + { + float yaw_val; + if (vp.orientation_set) + { + // Convert quaternion to yaw + tf2::Quaternion q(vp.orientation.x, vp.orientation.y, vp.orientation.z, vp.orientation.w); + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + yaw_val = static_cast(yaw); + } + else + { + yaw_val = static_cast(vp.orientation_yaw); // fallback if yaw is already set + } + + output_path.emplace_back(static_cast(vp.x), + static_cast(vp.y), + static_cast(vp.z), + yaw_val); + } + + RCLCPP_WARN(rclcpp::get_logger("exploration_planner"), "RRT Path generated"); + + return output_path; + } + + RCLCPP_WARN(rclcpp::get_logger("exploration_planner"), + "No RRT path created"); + + return output_path; +} + +std::optional ExplorationPlanner::select_viewpoint_and_plan(const ViewPoint& start_point, float timeout_duration) +{ + RCLCPP_WARN(rclcpp::get_logger("exploration_planner"), "Start planning"); + robot_pos_ = start_point; + + this->sampled_viewpoints.clear(); + this->sampled_viewpoints.reserve(this->viewp_sample_->viewpoint_list_->size()); + + for (const auto &p : *this->viewp_sample_->viewpoint_list_) + { + this->sampled_viewpoints.push_back(pclHSVtoPoint(p)); + } + + RCLCPP_WARN(rclcpp::get_logger("exploration_planner"), "Computing Viewpoint Scores"); + + for (int viewpoint_index = 0; viewpoint_index < this->sampled_viewpoints.size(); viewpoint_index++) + { + double euclidean_distance = distancePoint2Point(start_point, this->sampled_viewpoints[viewpoint_index]); + double momentum_change_reward = 0; + double momentum_change_angle = angleBetweenHeadingAndMomentum(this->sampled_viewpoints[viewpoint_index]); + momentum_change_reward = (std::cos(momentum_change_angle) + 1.0) / 2.0; // 0 (behind) and 1 (in front) + double momentum_change_reward_before_collision_check = momentum_change_reward; + + if ((euclidean_distance >= momentum_collision_check_distance_min_ && euclidean_distance <= momentum_collision_check_distance_) && + (!collision_checker_.collisionCheckInFreeSpaceVector(start_point, this->sampled_viewpoints[viewpoint_index]))) + { + // Line of sight collision free, therefore add momentum reward + momentum_change_reward *= momentum_change_max_reward_; + } + else + { + // Line of sight blocked, therefore just add a small reward + // since path planning will likely require changing directions anyway + momentum_change_reward *= momentum_change_collision_reward_; + } + // score from viewpoint sampler + double map_processor_score = this->sampled_viewpoints[viewpoint_index].viewpoint_score; + + double score = -euclidean_distance + momentum_change_reward + map_processor_score; + + // Heavily penalize if viewpoint is too close to current location or lookahead point + if (euclidean_distance < too_close_distance_) + { + score -= too_close_penalty_ - 2.0 * euclidean_distance; + } + + // Similarly, reward if there's lots of viewpoints in cylinder between planning point and viewpoint + float cylinder_reward = 0.0; + if (euclidean_distance >= too_close_distance_) + { + cylinder_reward = getCylinderReward(viewpoint_index, start_point) * (0.25 + 0.75 * momentum_change_reward_before_collision_check); + } + score += cylinder_reward; + + // Heavily penalize if robot has already been to this viewpoint + bool already_been_to_viewpoint = doesPointMatchTrajectory(this->sampled_viewpoints[viewpoint_index]); + if (already_been_to_viewpoint) + { + // ROS_ERROR("Already been to this viewpoint!"); + score -= odometry_match_penalty_; + } + + if ((!this->sampled_viewpoints[viewpoint_index].score_initialized) || (score > this->sampled_viewpoints[viewpoint_index].score)) + { + this->sampled_viewpoints[viewpoint_index].score = score; + this->sampled_viewpoints[viewpoint_index].score_initialized = true; + + // Ignore the viewpoint if the map_processor score is not 200 (unobserved by other robots) or 150 (frontier of other robot) + // Also ignore if this robot has already been near it + if (map_processor_score < 100 || already_been_to_viewpoint == true) + // if (already_been_to_viewpoint) + { + this->sampled_viewpoints[viewpoint_index].cleared_by_other_robot = true; + } + } + } + + RCLCPP_WARN(rclcpp::get_logger("exploration_planner"), "RRT Start"); + // RRT to generate path + Path output_path; + bool is_traj = false; + + if (this->sampled_viewpoints.empty()) + { + RCLCPP_INFO(rclcpp::get_logger("exploration_planner"), + "sampled_viewpoints empty"); + } + + for (int priority_level = 0; priority_level < priority_level_thred_ && priority_level < this->sampled_viewpoints.size(); priority_level++) + { + goalSelector(priority_level); + is_traj = rrt_planner_.build_RRT(start_point, explore_goal_, collision_checker_, true); + + // success + if (is_traj) + { + PointSet traj = rrt_planner_.getPath(); + output_path.reserve(traj.size()); + + // convert to our path format + for (const auto &vp : traj) + { + float yaw_val; + if (vp.orientation_set) + { + // Convert quaternion to yaw + tf2::Quaternion q(vp.orientation.x, vp.orientation.y, vp.orientation.z, vp.orientation.w); + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + yaw_val = static_cast(yaw); + } + else + { + yaw_val = static_cast(vp.orientation_yaw); // fallback if yaw is already set + } + + output_path.emplace_back(static_cast(vp.x), + static_cast(vp.y), + static_cast(vp.z), + yaw_val); + } + + RCLCPP_WARN(rclcpp::get_logger("exploration_planner"), "RRT Path generated"); + + return output_path; + } + } + + RCLCPP_WARN(rclcpp::get_logger("exploration_planner"), + "No RRT path created"); + return output_path; +} + +void ExplorationPlanner::goalSelector(int priority_level) +{ + // INPUT : viewpoint_cloud, Graph, robot_pos_; + // OUTPUT: explore_goal_; + + if (priority_level == 0) + { + // assignAwardToFrontiers(); + std::sort(this->sampled_viewpoints.begin(), this->sampled_viewpoints.end(), compareFrontier); + } + + if (priority_level < this->sampled_viewpoints.size()) + { + explore_goal_ = this->sampled_viewpoints[priority_level]; + explore_goal_cleared_by_other_robot_ = this->sampled_viewpoints[priority_level].cleared_by_other_robot; + } + else + { + RCLCPP_WARN(rclcpp::get_logger("exploration_planner"), "priority level > num of sampled viewpoints"); + } +} + +bool ExplorationPlanner::compareFrontier(const ViewPoint &p1, const ViewPoint &p2) +{ + // Using this comparitor will sort from largest to smallest score + if (p1.score > p2.score) + { + return true; + } + return false; +} + +bool ExplorationPlanner::doesPointMatchTrajectory(const ViewPoint &point) +{ + // Does the point match any point in executed_trajectory_ + // within given tolerances + + double distance, angle; + + // Iterate through the executed trajectory, looking for a match + for (const ViewPoint &executed_point : executed_trajectory_) + { + + // Check if it matches + distance = distancePoint2Point(executed_point, point); + if (distance <= odometry_match_distance_) + { + + angle = angleDiffPoint2Point(executed_point, point); + if (angle >= -odometry_match_angle_ && angle <= odometry_match_angle_) + { + // Match found! + return true; + } + } + } + return false; +} + +bool ExplorationPlanner::extractCylinderViewpointDistances( + const int &viewpoint_index, const ViewPoint &planning_point, std::vector &viewpoint_distances) +{ + viewpoint_distances.clear(); + + double distance_pp_to_v = distancePoint2Point(planning_point, this->sampled_viewpoints[viewpoint_index]); + + // Only reward viewpoints that are between too_close_distance_ and viewpoint_neighborhood_too_close_radius_ + // distance from planning point + if ((distance_pp_to_v > viewpoint_neighborhood_too_close_radius_) || (distance_pp_to_v < too_close_distance_)) + { + return false; + } + + // Count how many neighbors + int count = 0; + + // Iterate through all viewpoints + // INCLUDING itself + for (int i = 0; i < this->sampled_viewpoints.size(); ++i) + { + double distance_pp_to_viewpoint = distancePoint2Point(planning_point, this->sampled_viewpoints[i]); + + double distance_odom_to_viewpoint = distancePoint2Point(robot_pos_, this->sampled_viewpoints[i]); + + // If this viewpoint is closer than the subject viewpoint but is not too_close to planning point + // Then this *might* be a neighbor + if ((distance_pp_to_viewpoint <= distance_pp_to_v + 0.01) && (distance_pp_to_viewpoint > too_close_distance_) && (distance_odom_to_viewpoint > too_close_distance_)) + { + + // Extract the cylinder distances + double parallel_distance, perpendicular_distance; + bool on_line_seg = lineSegDistances(this->sampled_viewpoints[i], planning_point, this->sampled_viewpoints[viewpoint_index], + parallel_distance, perpendicular_distance); + + // Is on line segment through center of cylinder + if (on_line_seg) + { + + // Check if in 45deg cone from planning_point + if (perpendicular_distance <= parallel_distance) + { + + // Check if within cylinder + if (perpendicular_distance <= viewpoint_neighborhood_sphere_radius_) + { + // Then viewpoint is considered a "neighbor" + ++count; + viewpoint_distances.push_back(parallel_distance); + } + } + } + } + } + + // Reward this viewpoint if lots of neighboring viewpoints found + return count >= viewpoint_neighborhood_count_; +} + +float ExplorationPlanner::getCylinderReward(const int &viewpoint_index, const ViewPoint &planning_point) +{ + // Find if there's a consecutive set of viewpoints in cylinder between planning_point and viewpoint + double distance_pp_to_v = distancePoint2Point(planning_point, this->sampled_viewpoints[viewpoint_index]); + + // Extract the distances from planning_point for all viewpoints in cylinder + std::vector viewpoint_distances; + bool has_viewpoint_neighborhood = extractCylinderViewpointDistances(viewpoint_index, planning_point, viewpoint_distances); + + if (!has_viewpoint_neighborhood) + { + // no reward + return 0.0; + } + + // Process these distances + + // Sort in DESCENDING order + std::sort(viewpoint_distances.begin(), viewpoint_distances.end(), std::greater()); + + // Search through the distance in descending order + // Looking for a "gap" + float gap_start = 0; + int count_neighbors = 0; + for (int i = 1; i < viewpoint_distances.size(); i++) + { + float previous = viewpoint_distances[i - 1]; + float current = viewpoint_distances[i]; + + float gap = previous - current; // previous is greater than current + + if (gap > 3.0) + { + // Gap found + if (i < viewpoint_neighborhood_count_) + { + // No reward + return 0.0; + } + + // Stop searching for gap + gap_start = previous; + count_neighbors = i; + break; + } + } + + return (viewpoint_distances[0] - gap_start) + 0.1 * (float)count_neighbors; +} + +bool lineSegDistances(const ViewPoint &point, const ViewPoint &line_segment_start, const ViewPoint &line_segment_end, + double ¶llel_distance, double &perpendicular_distance) +{ + // code adapted from http://geomalgorithms.com/a02-_lines.html + // Return false if point is outside of line segment + // Otherwise return true, along with the parallel and perpendicular distances + + // vector end to start + double v_x = line_segment_end.x - line_segment_start.x; + double v_y = line_segment_end.y - line_segment_start.y; + double v_z = line_segment_end.z - line_segment_start.z; + + // vector start to point + double w_x = point.x - line_segment_start.x; + double w_y = point.y - line_segment_start.y; + double w_z = point.z - line_segment_start.z; + + // if outside one boundary, return false + double c1 = v_x * w_x + v_y * w_y + v_z * w_z; // dot product + if (c1 <= 0) + return false; + + // if outside other boundary, return false + double c2 = v_x * v_x + v_y * v_y + v_z * v_z; // dot product + if (c2 <= c1) + return false; + + // otherwise project point and get distance (seems inefficient?) + double b = c1 / c2; + ViewPoint point_projected; + point_projected.x = line_segment_start.x + b * v_x; + point_projected.y = line_segment_start.y + b * v_y; + point_projected.z = line_segment_start.z + b * v_z; + + perpendicular_distance = distancePoint2Point(point, point_projected); + parallel_distance = distancePoint2Point(line_segment_start, point_projected); + return true; +} + +double ExplorationPlanner::angleBetweenHeadingAndMomentum(const ViewPoint &point) +{ + // Get the momentum vector + double momentum_direction_x, momentum_direction_y, momentum_direction_z; + bool momentum_computed = computeMomentumVector(momentum_direction_x, momentum_direction_y, momentum_direction_z); + + if (!momentum_computed) + { + // No momentum -- all viewpoints will be equally weighted in terms of momentum + return 0.0; + } + + // vector to point + double diff_x = point.x - robot_pos_.x; + double diff_y = point.y - robot_pos_.y; + double diff_z = point.z - robot_pos_.z; + double diff_norm = std::sqrt(diff_x * diff_x + diff_y * diff_y + diff_z * diff_z); + + // avoid singularity + if (diff_norm < 0.01) + { + return 0.0; + } + + // normalise + diff_x /= diff_norm; + diff_y /= diff_norm; + diff_z /= diff_norm; + + // dot product + double dot_product = diff_x * momentum_direction_x + diff_y * momentum_direction_y + diff_z * momentum_direction_z; + + // cos(theta) = a.b / |a||b| + double relative_angle = std::acos(dot_product); + return relative_angle; +} + +bool ExplorationPlanner::computeMomentumVector(double &momentum_direction_x, double &momentum_direction_y, double &momentum_direction_z) +{ + // return false if no momentum computed + // momentum returned as a normalised (x,y) vector + // for now, assume z direction is 0, to encourage staying at same z level + + // Need at least two points to estimate momentum + if (this->momentum_executed_trajectory_.size() <= 1) + { + return false; + } + + // Get the start and end point of the vector + const ViewPoint ¤t_point = this->momentum_executed_trajectory_.back().point; + const ViewPoint &previous_point = this->momentum_executed_trajectory_.front().point; + + // Check not stationary first + double euclidean_distance = distancePoint2Point(current_point, previous_point); + if (euclidean_distance <= momentum_minimum_distance_) + { + return false; + } + + // Compute the momentum vector + momentum_direction_x = current_point.x - previous_point.x; + momentum_direction_y = current_point.y - previous_point.y; + momentum_direction_z = 0.0; + + // Normalise + double norm = std::sqrt(momentum_direction_x * momentum_direction_x + momentum_direction_y * momentum_direction_y + momentum_direction_z * momentum_direction_z); + + // avoid singularity (might happen since the above two distance measures are slightly different) + if (norm < 0.01) + { + return false; + } + + // normalise + momentum_direction_x /= norm; + momentum_direction_y /= norm; + momentum_direction_z /= norm; + + return true; +} + +// Junbin: Below are previous implementation to remove + +std::optional ExplorationPlanner::generate_straight_rand_path( + std::tuple start_point, float timeout_duration) +{ + // Locking mutex to prevent crashing when access the voxel map + std::optional path; + bool is_goal_point_valid = false; + // get start ti + const std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now(); + // RCLCPP_INFO(rclcpp::get_logger("exploration_planner"), "Starting Path Search..."); + + while (!is_goal_point_valid && std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_time) + .count() < timeout_duration) + { + std::tuple goal_point = generate_goal_point(start_point); + // RCLCPP_INFO(rclcpp::get_logger("exploration_planner"), "Point Generated..."); + if (std::get<2>(goal_point) == -1) + { + RCLCPP_INFO(rclcpp::get_logger("exploration_planner"), + "No valid goal point found in timeout duration"); + break; + } + + float x_diff = std::get<0>(goal_point) - std::get<0>(start_point); + float y_diff = std::get<1>(goal_point) - std::get<1>(start_point); + float z_diff = std::get<2>(goal_point) - std::get<2>(start_point); + float yaw = std::atan2(y_diff, x_diff); + + path = Path(); + std::tuple first_point( + std::get<0>(start_point), std::get<1>(start_point), std::get<2>(start_point), yaw); + path.value().push_back(first_point); + + std::tuple current_point = std::make_tuple( + std::get<0>(start_point), std::get<1>(start_point), std::get<2>(start_point)); + + // start moving in the direction of the goal point + while (get_point_distance(current_point, goal_point) > 0.001) + { + if (!check_if_collided(current_point)) + { + // add small delay to stop crashing + float new_x = std::get<0>(current_point) + x_diff / float(this->checking_point_cnt); + float new_y = std::get<1>(current_point) + y_diff / float(this->checking_point_cnt); + float new_z = std::get<2>(current_point) + z_diff / float(this->checking_point_cnt); + std::tuple new_point(new_x, new_y, new_z); + current_point = new_point; + std::tuple new_point_with_yaw( + std::get<0>(new_point), std::get<1>(new_point), std::get<2>(new_point), yaw); + path.value().push_back(new_point_with_yaw); + } + else + { + RCLCPP_INFO(rclcpp::get_logger("exploration_planner"), "Exploration: Collision detected"); + is_goal_point_valid = false; + break; + } + is_goal_point_valid = true; + } + } + if (!is_goal_point_valid) + { + RCLCPP_INFO(rclcpp::get_logger("exploration_planner"), "No Path found in time limit"); + return std::nullopt; + } + else + { + return path; + } +} + +bool ExplorationPlanner::check_if_collided_single_voxel( + const std::tuple &point, const std::tuple &voxel_center) +{ + // Check if the point is within the voxel + float x_max = std::get<0>(voxel_center) + std::get<0>(this->voxel_size_m) + this->collision_padding_m; + float x_min = std::get<0>(voxel_center) - std::get<0>(this->voxel_size_m) - this->collision_padding_m; + float y_max = std::get<1>(voxel_center) + std::get<1>(this->voxel_size_m) + this->collision_padding_m; + float y_min = std::get<1>(voxel_center) - std::get<1>(this->voxel_size_m) - this->collision_padding_m; + float z_max = std::get<2>(voxel_center) + std::get<2>(this->voxel_size_m) + this->collision_padding_m; + float z_min = std::get<2>(voxel_center) - std::get<2>(this->voxel_size_m) - this->collision_padding_m; + bool in_x = std::get<0>(point) < x_max && std::get<0>(point) > x_min; + bool in_y = std::get<1>(point) < y_max && std::get<1>(point) > y_min; + bool in_z = std::get<2>(point) < z_max && std::get<2>(point) > z_min; + if (in_x && in_y && in_z) + { + return true; + } + else + { + return false; + } +} + +bool ExplorationPlanner::check_if_collided(const std::tuple &point) +{ + // make sure the point is within the bounds -30 to 30 + if (std::get<0>(point) > 30 || std::get<0>(point) < -30 || std::get<1>(point) > 30 || + std::get<1>(point) < -30 || std::get<2>(point) > 30 || std::get<2>(point) < -30) + { + return true; + } + + std::lock_guard lock(this->mutex); + for (const std::tuple &voxel : this->voxel_points) + { + if (voxel == std::tuple(0, 0, 0)) + { + RCLCPP_INFO(rclcpp::get_logger("exploration_planner"), "Voxel is 0,0,0"); + } + if (check_if_collided_single_voxel(point, voxel)) + { + return true; + } + } + return false; +} + +std::tuple ExplorationPlanner::generate_goal_point( + std::tuple start_point) +{ + float time_out_duration = 1.0; + const clock_t start_time = clock(); + + while ((clock() - start_time) / CLOCKS_PER_SEC < time_out_duration) + { + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution distribution_xy(-this->max_start_to_goal_dist_m_, + this->max_start_to_goal_dist_m_); + std::uniform_real_distribution distribution_z(-this->max_z_change_m_, this->max_z_change_m_); + float delta_x = distribution_xy(gen); + float delta_y = distribution_xy(gen); + float delta_z = distribution_z(gen); + float rand_x = std::get<0>(start_point) + delta_x; + float rand_y = std::get<1>(start_point) + delta_y; + float rand_z = std::get<2>(start_point) + delta_z; + rand_z = std::max(0.5f, rand_z); // ensure don't go below the ground + std::tuple rand_point(rand_x, rand_y, rand_z); + std::tuple start_point_wo_yaw( + std::get<0>(start_point), std::get<1>(start_point), std::get<2>(start_point)); + float dist = get_point_distance(start_point_wo_yaw, rand_point); + float new_angle = std::atan2(std::get<1>(rand_point) - std::get<1>(start_point_wo_yaw), + std::get<0>(rand_point) - std::get<0>(start_point_wo_yaw)); + float angle_diff = std::abs(std::get<3>(start_point) - new_angle); + // if the z value of the point is low enough + if (angle_diff <= this->max_yaw_change_degrees * 3.14159265359 / 180) + { + // if the point is close enough to the start point + if (dist < this->max_start_to_goal_dist_m_) + { + // if the point doesnt collide with any of the voxels + if (!(check_if_collided(rand_point))) + { + return rand_point; + } + } + } + } + return std::tuple(0, 0, -1); +} + +double get_point_distance(const std::tuple &point1, + const std::tuple &point2) +{ + float x_diff = std::get<0>(point1) - std::get<0>(point2); + float y_diff = std::get<1>(point1) - std::get<1>(point2); + float z_diff = std::get<2>(point1) - std::get<2>(point2); + float dist = std::sqrt(x_diff * x_diff + y_diff * y_diff + z_diff * z_diff); + if (std::isnan(dist) || std::isinf(dist)) + { + RCLCPP_ERROR(rclcpp::get_logger("exploration_planner"), "Distance is nan or inf"); + } + return dist; +} + +double deg2rad(double deg) { return deg * M_PI / 180.0; } + +double rad2deg(double rad) { return rad * 180.0 / M_PI; } diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/exploration_node.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/exploration_node.cpp new file mode 100644 index 000000000..cdbc41a72 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/exploration_node.cpp @@ -0,0 +1,916 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "../include/exploration_node.hpp" +#include "../include/exploration_logic.hpp" + +#include +#include +#include + +#include "utils/utils.hpp" + +std::optional ExplorationNode::readParameters() +{ + // Read in parameters based off the default yaml file + init_params params; + this->declare_parameter("robot_frame_id"); + if (!this->get_parameter("robot_frame_id", this->robot_frame_id_)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: robot_frame_id"); + return std::optional{}; + } + this->declare_parameter("lidar_frame_id"); + if (!this->get_parameter("lidar_frame_id", this->lidar_frame_id_)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: lidar_frame_id"); + return std::optional{}; + } + this->declare_parameter("map_frame_id"); + if (!this->get_parameter("map_frame_id", this->map_frame_id_)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: map_frame_id"); + return std::optional{}; + } + this->declare_parameter("pub_global_plan_topic"); + if (!this->get_parameter("pub_global_plan_topic", this->pub_global_plan_topic_)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_global_plan_topic"); + return std::optional{}; + } + this->declare_parameter("pub_goal_point_viz_topic"); + if (!this->get_parameter("pub_goal_point_viz_topic", this->pub_goal_point_viz_topic_)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_goal_point_viz_topic"); + return std::optional{}; + } + this->declare_parameter("pub_trajectory_viz_topic"); + if (!this->get_parameter("pub_trajectory_viz_topic", this->pub_trajectory_viz_topic_)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_trajectory_viz_topic"); + return std::optional{}; + } + this->declare_parameter("pub_grid_viz_topic"); + if (!this->get_parameter("pub_grid_viz_topic", this->pub_grid_viz_topic_)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_grid_viz_topic"); + return std::optional{}; + } + this->declare_parameter("pub_frontier_viz_topic"); + if (!this->get_parameter("pub_frontier_viz_topic", this->pub_frontier_viz_topic_)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_frontier_viz_topic"); + return std::optional{}; + } + this->declare_parameter("pub_clustered_frontier_viz_topic"); + if (!this->get_parameter("pub_clustered_frontier_viz_topic", this->pub_clustered_frontier_viz_topic_)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_clustered_frontier_viz_topic"); + return std::optional{}; + } + this->declare_parameter("sub_lidar_topic"); + if (!this->get_parameter("sub_lidar_topic", this->sub_lidar_topic_)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: sub_map_topic"); + return std::optional{}; + } + this->declare_parameter("sub_map_topic"); + if (!this->get_parameter("sub_map_topic", this->sub_map_topic_)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: sub_map_topic"); + return std::optional{}; + } + this->declare_parameter("sub_vdb_topic"); + if (!this->get_parameter("sub_vdb_topic", this->sub_vdb_topic_)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: sub_vdb_topic"); + return std::optional{}; + } + this->declare_parameter("sub_robot_tf_topic"); + if (!this->get_parameter("sub_robot_tf_topic", this->sub_robot_tf_topic_)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: sub_robot_tf_topic"); + return std::optional{}; + } + this->declare_parameter("srv_exploration_toggle_topic"); + if (!this->get_parameter("srv_exploration_toggle_topic", this->srv_exploration_toggle_topic_)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: srv_exploration_toggle_topic"); + return std::optional{}; + } + this->declare_parameter("publish_visualizations"); + if (!this->get_parameter("publish_visualizations", this->publish_visualizations)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: publish_visualizations"); + return std::optional{}; + } + this->declare_parameter("num_paths_to_generate"); + if (!this->get_parameter("num_paths_to_generate", this->num_paths_to_generate_)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: num_paths_to_generate"); + return std::optional{}; + } + this->declare_parameter("max_start_to_goal_dist_m"); + if (!this->get_parameter("max_start_to_goal_dist_m", params.max_start_to_goal_dist_m)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: max_start_to_goal_dist_m"); + return std::optional{}; + } + this->declare_parameter("checking_point_cnt"); + if (!this->get_parameter("checking_point_cnt", params.checking_point_cnt)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: checking_point_cnt"); + return std::optional{}; + } + this->declare_parameter("max_z_change_m"); + if (!this->get_parameter("max_z_change_m", params.max_z_change_m)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: max_z_change_m"); + return std::optional{}; + } + this->declare_parameter("collision_padding_m"); + if (!this->get_parameter("collision_padding_m", params.collision_padding_m)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: collision_padding_m"); + return std::optional{}; + } + this->declare_parameter("path_end_threshold_m"); + if (!this->get_parameter("path_end_threshold_m", params.path_end_threshold_m)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: path_end_threshold_m"); + return std::optional{}; + } + this->declare_parameter("max_yaw_change_degrees"); + if (!this->get_parameter("max_yaw_change_degrees", params.max_yaw_change_degrees)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: max_yaw_change_degrees"); + return std::optional{}; + } + this->declare_parameter("cluster_cube_dim"); + if (!this->get_parameter("cluster_cube_dim", this->cluster_cube_dim)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: cluster_cube_dim"); + return std::optional{}; + } + this->declare_parameter("voxel_cluster_count_thresh"); + if (!this->get_parameter("voxel_cluster_count_thresh", this->voxel_cluster_count_thresh)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: voxel_cluster_count_thresh"); + return std::optional{}; + } + this->declare_parameter("position_change_threshold"); + if (!this->get_parameter("position_change_threshold", this->position_change_threshold)) + { + + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: position_change_threshold"); + return std::optional{}; + } + this->declare_parameter("map_voxel_resolution"); + if (!this->get_parameter("map_voxel_resolution", this->map_voxel_resolution)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: map_voxel_resolution"); + return std::optional{}; + } + this->declare_parameter("stall_timeout_seconds"); + if (!this->get_parameter("stall_timeout_seconds", this->stall_timeout_seconds)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: stall_timeout_seconds"); + return std::optional{}; + } + + // viewpoint sampling + params.kLOcc_ = this->declare_parameter("l_occ", 1.0); + this->get_parameter("map_voxel_resolution", params.kVoxSize_); + params.kBboxLength_ = this->declare_parameter("bbox_length", 3.0); + params.kBboxBreadth_ = this->declare_parameter("bbox_breadth", 3.0); + params.kBboxHeight_ = this->declare_parameter("bbox_height", 3.0); + params.kViewingDistance_ = this->declare_parameter("viewing_distance", 3.0); + params.kNumViewpointsPerCluster_ = this->declare_parameter("num_viewpts_per_cluster", 6); + params.kViewingDistanceInnerR_ = this->declare_parameter("kViewingDistanceInnerR", 3.0); + params.kViewingDistanceOuterR_ = this->declare_parameter("kViewingDistanceOuterR", 6.0); + params.kViewingDistanceZOffset_ = this->declare_parameter("kViewingDistanceZOffset", 2.0); + params.kViewpBBoxUnknownFracThresh_ = this->declare_parameter("viewp_bbox_unknown_frac_thresh", 0.85); + params.kViewpBBoxOccupiedFracThresh_ = this->declare_parameter("viewp_bbox_known_occ_thresh", 0.0); + params.kCollCheckEndpointOffset_ = this->declare_parameter("coll_check_endpoint_offset", 0.4); + params.kRobotModelBBoxFraction_ = this->declare_parameter("robot_model_bbox_fraction", 0.1); + + // viewpoint selection + params.too_close_distance = this->declare_parameter("too_close_distance", 4.0); + params.too_close_penalty = this->declare_parameter("too_close_penalty", 5.0); + params.odometry_match_distance = this->declare_parameter("odometry_match_distance", 3.0); + params.odometry_match_angle = this->declare_parameter("odometry_match_angle", 3.14159); + params.odometry_match_penalty = this->declare_parameter("odometry_match_penalty", 1000.0); + params.momentum_collision_check_distance = this->declare_parameter("momentum_collision_check_distance", 15.0); + params.momentum_collision_check_distance_min = this->declare_parameter("momentum_collision_check_distance_min", 4.0); + params.momentum_change_max_reward = this->declare_parameter("momentum_change_max_reward", 6.0); + params.momentum_change_collision_reward = this->declare_parameter("momentum_change_collision_reward", 2.0); + params.viewpoint_neighborhood_too_close_radius = this->declare_parameter("viewpoint_neighborhood_too_close_radius", 10.0); + params.viewpoint_neighborhood_sphere_radius = this->declare_parameter("viewpoint_neighborhood_sphere_radius", 6.0); + params.viewpoint_neighborhood_count = this->declare_parameter("viewpoint_neighborhood_count", 5); + params.momentum_minimum_distance = this->declare_parameter("momentum_minimum_distance", 0.5); + params.momentum_time = this->declare_parameter("momentum_time", 2.0); + params.momentum_collision_check_step_size = this->declare_parameter("momentum_collision_check_step_size", 0.4); + + this->momentum_time_ = params.momentum_time; + // RRT + params.planner_norm_limit_ = this->declare_parameter("planner_norm_limit", 0.1); + params.max_explore_dist_ = this->declare_parameter("max_explore_dist", 100.0); + params.planner_max_iter_ = this->declare_parameter("planner_max_iter", 20000); + params.max_connection_iter_ = this->declare_parameter("max_connection_iter", 1000); + params.traj_smooth_horizon_ = this->declare_parameter("traj_smooth_horizon", 50); + params.rrt_region_nodes_count_ = this->declare_parameter("rrt_region_nodes_count", 16); + params.rrt_region_nodes_radius_ = this->declare_parameter("rrt_region_nodes_radius", 0.8); + params.rrt_region_nodes_z_layers_count_up_ = this->declare_parameter("rrt_region_nodes_z_layers_count_up", 1); + params.rrt_region_nodes_z_layers_step_ = this->declare_parameter("rrt_region_nodes_z_layers_step", 0.4); + + params.priority_level_thred_ = this->declare_parameter("priority_level_thred", 20); + params.dense_step_ = this->declare_parameter("dense_step", 0.1); + return params; +} + +ExplorationNode::ExplorationNode() : Node("exploration_node") +{ + // Initialize float 543 grid + openvdb::initialize(); + + // Initialize the exploration planner + std::optional params_opt = ExplorationNode::readParameters(); + + this->world_frame_id_ = this->map_frame_id_; + + if (params_opt.has_value()) + { + this->params = params_opt.value(); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Failed to initialize exploration planner"); + } + + this->params.voxel_size_m = std::tuple(map_voxel_resolution, map_voxel_resolution, map_voxel_resolution); + + // Initialize exploration planner + this->exploration_planner = std::make_unique(this->params); + RCLCPP_INFO(this->get_logger(), "Initialized exploration planner logic"); + + VDBUtil::setVoxelSize(*(this->exploration_planner->occ_grid), map_voxel_resolution); + VDBUtil::setVoxelSize(*(this->exploration_planner->prev_grid), map_voxel_resolution); + VDBUtil::setVoxelSize(*(this->exploration_planner->frontier_grid), map_voxel_resolution); + + RCLCPP_INFO(this->get_logger(), "Initialized exploration inner voxel grids"); +} + +void ExplorationNode::initialize() +{ + + // TF buffer and listener + tf_buffer = std::make_shared(this->get_clock()); + tf_listener = std::make_shared(*tf_buffer); + + this->sub_lidar = this->create_subscription( + sub_lidar_topic_, rclcpp::QoS(1).durability_volatile().best_effort(), std::bind(&ExplorationNode::lidarCallback, this, std::placeholders::_1)); + + // Trying message filter to avoid extrapolation when lookuptransform, not succedded yet + + // auto node_ptr = rclcpp::Node::shared_from_this(); + // RCLCPP_WARN(this->get_logger(), "Type: %s", typeid(*node_ptr).name()); + + // tf_buffer = std::make_shared(this->get_clock(), tf2::durationFromSec(10.0), node_ptr); + // tf_listener = std::make_shared(*tf_buffer); + + // lidar_filter_sub_ = std::make_shared>( + // this, sub_lidar_topic_, rmw_qos_profile_sensor_data); + + // tf_filter_ = std::make_shared>( + // *lidar_filter_sub_, *tf_buffer, map_frame_id_, 10, this->get_node_logging_interface(), this->get_node_clock_interface()); + + // tf_filter_->registerCallback(&ExplorationNode::lidarCallback, this); + + // Previous setup + + // this->sub_map = this->create_subscription( + // sub_map_topic_, 10, std::bind(&ExplorationNode::mapCallback, this, std::placeholders::_1)); + + // this->sub_vdb = this->create_subscription( + // sub_vdb_topic_, 10, std::bind(&ExplorationNode::vdbCallback, this, std::placeholders::_1)); + + this->pub_global_plan = this->create_publisher(pub_global_plan_topic_, 10); + this->pub_goal_point = + this->create_publisher(pub_goal_point_viz_topic_, 10); + this->pub_trajectory_lines = + this->create_publisher(pub_trajectory_viz_topic_, 10); + this->pub_vdb = + this->create_publisher(pub_grid_viz_topic_, 10); + this->pub_frontier_vis = + this->create_publisher(pub_frontier_viz_topic_, 10); + this->pub_clustered_frontier_vis = + this->create_publisher(pub_clustered_frontier_viz_topic_, 10); + this->planning_debug_vis = + this->create_publisher("/exploration_debug_vis", 10); + + // Set up the timer + this->timer = this->create_wall_timer(std::chrono::seconds(5), + std::bind(&ExplorationNode::timerCallback, this)); + // Set up the service + this->srv_exploration_toggle = this->create_service( + srv_exploration_toggle_topic_, std::bind(&ExplorationNode::ExplorationToggleCallback, this, + std::placeholders::_1, std::placeholders::_2)); + RCLCPP_INFO(this->get_logger(), "Exploration node initialized"); +} + +void ExplorationNode::ExplorationToggleCallback( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + if (this->enable_exploration == false) + { + this->enable_exploration = true; + response->success = true; + response->message = "Exploration enabled"; + RCLCPP_INFO(this->get_logger(), "Exploration enabled"); + } + else + { + this->enable_exploration = false; + response->success = true; + response->message = "Exploration disabled"; + RCLCPP_INFO(this->get_logger(), "Exploration disabled"); + } +} + +void ExplorationNode::lidarCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) +{ + geometry_msgs::msg::TransformStamped cloud_origin_tf; + + pcl::PointCloud::Ptr incoming_cloud(new pcl::PointCloud); + pcl::fromROSMsg(*msg, *incoming_cloud); + + try + { + cloud_origin_tf = this->tf_buffer->lookupTransform( + map_frame_id_, lidar_frame_id_, msg->header.stamp, rclcpp::Duration(0, 100000000)); + // map_frame_id_, lidar_frame_id_, msg->header.stamp); + } + catch (tf2::TransformException &ex) + { + RCLCPP_ERROR(this->get_logger(), + "Could not transform %s to %s: %s", + map_frame_id_.c_str(), + lidar_frame_id_.c_str(), + ex.what()); + return; + } + + // pre-processing: remove nan + std::vector idx; + pcl::removeNaNFromPointCloud(*incoming_cloud, *incoming_cloud, idx); + + double min_range = 0.5; + + // pre-processing: remove too close points + pcl::PointCloud::Ptr tmp(new pcl::PointCloud); + tmp->reserve(incoming_cloud->size()); + const float r2min = min_range * min_range; + for (const auto &p : incoming_cloud->points) + { + const float r2 = p.x * p.x + p.y * p.y + p.z * p.z; + if (r2 >= r2min) + tmp->points.push_back(p); + } + tmp->width = static_cast(tmp->points.size()); + tmp->height = 1; + tmp->is_dense = true; + incoming_cloud.swap(tmp); + + pcl::transformPointCloud(*incoming_cloud, *incoming_cloud, tf2::transformToEigen(cloud_origin_tf).matrix()); + tf2::Vector3 origin_vec(cloud_origin_tf.transform.translation.x, + cloud_origin_tf.transform.translation.y, + cloud_origin_tf.transform.translation.z); + + VDBUtil::updateOccMapFromNdArray(this->exploration_planner->occ_grid, incoming_cloud, origin_vec); + this->exploration_planner->collision_checker_.updateGridPtr(this->exploration_planner->occ_grid); + + // visualization_msgs::msg::Marker vdb_marker_msg; + // generateVDBMarker(this->exploration_planner->occ_grid, map_frame_id_, vdb_marker_msg); + // vdb_marker_msg.header.stamp = this->now(); + // this->pub_vdb->publish(vdb_marker_msg); + + // RCLCPP_WARN(this->get_logger(), "Lidar Msg Received"); +} + +void ExplorationNode::mapCallback(const visualization_msgs::msg::Marker::SharedPtr msg) +{ + // updating the local voxel points and generating a path only if the path is not executing + if (!this->received_first_map) + { + this->received_first_map = true; + this->world_frame_id_ = msg->header.frame_id; + RCLCPP_INFO(this->get_logger(), "Received first map"); + this->params.voxel_size_m = + std::tuple(msg->scale.x, msg->scale.y, msg->scale.z); + // this->exploration_planner = ExplorationPlanner(this->params); + this->exploration_planner = std::make_unique(this->params); + + RCLCPP_INFO(this->get_logger(), "Initialized exploration planner logic"); + } + this->exploration_planner->voxel_points.clear(); + for (int i = 0; i < msg->points.size(); i++) + { + this->exploration_planner->voxel_points.push_back( + std::tuple(msg->points[i].x, msg->points[i].y, msg->points[i].z)); + } +} + +void ExplorationNode::vdbCallback(const std_msgs::msg::ByteMultiArray::SharedPtr msg) +{ + std::string raw_data(reinterpret_cast(msg->data.data()), msg->data.size()); + std::istringstream iss(raw_data, std::ios::binary); + + openvdb::io::Stream in_stream(iss); + std::vector grids = *in_stream.getGrids(); + + if (grids.empty()) + { + RCLCPP_WARN(this->get_logger(), "Received VDB grid message, but no grids were found."); + return; + } + + using GridT = openvdb::Grid::Type>; + GridT::Ptr float_grid = openvdb::gridPtrCast(grids.front()); + + if (!float_grid) + { + RCLCPP_WARN(this->get_logger(), "First grid in received VDB message is not a FloatGrid."); + return; + } + + if (this->exploration_planner) + { + this->exploration_planner->vdb_grid = float_grid; + + if (!this->exploration_planner->first_grid_received) + { + this->exploration_planner->first_grid_received = true; + this->exploration_planner->frontier_grid->setTransform(float_grid->transform().copy()); + this->exploration_planner->prev_grid->setTransform(float_grid->transform().copy()); + RCLCPP_WARN(this->get_logger(), "Exploration Grids Initialized"); + } + // RCLCPP_WARN(this->get_logger(), "Successfully received and stored VDB FloatGrid (%zu active voxels).", + // this->exploration_planner->vdb_grid->activeVoxelCount()); + // RCLCPP_WARN(this->get_logger(), "received type is %s", grids.front()->type().c_str()); + // RCLCPP_WARN(this->get_logger(), "our type is %s", this->exploration_planner->vdb_grid->type().c_str()); + + // visualization_msgs::msg::Marker vdb_marker_msg; + // generateVDBMarker(this->exploration_planner->vdb_grid, "map", vdb_marker_msg); + // vdb_marker_msg.header.stamp = this->now(); + // this->pub_vdb->publish(vdb_marker_msg); + } + else + { + RCLCPP_WARN(this->get_logger(), "Exploration planner is not yet initialized."); + } +} + +void ExplorationNode::tfCallback(const tf2_msgs::msg::TFMessage::SharedPtr msg) +{ + // get the current location + for (int i = 0; i < msg->transforms.size(); i++) + { + if (msg->transforms[i].child_frame_id.c_str() == this->robot_frame_id_) + { + this->current_location = msg->transforms[i].transform; + if (!this->received_first_robot_tf) + { + this->received_first_robot_tf = true; + RCLCPP_WARN(this->get_logger(), "Received first robot_tf"); + } + } + } +} + +void ExplorationNode::generate_plan() +{ + RCLCPP_INFO(this->get_logger(), "Starting to generate plan..."); + + // std::tuple start_loc; + // if (this->generated_paths.size() == 0) + // { + // geometry_msgs::msg::Quaternion orientation = this->current_location.rotation; + // tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); + // q.normalize(); + // double roll, pitch, yaw; + // tf2::Matrix3x3 m(q); + // m.getRPY(roll, pitch, yaw); + // start_loc = std::make_tuple(this->current_location.translation.x, + // this->current_location.translation.y, + // this->current_location.translation.z, yaw); + // } + // else + // { + // geometry_msgs::msg::Quaternion orientation = + // this->generated_paths.back().poses.back().pose.orientation; + // tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); + // q.normalize(); + // double roll, pitch, yaw; + // tf2::Matrix3x3 m(q); + // m.getRPY(roll, pitch, yaw); + // start_loc = std::make_tuple(this->generated_paths.back().poses.back().pose.position.x, + // this->generated_paths.back().poses.back().pose.position.y, + // this->generated_paths.back().poses.back().pose.position.z, yaw); + // } + + ViewPoint start_point; + start_point.x = this->current_location.translation.x; + start_point.y = this->current_location.translation.y; + start_point.z = this->current_location.translation.z; + + tf2::Quaternion q; + tf2::fromMsg(this->current_location.rotation, q); + q.normalize(); + + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + + start_point.orientation = this->current_location.rotation; + start_point.orientation_set = true; + start_point.orientation_yaw = yaw; + + // Convert start_loc to Point as starting point? + + // Replace the generate_straight_rand_path with the viewpoint score and RRT? + + float timeout_duration = 5.0; + std::optional gen_path_opt = + // this->exploration_planner->generate_straight_rand_path(start_loc, timeout_duration); + this->exploration_planner->select_viewpoint_and_plan(start_point, timeout_duration); + if (gen_path_opt.has_value() && gen_path_opt.value().size() > 0) + { + RCLCPP_INFO(this->get_logger(), "Generated path with %ld points", + gen_path_opt.value().size()); + + // set the current goal location + this->current_goal_location = geometry_msgs::msg::Transform(); + this->current_goal_location.translation.x = std::get<0>(gen_path_opt.value().back()); + this->current_goal_location.translation.y = std::get<1>(gen_path_opt.value().back()); + this->current_goal_location.translation.z = std::get<2>(gen_path_opt.value().back()); + float z_rot = std::get<3>(gen_path_opt.value().back()); + tf2::Quaternion q; + q.setRPY(0, 0, z_rot); // Roll = 0, Pitch = 0, Yaw = yaw + q.normalize(); + this->current_goal_location.rotation.x = q.x(); + this->current_goal_location.rotation.y = q.y(); + this->current_goal_location.rotation.z = q.z(); + this->current_goal_location.rotation.w = q.w(); + + // publish the path + nav_msgs::msg::Path generated_single_path; + generated_single_path = nav_msgs::msg::Path(); + generated_single_path.header.stamp = this->now(); + generated_single_path.header.frame_id = world_frame_id_; + for (auto point : gen_path_opt.value()) + { + geometry_msgs::msg::PoseStamped point_msg; + point_msg.pose.position.x = std::get<0>(point); + point_msg.pose.position.y = std::get<1>(point); + point_msg.pose.position.z = std::get<2>(point); + // convert yaw rotation to quaternion + tf2::Quaternion q; + q.setRPY(0, 0, std::get<3>(point)); // Roll = 0, Pitch = 0, Yaw = yaw + q.normalize(); + point_msg.pose.orientation.x = q.x(); + point_msg.pose.orientation.y = q.y(); + point_msg.pose.orientation.z = q.z(); + point_msg.pose.orientation.w = q.w(); + point_msg.header.stamp = this->now(); + generated_single_path.poses.push_back(point_msg); + } + geometry_msgs::msg::PoseStamped last_goal_loc = generated_single_path.poses.back(); + this->current_goal_location.translation.x = last_goal_loc.pose.position.x; + this->current_goal_location.translation.y = last_goal_loc.pose.position.y; + this->current_goal_location.translation.z = last_goal_loc.pose.position.z; + this->current_goal_location.rotation.z = last_goal_loc.pose.orientation.z; + this->generated_paths.push_back(generated_single_path); + + // debug vis of RRT + PointSet coarse_path = this->exploration_planner->rrt_planner_.getCoarsePath(); + visualization_msgs::msg::MarkerArray rrt_vis_array; + + if (coarse_path.empty()) + { + planning_debug_vis->publish(rrt_vis_array); + } + else + { + const std::string frame_id = "map"; + const rclcpp::Time stamp = this->now(); + + // ----- LINE_STRIP for the polyline ----- + visualization_msgs::msg::Marker line; + line.header.frame_id = frame_id; + line.header.stamp = stamp; + line.ns = "rrt_coarse_path"; + line.id = 0; + line.type = visualization_msgs::msg::Marker::LINE_STRIP; + line.action = visualization_msgs::msg::Marker::ADD; + line.pose.orientation.w = 1.0; // identity + line.scale.x = 0.03; // line width (m) + line.color.r = 0.1f; + line.color.g = 0.5f; + line.color.b = 1.0f; + line.color.a = 0.9f; + line.lifetime = rclcpp::Duration(0, 0); // forever + + line.points.reserve(coarse_path.size()); + for (const auto &v : coarse_path) + { + geometry_msgs::msg::Point p; + p.x = v.x; + p.y = v.y; + p.z = v.z; + line.points.push_back(p); + } + rrt_vis_array.markers.push_back(line); + + // ----- SPHERE_LIST for vertices ----- + visualization_msgs::msg::Marker verts; + verts.header.frame_id = frame_id; + verts.header.stamp = stamp; + verts.ns = "rrt_coarse_path"; + verts.id = 1; + verts.type = visualization_msgs::msg::Marker::SPHERE_LIST; + verts.action = visualization_msgs::msg::Marker::ADD; + verts.pose.orientation.w = 1.0; + verts.scale.x = 0.10; + verts.scale.y = 0.10; + verts.scale.z = 0.10; // sphere diameter (m) + verts.color.r = 1.0f; + verts.color.g = 0.2f; + verts.color.b = 0.2f; + verts.color.a = 0.9f; + verts.lifetime = rclcpp::Duration(0, 0); + + verts.points.reserve(coarse_path.size()); + for (const auto &v : coarse_path) + { + geometry_msgs::msg::Point p; + p.x = v.x; + p.y = v.y; + p.z = v.z; + verts.points.push_back(p); + } + rrt_vis_array.markers.push_back(verts); + + // publish + planning_debug_vis->publish(rrt_vis_array); + } + } + else + { + RCLCPP_ERROR(this->get_logger(), "Failed to generate path, size was 0"); + } +} + +void ExplorationNode::publish_plan() +{ + nav_msgs::msg::Path full_path; + for (auto path : this->generated_paths) + { + for (auto point : path.poses) + { + full_path.poses.push_back(point); + } + } + full_path.header.stamp = this->now(); + full_path.header.frame_id = this->world_frame_id_; + this->pub_global_plan->publish(full_path); + RCLCPP_INFO(this->get_logger(), "Published full path"); +} + +void ExplorationNode::timerCallback() +{ + RCLCPP_INFO(this->get_logger(), "timer callback start"); + // get current TF to world + try + { + geometry_msgs::msg::TransformStamped transform_stamped = + this->tf_buffer->lookupTransform(this->world_frame_id_, this->robot_frame_id_, + rclcpp::Time(0)); + this->current_location = transform_stamped.transform; + if (!this->received_first_robot_tf) + { + this->received_first_robot_tf = true; + this->last_location = this->current_location; + this->last_position_change = this->now(); + RCLCPP_WARN(this->get_logger(), "Received first robot_tf"); + } + } + catch (tf2::TransformException &ex) + { + RCLCPP_ERROR(this->get_logger(), "Robot tf not received: %s", ex.what()); + } + + // Maintain the trajectory + ViewPoint vp; + vp.x = this->current_location.translation.x; + vp.y = this->current_location.translation.y; + vp.z = this->current_location.translation.z; + + vp.orientation.x = this->current_location.rotation.x; + vp.orientation.y = this->current_location.rotation.y; + vp.orientation.z = this->current_location.rotation.z; + vp.orientation.w = this->current_location.rotation.w; + + vp.orientation_set = true; + + double roll, pitch, yaw; + tf2::Quaternion tf2_q; + tf2::fromMsg(vp.orientation, tf2_q); // geometry_msgs -> tf2::Quaternion + tf2::Matrix3x3(tf2_q).getRPY(roll, pitch, yaw); + vp.orientation_yaw = yaw; + + this->exploration_planner->executed_trajectory_.push_back(vp); + this->exploration_planner->momentum_executed_trajectory_.push_back(TimedViewPoint{vp, this->now()}); + + // Prune old momentum samples (keep last momentum_time_ seconds) + const rclcpp::Time now = this->now(); + const rclcpp::Duration keep = rclcpp::Duration::from_seconds(this->momentum_time_); + + auto &traj = this->exploration_planner->momentum_executed_trajectory_; + while (!traj.empty() && (traj.front().time + keep) < now) + { + traj.erase(traj.begin()); + RCLCPP_INFO_ONCE(this->get_logger(), "Trim momentum traj"); + } + + // Update frontier map + + openvdb::Vec3d xyz; + openvdb::Coord ijk; + openvdb::FloatGrid::Accessor prev_acc = this->exploration_planner->prev_grid->getAccessor(); + openvdb::FloatGrid::Accessor lidar_acc = this->exploration_planner->occ_grid->getAccessor(); + openvdb::BoolGrid::Accessor frontier_acc = this->exploration_planner->frontier_grid->getAccessor(); + + for (auto iter = this->exploration_planner->frontier_grid->cbeginValueOn(); iter; ++iter) + { + ijk = iter.getCoord(); + // if the surrounding voxels are still frontier + bool still_front = surface_edge_frontier(lidar_acc, ijk); + if (!still_front) + { + frontier_acc.setValueOff(ijk, false); + } + } + // go through current free voxels + for (auto iter = this->exploration_planner->occ_grid->cbeginValueOn(); iter; ++iter) + { + ijk = iter.getCoord(); + openvdb::Vec3d xyz = this->exploration_planner->occ_grid->indexToWorld(ijk); + + // if (xyz.x() > explore_x_min_ && + // xyz.x() < explore_x_max_ && + // xyz.y() > explore_y_min_ && + // xyz.y() < explore_y_max_ && + // xyz.z() > explore_z_min_ && + // xyz.z() < explore_z_max_) + { + if (lidar_acc.getValue(ijk) < 0.0) + { // new free voxels + if (prev_acc.getValue(ijk) >= 0.0) + { + bool is_front = surface_edge_frontier(lidar_acc, ijk); + if (is_front) + { + frontier_acc.setValueOn(ijk, true); + } + } + } + } + } + + // update previous grid + this->exploration_planner->prev_grid = this->exploration_planner->occ_grid->deepCopy(); + + // clustering + openvdb::FloatGrid::Ptr visualized_cluster_grid_; + visualized_cluster_grid_ = openvdb::FloatGrid::create(0.0); + visualized_cluster_grid_->setTransform(this->exploration_planner->occ_grid->transform().copy()); + + std::vector> clustered_points_; + pcl::PointCloud::Ptr cluster_centroids_(new pcl::PointCloud); + + clustered_points_.clear(); + cluster_centroids_->clear(); + + point_clustering_vis(this->exploration_planner->frontier_grid, + this->cluster_cube_dim, + cluster_centroids_, + this->voxel_cluster_count_thresh, + clustered_points_, + visualized_cluster_grid_); + + // visualize + visualization_msgs::msg::Marker vdb_marker_msg; + generateVDBMarker(this->exploration_planner->occ_grid, map_frame_id_, vdb_marker_msg); + vdb_marker_msg.header.stamp = this->now(); + this->pub_vdb->publish(vdb_marker_msg); + + visualization_msgs::msg::Marker frontier_marker_msg; + generateFrontierMarker(this->exploration_planner->frontier_grid, map_frame_id_, frontier_marker_msg); + frontier_marker_msg.header.stamp = this->now(); + this->pub_frontier_vis->publish(frontier_marker_msg); + + visualization_msgs::msg::Marker clustered_marker_msg; + generateClusterMarker(visualized_cluster_grid_, map_frame_id_, clustered_marker_msg); + this->pub_clustered_frontier_vis->publish(clustered_marker_msg); + + // viewpoint sampling + this->exploration_planner->viewp_sample_->reset(clustered_points_); + this->exploration_planner->viewp_sample_->updateGridPtr(this->exploration_planner->occ_grid); + this->exploration_planner->viewp_sample_->updateCentroidListPtr(cluster_centroids_); + this->exploration_planner->viewp_sample_->sampleViewpoints(); + + RCLCPP_WARN(this->get_logger(), "Viewpoints Sampled %zu", this->exploration_planner->viewp_sample_->viewpoint_list_->size()); + + if (this->enable_exploration) + { + if (!this->is_path_executing) + { + if (this->exploration_planner->viewp_sample_->viewpoint_list_->size() > 0 && this->received_first_robot_tf) + { + + this->generate_plan(); + + this->publish_plan(); + this->is_path_executing = true; + this->last_position_change = this->now(); // Reset stall timer when starting new plan + } + else + { + RCLCPP_INFO(this->get_logger(), "viewpoint list size is %zu", this->exploration_planner->viewp_sample_->viewpoint_list_->size()); + RCLCPP_INFO(this->get_logger(), "tf received is %d", this->received_first_robot_tf); + } + } + else + { + // check if the robot has reached the goal point + std::tuple current_point = std::make_tuple( + this->current_location.translation.x, this->current_location.translation.y, + this->current_location.translation.z); + std::tuple goal_point = std::make_tuple( + this->current_goal_location.translation.x, this->current_goal_location.translation.y, + this->current_goal_location.translation.z); + if (get_point_distance(current_point, goal_point) < + this->exploration_planner->path_end_threshold_m) + { + this->is_path_executing = false; + this->generated_paths.clear(); + RCLCPP_INFO(this->get_logger(), "Reached goal point"); + } + else + { + // Check if position has changed significantly + std::tuple last_point = std::make_tuple(this->last_location.translation.x, + this->last_location.translation.y, + this->last_location.translation.z); + + if (get_point_distance(current_point, last_point) > this->position_change_threshold) + { + this->last_location = this->current_location; + this->last_position_change = this->now(); + } + else + { + // Check if we've been stationary for too long + rclcpp::Duration stall_duration = this->now() - this->last_position_change; + if (stall_duration.seconds() > this->stall_timeout_seconds) + { + RCLCPP_INFO(this->get_logger(), "Robot stationary for %f seconds, clearing plan", + stall_duration.seconds()); + this->is_path_executing = false; + this->generated_paths.clear(); + this->last_position_change = this->now(); // Reset timer to avoid spam + } + } + } + } + } + + RCLCPP_INFO(this->get_logger(), "timer callback end"); +} \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/exploration_node_run.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/exploration_node_run.cpp new file mode 100644 index 000000000..99ac0c206 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/exploration_node_run.cpp @@ -0,0 +1,11 @@ +#include "../include/exploration_node.hpp" + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + node->initialize(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/rrt_planner.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/rrt_planner.cpp new file mode 100644 index 000000000..d67cfca38 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/rrt_planner.cpp @@ -0,0 +1,784 @@ +/* + * Modified RRT Connection Function with Voxel Planner + * Copyright (C) 2020 Fan Yang - All rights reserved + * Original Author: fanyang2@andrew.cmu.edu + * Modified by: junbiny@andrew.cmu.edu + */ + +#include "utils/rrt_planner.h" + +// #define PI 3.14159265358979323846 // already in custom_point.h + +/* ---------------------------------------------------------------------------- */ +RRT_Planner::RRT_Planner() {} + +bool RRT_Planner::RRT_Init( // const float robot_model_cube_dim, + // const openvdb::BoolGrid::Ptr& grid_map, + // const float l_occ, + // const float voxel_dim, + const float norm_limit, + const float max_explore_dist, + const int max_iter, + const int max_connection_iter, + const int smooth_horizon, + // const bool consider_unknown_vox_occupied, + const int rrt_region_nodes_count, + const float rrt_region_nodes_radius, + const int rrt_region_nodes_z_layers_count_up, + const float rrt_region_nodes_z_layers_step, + const float dense_step) +{ + // Initialization + kSmooth_horizon_ = smooth_horizon; + // kRobot_model_cube_dim_ = robot_model_cube_dim; + kNorm_limit_ = norm_limit; + dense_step_ = dense_step; + kNorm_limit_sq_ = kNorm_limit_ * kNorm_limit_; + // kVoxel_dim_ = voxel_dim; + kMax_explore_dist_ = max_explore_dist; + kMax_iter_ = max_iter; + kMax_connection_iter_ = max_connection_iter; + // kConsider_unknown_vox_occupied_ = consider_unknown_vox_occupied; + + // Clear graph + start_tree_.clear(); + end_tree_.clear(); + + last_point_found_ = false; + + // grid_map_ = openvdb::BoolGrid::Ptr(grid_map); + // kL_occ_ = l_occ; + + kRRT_region_nodes_count_ = rrt_region_nodes_count; + kRRT_region_nodes_radius_ = rrt_region_nodes_radius; + kRRT_region_nodes_z_layers_count_up_ = rrt_region_nodes_z_layers_count_up; + kRRT_region_nodes_z_layers_step_ = rrt_region_nodes_z_layers_step; + + // Set seed + unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); + generator_ = std::default_random_engine(seed); + distribution_uniform_ = std::uniform_real_distribution(0.0, 1.0); + + // Debugging publishers + // ros::NodeHandle nh; + // viz_tree_start_pub_ = nh.advertise("viz_rrt_tree_start", 1); + // viz_tree_end_pub_ = nh.advertise("viz_rrt_tree_end", 1); + + return true; +} + +void RRT_Planner::addRegionVertices(Tree &tree) +{ + // Assumes edges has been initialised with one node + // Add new nodes, without collision checking, around this point + // These node form a cylinder + + if (kRRT_region_nodes_count_ > 0) + { + + // Get the existing point + const ViewPoint &point_center = tree.vertices[0]; + + float two_PI_over_count = 2.0 * PI / (float)kRRT_region_nodes_count_; + + // Add new points on a circle around the center + for (int i = 0; i < kRRT_region_nodes_count_; i++) + { + + float theta = (float)i * two_PI_over_count; + + float x = point_center.x + kRRT_region_nodes_radius_ * cos(theta); + float y = point_center.y + kRRT_region_nodes_radius_ * sin(theta); + + for (int j = -kRRT_region_nodes_z_layers_count_up_; j <= kRRT_region_nodes_z_layers_count_up_; j++) + { + // Create a new point for each z layer + ViewPoint point_new; + point_new.x = x; + point_new.y = y; + point_new.z = point_center.z + j * kRRT_region_nodes_z_layers_step_; + + // Ship it + tree.insertPoint(point_new, point_center); + } + } + } +} + +bool RRT_Planner::build_RRT(const ViewPoint &start_point, + const ViewPoint &end_point, collision_checker_ns::CollisionChecker &collision_checker, const bool reset_trees) +{ + if (collision_checker.collisionCheckInFreeSpace(start_point)) + { + std::cout << "Starting Point Not Free Enough \n"; + } + + if (collision_checker.collisionCheckInFreeSpace(end_point)) + { + std::cout << "End Point Not Free Enough \n"; + } + + // Initialize the trees + if (reset_trees) + { + start_tree_.clear(); + end_tree_.clear(); + } + + // Initialize flags + last_point_found_ = false; + bool is_path = false; + bool is_trace_path = true; + + // Save start and end points in tree + start_point_ = start_point; + end_point_ = end_point; + start_tree_.goal = end_point_; + end_tree_.goal = start_point_; + + if (reset_trees) + { + start_tree_.vertices.push_back(start_point_); + end_tree_.vertices.push_back(end_point_); + + // Add region vertices + addRegionVertices(start_tree_); + addRegionVertices(end_tree_); + } + + // Intermediate points + ViewPoint p_random; + ViewPoint p_nearest, p_new, p_nearest_goal, p_new_goal; + + // Create pointers to the two trees so they can be efficiently swapped + Tree *treeA = &start_tree_; + Tree *treeB = &end_tree_; + + // Iterate until goal found or max_iterations + for (int iteration = 0; iteration < kMax_iter_; ++iteration) + { + + // Pick a new random point + p_random = randomPoint(treeA->goal); + + // Get the closest point in graph to random point + p_nearest = nearestPoint(p_random, treeA->vertices); + + // Extend towards random point + extend(p_random, p_nearest, p_new); + + if (validEdge(p_new, p_nearest, collision_checker)) + { + + // Save the new vertex + treeA->insertPoint(p_new, p_nearest); + + // Extend from goal, too + p_nearest_goal = nearestPoint(p_new, treeB->vertices); + extend(p_new, p_nearest_goal, p_new_goal); // direction; start; new + ViewPoint p_new2; + if (validEdge(p_new_goal, p_nearest_goal, collision_checker)) + { + + // Save the new vertex + treeB->insertPoint(p_new_goal, p_nearest_goal); + + // Try to connect to the goal + int connection_iters = 0; + while (!nearGoal(p_new_goal, p_new) && connection_iters <= kMax_connection_iter_) + { + + // Extend towards goal + extend(p_new, p_new_goal, p_new2); + + if (validEdge(p_new2, p_new_goal, collision_checker)) + { + + // Save the new vertex + treeB->insertPoint(p_new2, p_new_goal); + p_new_goal = p_new2; + } + else + { + break; + } + + // Increment loop counter + connection_iters++; + } + if (connection_iters > kMax_connection_iter_) + { + printPoint(p_new); + printPoint(p_new_goal); + } + } + + // Check termination condition, add edge between trees (sort of) + if (nearGoal(p_new_goal, p_new)) + { + // treeB->edges[p_new] = p_new_goal; + treeB->insertPoint(p_new, p_new_goal); + last_point_ = p_new; + last_point_found_ = true; + is_path = true; + break; + } + } + + // Use the goal tree if it was better + if (treeA->vertices.size() > treeB->vertices.size()) + { + swapTreePointers(&treeA, &treeB); + } + } + + // Construct the path from the graph + if (is_path) + { + path_.clear(); + coarse_path_.clear(); + is_trace_path = extractPathFromGraph(collision_checker); + if (!is_trace_path) + { + path_.clear(); + } + } + + return (is_path && is_trace_path); +} + +// bool RRT_Planner::visualizeTrees() +// { +// // Convert both trees to point cloud msgs +// // So they can be viewed in rviz +// // (just the vertices, not the edges) + +// // First tree +// PCLCloudType pcl_cloud_start = pointSet2PCL(start_tree_.vertices); +// sensor_msgs::PointCloud2Ptr pcl_cloud_start_ros(new sensor_msgs::PointCloud2); +// pcl::toROSMsg(pcl_cloud_start, *pcl_cloud_start_ros); +// pcl_cloud_start_ros->header.frame_id = "map"; +// pcl_cloud_start_ros->header.stamp = ros::Time::now(); +// viz_tree_start_pub_.publish(pcl_cloud_start_ros); + +// // Second tree +// PCLCloudType pcl_cloud_end = pointSet2PCL(end_tree_.vertices); +// sensor_msgs::PointCloud2Ptr pcl_cloud_end_ros(new sensor_msgs::PointCloud2); +// pcl::toROSMsg(pcl_cloud_end, *pcl_cloud_end_ros); +// pcl_cloud_end_ros->header.frame_id = "map"; +// pcl_cloud_end_ros->header.stamp = ros::Time::now(); +// viz_tree_end_pub_.publish(pcl_cloud_end_ros); +// } + +/*bool RRT_Planner::collisionCheckInFreeSpace(const ViewPoint& p) { + bool is_in_collision; + // TODO - this conversion is unnecessary -> see second comment below + pcl::PointXYZI query_pt = point2PCL(p); + // LEGACY DEBT - TODO convert method to new point type + if(grid_map_ == NULL) { + // std::cout << "[RRT_Planner::collisionCheckInFreeSpace] Grid ptr within class is NULL - not initialized." << "\n"; + return false; + } + + if(!vdbmap::VDBUtil::checkPointInFreeSpace(grid_map_, query_pt, + kRobot_model_cube_dim_, kL_occ_, + kConsider_unknown_vox_occupied_)) { + is_in_collision = true; + return is_in_collision; + } else { + is_in_collision = false; + return is_in_collision; + } +}*/ + +void RRT_Planner::swapTreePointers(Tree **a, Tree **b) +{ + + // Swap the tree pointers + Tree *temp_tree = *a; + *a = *b; + *b = temp_tree; +} + +ViewPoint RRT_Planner::randomPoint(ViewPoint goal) +{ + + // With some probability, select goal instead of random point + double r = distribution_uniform_(generator_); + if (r < 0.90) + { + + // New random point + ViewPoint p; + float range, range_z; + + float dist = norm(subtract(end_point_, start_point_)); + + if (r < 0.50) + { + // Sample more at closer range + range = (dist + 3) * 1.5; + range_z = (end_point_.z - start_point_.z + 3) * 1.5; + } + else + { + // Wider sample range + range = (dist + 5) * 2.0; + range_z = (end_point_.z - start_point_.z + 3) * 2.0; + } + + p.x = (distribution_uniform_(generator_) - 0.5) * range + (start_point_.x + end_point_.x) / 2.0; + p.y = (distribution_uniform_(generator_) - 0.5) * range + (start_point_.y + end_point_.y) / 2.0; + p.z = (distribution_uniform_(generator_) - 0.5) * range_z + (start_point_.z + end_point_.z) / 2.0; + + return p; + } + else + { + + // Select the goal instead + return goal; + } +} + +void RRT_Planner::reversePath(PointSet &path) +{ + // Reverse the order of the trajectory, save in place + + std::reverse(path.begin(), path.end()); +} + +void RRT_Planner::extend(const ViewPoint &p_rand, const ViewPoint &p_near, ViewPoint &p_new) +{ + // Extend from p_near towards p_rand + // Return the extended point p_new + + // Get unit vector in direction + ViewPoint direction = subtract(p_rand, p_near); + double diff = norm(direction); + direction = divide(direction, diff); + + // Extend along unit vector, without going past p_rand + ViewPoint dist_step = multiply(direction, fmin(kNorm_limit_, diff)); + + // Get the new point + p_new = addPoint(p_near, dist_step); +} + +// bool RRT_Planner::validEdge(const ViewPoint &p_new, const ViewPoint &p_near, collision_checker_ns::CollisionChecker &collision_checker) +// { +// // Collision checking at discrete points between the two points? + +// ViewPoint check_point = p_near; +// ViewPoint direction = subtract(p_new, p_near); +// double diff = norm(direction); +// direction = divide(direction, diff); +// ViewPoint step = multiply(direction, double(kNorm_limit_)); // kVoxel_dim_ + +// int num_steps = ceil(diff / kNorm_limit_); // kVoxel_dim_ + +// // while(norm(subtract(check_point,p_near)) < diff) { +// for (int i = 0; i < num_steps; i++) +// { +// check_point = addPoint(check_point, step); +// if (collision_checker.collisionCheckInFreeSpace(check_point)) +// { +// return false; +// } +// } +// return true; +// } + +bool RRT_Planner::validEdge(const ViewPoint &p_new, const ViewPoint &p_near, collision_checker_ns::CollisionChecker &collision_checker) +{ + openvdb::Vec3d hit_point; + if (collision_checker.collisionCheckRayStrict(p_new, p_near, hit_point)) + { + return false; + } + return true; +} + +ViewPoint RRT_Planner::nearestPoint(const ViewPoint &p_rand, const PointSet &vertices) +{ + // Get the nearest vertex + // TODO KD-tree? + + bool first = true; + double min_norm_dist_sq = 0; + ViewPoint p_near; + + // Iterate through the vertices + for (const auto &vertex : vertices) + { + + // Compute the distance + double norm_dist_sq = normSq(subtract(vertex, p_rand)); + if (first || (norm_dist_sq < min_norm_dist_sq)) + { + min_norm_dist_sq = norm_dist_sq; + p_near = vertex; + first = false; + } + } + return p_near; +} + +inline bool RRT_Planner::nearGoal(const ViewPoint &p_new, const ViewPoint &goal) +{ + // Check if p_new is sufficiently close to the goal + + ViewPoint diff = subtract(goal, p_new); + double norm_dist_sq = normSq(diff); + if (norm_dist_sq < kNorm_limit_sq_) + { + return true; + } + return false; +} + +bool RRT_Planner::extractPathFromGraph(collision_checker_ns::CollisionChecker &collision_checker) +{ + // Extract path from graph + + bool is_trace_success = true; + + // Was the goal found? + if (!last_point_found_) + { + + // If not, stay stationary + path_.push_back(start_point_); + } + else + { + + // Found goal -- build path along graph + + // Initialize stuff + ViewPoint current; + PointSet path; + current = last_point_; + path.push_back(current); + std::size_t n_start_edges = start_tree_.edges.size(); + + // Construct a path + if (start_tree_.edges.count(current)) + { + + bool solution_found = false; + + // Upper bound to avoid infinite loop + for (int i = 0; i < 2 * n_start_edges; ++i) + { + + // Find parent + auto next_edge = start_tree_.edges.find(current); + if (next_edge != start_tree_.edges.end()) + { + + // Follow parent + current = next_edge->second; + path.push_back(current); + } + else + { + + // Reached root + solution_found = true; + break; + } + + // This condition appears to check for infinite loops? + // if (start_tree_.edges.find(parent) != start_tree_.edges.end() && equal(start_tree_.edges.find(parent)->second,current)) { + // is_trace_success = false; + // return false; + // }else{ + // Follow parent + // current = parent; + // } + // path.push_back(current); + } + if (!solution_found) + { + // Something has gone wrong + is_trace_success = false; + return false; + } + } + else + { + // Something has gone wrong + is_trace_success = false; + return false; + } + + // Do the same for end_edges + + // Initialize stuff + ViewPoint current1; + PointSet path1; + current1 = last_point_; + path1.push_back(current1); + std::size_t n_end_edges = end_tree_.edges.size(); + + // Construct a path + if (end_tree_.edges.count(current1)) + { + + bool solution_found = false; + + // Upper bound to avoid infinite loop + for (int i = 0; i < 2 * n_end_edges; ++i) + { + + // Find parent + auto next_edge = end_tree_.edges.find(current1); + if (next_edge != end_tree_.edges.end()) + { + + // Follow parent + current1 = next_edge->second; + path1.push_back(current1); + } + else + { + + // Reached root + solution_found = true; + break; + } + } + if (!solution_found) + { + // Something has gone wrong + is_trace_success = false; + return false; + } + } + else + { + // Something has gone wrong + is_trace_success = false; + return false; + } + + // Stitch the two paths together + if (equal(current, start_point_)) + { + reversePath(path); + path_.insert(path_.end(), path.begin(), path.end()); + path_.insert(path_.end(), path1.begin(), path1.end()); + } + else + { + // This case should no longer happen (since trees don't get directly swapped any more) + reversePath(path1); + path_.insert(path_.end(), path1.begin(), path1.end()); + path_.insert(path_.end(), path.begin(), path.end()); + } + } + + // Error checking + int path_size = path_.size(); + if (!equal(path_[0], start_point_) || !equal(path_[path_size - 1], end_point_)) + { + // Something has gone wrong + is_trace_success = false; + return false; + } + + // copy coarse path + coarse_path_ = path_; + + // Smooth the trajectory + pathSmooth(path_, collision_checker); + + // Ship it! + return is_trace_success; +} + +PointSet RRT_Planner::getPath() +{ + return path_; +} + +PointSet RRT_Planner::getCoarsePath() +{ + return coarse_path_; +} + +void RRT_Planner::interpolate(const ViewPoint &p1, const ViewPoint &p2, PointSet &path) +{ + // Interpolate points between two point -- exclusive of p1 and p2 + + ViewPoint check_point = p1; + ViewPoint direction = subtract(p2, p1); + double dist = norm(direction); + direction = divide(direction, dist); + ViewPoint step = multiply(direction, dense_step_); + while (norm(subtract(check_point, p1)) < dist - dense_step_) + { + check_point = addPoint(check_point, step); + path.push_back(check_point); + } +} + +// void RRT_Planner::pathSmooth(PointSet &path, collision_checker_ns::CollisionChecker &collision_checker) +// { +// /* Interpolate points --> smooth path */ +// // DEBUG +// // std::cout << "Smoothing Path ..."< check_idx) +// { +// forward_idx = int(fmin(forward_idx, num_path - 1)); +// p_forward = temp_path[forward_idx]; +// if (validEdge(p_forward, p_check, collision_checker)) +// { +// interpolate(p_check, p_forward, path); +// check_idx = forward_idx - 1; +// break; +// } +// forward_idx--; +// } +// last_p = temp_path[check_idx]; +// check_idx++; +// cur_p = temp_path[check_idx]; +// if (normSq(subtract(cur_p, last_p)) > kNorm_limit_sq_) +// { +// interpolate(last_p, cur_p, path); +// } +// } +// } + +void RRT_Planner::pathSmooth(PointSet &path, + collision_checker_ns::CollisionChecker &collision_checker) +{ + // straight line, no shortening + if (path.size() < 2) + return; + + // original discretized path (copy) + PointSet Pr; + Pr.push_back(path.front()); + for (size_t j = 0; j + 1 < path.size(); ++j) + { + if (equal(path[j], path[j+1])) continue; + interpolate(path[j], path[j+1], Pr); + Pr.push_back(path[j+1]); + } + + path.clear(); + + // Start with the first point + ViewPoint anchor = Pr.front(); + path.push_back(anchor); + + // Helpers to bridge ViewPoint <-> VDB world coords + auto fromVdb = [](const openvdb::Vec3d &v) -> ViewPoint + { + ViewPoint p; + p.x = v.x(); + p.y = v.y(); + p.z = v.z(); + return p; + }; + + // Tunables + const double step_delta = std::max(1e-3, static_cast(dense_step_)); // linear push step + const int max_pivot_steps = 5; // guard for very thick obstacles + const double normal_inflation = 1.0; // for estimateSurfaceNormalAtHit + + // Scan forward along Pr, only appending when blocked (or at the very end) + + for (size_t i = 1; i < Pr.size(); ++i) + { + // Try to see Pr[i] from current anchor + openvdb::Vec3d hit_world; + const bool blocked = collision_checker.collisionCheckRayStrict(anchor, Pr[i], hit_world); + + if (!blocked) + { + // Visible, connect only if it's the last point on path + if (i == Pr.size() - 1) + { + interpolate(anchor, Pr[i], path); + path.push_back(Pr[i]); + } + continue; + } + + // Blocked on Pr[i]: try to insert a first-hit pivot + bool pivot_inserted = false; + + // Estimate outward normal from occupancy around the hit + openvdb::Vec3d n_world; + bool have_normal = collision_checker.estimateSurfaceNormalAtHit(hit_world, + normal_inflation, + n_world); + if (have_normal) + { + // Linearly push along the normal until the local footprint is free + double delta = 0; + ViewPoint pivot; + + for (int step_count=0; step_count < max_pivot_steps; step_count++) + { + delta += step_delta; + pivot = fromVdb(hit_world + n_world * delta); + if (!collision_checker.collisionCheckInFreeSpace(pivot)) + { + if (validEdge(anchor, pivot, collision_checker) and + validEdge(pivot, Pr[i], collision_checker)) + { + pivot_inserted = true; + interpolate(anchor, pivot, path); + anchor = pivot; + // new anchor + path.push_back(anchor); + if (i == Pr.size() - 1) + { + interpolate(anchor, Pr[i], path); + path.push_back(Pr[i]); + } + break; + } + } + } + } + else + { + std::cout << "no normal at hit point " << hit_world.x() << ", " << hit_world.y() << ", " << hit_world.z() << '\n'; + pivot_inserted = false; + } + + // No pivot added, then fast forward to the last visible point on path + if (!pivot_inserted) + { + // Pivot failed (or became unnecessary) — append the last visible original waypoint + interpolate(anchor, Pr[i-1], path); + anchor = Pr[i-1]; + // new anchor + path.push_back(anchor); + if (i == Pr.size() - 1) + { + path.push_back(Pr[i]); + } + } + } +} + +// void RRT_Planner::updateGridPtr(const openvdb::BoolGrid::Ptr& grid) { +// grid_map_ = grid; +// } diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/utils.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/utils.cpp new file mode 100644 index 000000000..5d7768f51 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/utils.cpp @@ -0,0 +1,772 @@ +#include "utils/utils.hpp" +#include +#include +#include + +std_msgs::msg::ColorRGBA heightToColor(double norm_z) +{ + std_msgs::msg::ColorRGBA color; + color.a = 1.0; + + double h = norm_z * 0.8; + int i = static_cast(h * 6.0); + double f = (h * 6.0) - i; + double q = 1.0 - f; + + switch (i % 6) + { + case 0: + color.r = 1.0; + color.g = f; + color.b = 0.0; + break; + case 1: + color.r = q; + color.g = 1.0; + color.b = 0.0; + break; + case 2: + color.r = 0.0; + color.g = 1.0; + color.b = f; + break; + case 3: + color.r = 0.0; + color.g = q; + color.b = 1.0; + break; + case 4: + color.r = f; + color.g = 0.0; + color.b = 1.0; + break; + case 5: + color.r = 1.0; + color.g = 0.0; + color.b = q; + break; + default: + color.r = 1.0; + color.g = 0.5; + color.b = 0.5; + break; + } + return color; +} + +std_msgs::msg::ColorRGBA valueToRainbowColor(float value) +{ + std_msgs::msg::ColorRGBA color; + value = std::fmod(value, 1.0f); // Wrap values > 1.0 around + + float r = std::fabs(value * 6 - 3) - 1; + float g = 2 - std::fabs(value * 6 - 2); + float b = 2 - std::fabs(value * 6 - 4); + + color.r = std::clamp(r, 0.0f, 1.0f); + color.g = std::clamp(g, 0.0f, 1.0f); + color.b = std::clamp(b, 0.0f, 1.0f); + color.a = 1.0; + return color; +} + +// void generateVDBMarker(const GridT::Ptr& grid, +// const std::string& frame_id, +// visualization_msgs::msg::Marker& marker_msg) +// { +// marker_msg.header.frame_id = frame_id; +// marker_msg.header.stamp = rclcpp::Clock().now(); +// marker_msg.ns = "vdb_grid"; +// marker_msg.id = 0; +// marker_msg.type = visualization_msgs::msg::Marker::CUBE_LIST; +// marker_msg.action = visualization_msgs::msg::Marker::ADD; +// marker_msg.pose.orientation.w = 1.0; +// marker_msg.color.a = 1.0; +// marker_msg.frame_locked = true; + +// double voxel_size = grid->voxelSize()[0]; +// marker_msg.scale.x = voxel_size; +// marker_msg.scale.y = voxel_size; +// marker_msg.scale.z = voxel_size; + +// openvdb::CoordBBox bbox = grid->evalActiveVoxelBoundingBox(); +// double z_min = grid->indexToWorld(bbox.min()).z(); +// double z_max = grid->indexToWorld(bbox.max()).z(); +// double dz = z_max - z_min; + +// for (auto iter = grid->cbeginValueOn(); iter; ++iter) { +// openvdb::Vec3d world = grid->indexToWorld(iter.getCoord()); + +// geometry_msgs::msg::Point pt; +// pt.x = world.x(); +// pt.y = world.y(); +// pt.z = world.z(); +// marker_msg.points.push_back(pt); + +// double norm_z = (dz > 1e-3) ? (world.z() - z_min) / dz : 0.5; +// marker_msg.colors.push_back(heightToColor(norm_z)); +// } + +// if (marker_msg.points.empty()) { +// marker_msg.action = visualization_msgs::msg::Marker::DELETE; +// } +// } + +void generateVDBMarker(const openvdb::FloatGrid::Ptr &grid, + const std::string &frame_id, + visualization_msgs::msg::Marker &marker_msg) +{ + marker_msg.header.frame_id = frame_id; + marker_msg.header.stamp = rclcpp::Clock().now(); + marker_msg.ns = "vdb_grid"; + marker_msg.id = 0; + marker_msg.type = visualization_msgs::msg::Marker::CUBE_LIST; + marker_msg.action = visualization_msgs::msg::Marker::ADD; + marker_msg.pose.orientation.w = 1.0; + marker_msg.color.a = 1.0; + marker_msg.frame_locked = true; + + double voxel_size = grid->voxelSize()[0]; + marker_msg.scale.x = voxel_size; + marker_msg.scale.y = voxel_size; + marker_msg.scale.z = voxel_size; + + openvdb::CoordBBox bbox = grid->evalActiveVoxelBoundingBox(); + double z_min = grid->indexToWorld(bbox.min()).z(); + double z_max = grid->indexToWorld(bbox.max()).z(); + double dz = z_max - z_min; + + for (auto iter = grid->cbeginValueOn(); iter; ++iter) + { + if (iter.getValue() <= 0.0f) + { + continue; + } + + openvdb::Vec3d world = grid->indexToWorld(iter.getCoord()); + + geometry_msgs::msg::Point pt; + pt.x = world.x(); + pt.y = world.y(); + pt.z = world.z(); + marker_msg.points.push_back(pt); + + double norm_z = (dz > 1e-3) ? (world.z() - z_min) / dz : 0.5; + marker_msg.colors.push_back(heightToColor(norm_z)); + } + + if (marker_msg.points.empty()) + { + marker_msg.action = visualization_msgs::msg::Marker::DELETE; + } +} + +void generateFrontierMarker(const openvdb::BoolGrid::Ptr &grid, + const std::string &frame_id, + visualization_msgs::msg::Marker &marker_msg) +{ + marker_msg.header.frame_id = frame_id; + marker_msg.header.stamp = rclcpp::Clock().now(); + marker_msg.ns = "vdb_grid"; + marker_msg.id = 0; + marker_msg.type = visualization_msgs::msg::Marker::CUBE_LIST; + marker_msg.action = visualization_msgs::msg::Marker::ADD; + marker_msg.pose.orientation.w = 1.0; + marker_msg.color.a = 1.0; + marker_msg.frame_locked = true; + + double voxel_size = grid->voxelSize()[0]; + marker_msg.scale.x = voxel_size; + marker_msg.scale.y = voxel_size; + marker_msg.scale.z = voxel_size; + + openvdb::CoordBBox bbox = grid->evalActiveVoxelBoundingBox(); + double z_min = grid->indexToWorld(bbox.min()).z(); + double z_max = grid->indexToWorld(bbox.max()).z(); + double dz = z_max - z_min; + + for (auto iter = grid->cbeginValueOn(); iter; ++iter) + { + if (!iter.getValue()) + { + continue; + } + + openvdb::Vec3d world = grid->indexToWorld(iter.getCoord()); + + geometry_msgs::msg::Point pt; + pt.x = world.x(); + pt.y = world.y(); + pt.z = world.z(); + + std_msgs::msg::ColorRGBA color; + color.r = 1.0; + color.g = 0.0; + color.b = 0.0; + color.a = 1.0; + + marker_msg.points.push_back(pt); + marker_msg.colors.push_back(color); + } + + if (marker_msg.points.empty()) + { + marker_msg.action = visualization_msgs::msg::Marker::DELETE; + } +} + +void generateClusterMarker(const openvdb::FloatGrid::Ptr &grid, + const std::string &frame_id, + visualization_msgs::msg::Marker &marker_msg) +{ + marker_msg.header.frame_id = frame_id; + marker_msg.header.stamp = rclcpp::Clock().now(); + marker_msg.ns = "cluster_grid"; + marker_msg.id = 0; + marker_msg.type = visualization_msgs::msg::Marker::CUBE_LIST; + marker_msg.action = visualization_msgs::msg::Marker::ADD; + marker_msg.pose.orientation.w = 1.0; + marker_msg.color.a = 1.0; + marker_msg.frame_locked = true; + + double voxel_size = grid->voxelSize()[0]; + marker_msg.scale.x = voxel_size; + marker_msg.scale.y = voxel_size; + marker_msg.scale.z = voxel_size; + + for (auto iter = grid->cbeginValueOn(); iter; ++iter) + { + float cluster_val = iter.getValue(); + if (cluster_val <= 0.0f) + continue; // skip unclustered voxels + + openvdb::Vec3d world = grid->indexToWorld(iter.getCoord()); + + geometry_msgs::msg::Point pt; + pt.x = world.x(); + pt.y = world.y(); + pt.z = world.z(); + marker_msg.points.push_back(pt); + + marker_msg.colors.push_back(valueToRainbowColor(cluster_val)); + } + + if (marker_msg.points.empty()) + { + marker_msg.action = visualization_msgs::msg::Marker::DELETE; + } +} + +bool surface_edge_frontier(openvdb::FloatGrid::Accessor accessor, + openvdb::Coord ijk) +{ + openvdb::math::Coord ijk_n; + float gridvalue; + std::vector unknown_list; + std::vector occupied_list; + for (int di = -1; di <= 1; ++di) + { + ijk_n[0] = ijk[0] + di; + for (int dj = -1; dj <= 1; ++dj) + { + ijk_n[1] = ijk[1] + dj; + for (int dk = -1; dk <= 1; ++dk) + { + ijk_n[2] = ijk[2] + dk; + bool surround_state = accessor.probeValue(ijk_n, gridvalue); + // if there is unknown neighbor, surround_state will be inactive + if (!surround_state) + { + unknown_list.push_back(Eigen::Vector3d(di, dj, dk)); + // std::cout << "pushing unknown" << '\n'; + } + else if (gridvalue > 0) + { + occupied_list.push_back(Eigen::Vector3d(di, dj, dk)); + // std::cout << "pushing occ" << '\n'; + } + } + } + } + if (unknown_list.empty() || occupied_list.empty()) + { + return false; + } + for (size_t i = 0; i < unknown_list.size(); ++i) + { + for (size_t j = 0; j < occupied_list.size(); ++j) + { + Eigen::Vector3d un_occ_diff_vec = unknown_list[i] - occupied_list[j]; + // std::cout << "diff is " << un_occ_diff_vec.dot(un_occ_diff_vec) << '\n'; + if (un_occ_diff_vec.dot(un_occ_diff_vec) <= 3) + { + return true; + } + } + } + return false; +} + +void create_clustering_coordbbox(const openvdb::BoolGrid::Ptr grid, + openvdb::math::CoordBBox &coord_bbox_out, + const openvdb::Vec3d ¢er, + const float frontier_count_cube_dim) +{ + openvdb::Vec3d bbox_min, bbox_max; + openvdb::Coord ijk_bbox_min, ijk_bbox_max; + openvdb::math::Transform &grid_tf(grid->transform()); + + bbox_min.x() = center.x() - (frontier_count_cube_dim / 2); + bbox_max.x() = center.x() + (frontier_count_cube_dim / 2); + + bbox_min.y() = center.y() - (frontier_count_cube_dim / 2); + bbox_max.y() = center.y() + (frontier_count_cube_dim / 2); + + bbox_min.z() = center.z() - (frontier_count_cube_dim / 2); + bbox_max.z() = center.z() + (frontier_count_cube_dim / 2); + + ijk_bbox_min = grid_tf.worldToIndexCellCentered(bbox_min); + ijk_bbox_max = grid_tf.worldToIndexCellCentered(bbox_max); + + coord_bbox_out.reset(ijk_bbox_min, ijk_bbox_max); +} + +void point_clustering_vis(openvdb::BoolGrid::Ptr grid, + const float cluster_cube_dim, + pcl::PointCloud::Ptr cluster_cloud, + const int voxel_cluster_count_thresh, + std::vector> &clustered_points, + openvdb::FloatGrid::Ptr visualized_clusters) +{ + // for visualization only + float cluster_num = 0; + openvdb::FloatGrid::Accessor vis_acc = visualized_clusters->getAccessor(); + // + + openvdb::BoolGrid::Ptr grid_copy = grid->deepCopy(); + int voxel_count = 0; + openvdb::BoolGrid::Accessor acc = grid_copy->getAccessor(); + const openvdb::math::Transform &grid_tf(grid_copy->transform()); + pcl::PointXYZI cluster_score_point; + bool voxel_value = false; + std::vector points_buffer; + for (openvdb::BoolGrid::ValueOnCIter itr = grid_copy->cbeginValueOn(); itr; ++itr) + { + voxel_value = itr.getValue(); + // voxels that have not been "marked" before + if (voxel_value) + { + // for visualization only + std::vector vis_boxes; + // + + openvdb::Coord ijk = itr.getCoord(); + openvdb::Vec3d xyz = grid_tf.indexToWorld(ijk); + // create a bbox centered around xyz + openvdb::math::CoordBBox bbox; + create_clustering_coordbbox(grid_copy, bbox, xyz, cluster_cube_dim); + // iterate through bbox and find intersecting grid voxels + // save points to buffer, copy from buffer to output list + // if cluster is big enough + // count neighboring voxels (with voxel_value > voxel_mark_priority_medium) within bbox + voxel_count = 0; + points_buffer.clear(); + for (auto ijk = bbox.beginXYZ(); ijk; ++ijk) + { + openvdb::Coord idx = *ijk; + openvdb::Vec3d pt_xyz = grid_tf.indexToWorld(idx); + if (acc.getValue(idx)) + { + Eigen::Vector3d point{pt_xyz.x(), pt_xyz.y(), pt_xyz.z()}; + points_buffer.push_back(point); + voxel_count += 1; + + // for visualization only + openvdb::Coord vis_ijk = idx; + vis_boxes.push_back(vis_ijk); + // + } + } + // if nn vox count > thresh + // insert xyz and nn vox count as an XYZI point into cluster cloud + // mark all nn voxels and cluster centroid vox as --> voxel_mark_priority_medium + if (voxel_count >= voxel_cluster_count_thresh) + { + Eigen::Vector3d cluster_centroid = VDBUtil::computeCentroid(points_buffer); + cluster_score_point.x = cluster_centroid.x(); + cluster_score_point.y = cluster_centroid.y(); + cluster_score_point.z = cluster_centroid.z(); + cluster_score_point.intensity = voxel_count; + cluster_cloud->push_back(cluster_score_point); + mark_voxels_bool(points_buffer, grid_copy, false); + clustered_points.push_back(points_buffer); + + // for visualization only, one cluster one value, thus one color. + cluster_num += 0.1; + for (size_t pt = 0; pt < vis_boxes.size(); ++pt) + { + vis_acc.setValue(vis_boxes[pt], cluster_num); + } + // + } + } + } +} + +void mark_voxels_bool(const std::vector &point_list, + openvdb::BoolGrid::Ptr grid, + const bool voxel_value) +{ + openvdb::BoolGrid::Accessor acc = grid->getAccessor(); + const openvdb::math::Transform &grid_tf(grid->transform()); + for (auto &point : point_list) + { + openvdb::Vec3d xyz{point.x(), point.y(), point.z()}; + openvdb::Coord ijk = grid_tf.worldToIndexCellCentered(xyz); + acc.setValue(ijk, voxel_value); + } +} + +Eigen::Vector3d VDBUtil::computeCentroid(const std::vector &point_list) +{ + + Eigen::Vector3d sum = std::accumulate(point_list.begin(), + point_list.end(), + Eigen::Vector3d::Zero().eval()); + // Eigen::Vector3d centroid {sum / point_list.size()}; + + return sum / point_list.size(); +} + +void VDBUtil::updateOccMapFromNdArray(openvdb::FloatGrid::Ptr grid_logocc, + pcl::PointCloud::Ptr xyz, + const tf2::Vector3 &origin, + float l_free, + float l_occ, + float l_min, + float l_max, + int hit_thickness, + double threshold, + float visited_cleared_logocc_min_thresh) +{ + openvdb::FloatGrid::Accessor acc = grid_logocc->getAccessor(); + openvdb::Vec3d origin2(origin.x(), origin.y(), origin.z()); + openvdb::Vec3d origin_ijk = grid_logocc->worldToIndex(origin2); + + for (size_t i = 0; i < xyz->points.size(); ++i) + { + openvdb::Vec3d p_ijk = grid_logocc->worldToIndex(openvdb::Vec3d(xyz->points[i].x, + xyz->points[i].y, + xyz->points[i].z)); + + openvdb::Vec3d dir(p_ijk - origin_ijk); + double range = dir.length(); + dir.normalize(); + openvdb::math::Ray ray(origin_ijk, dir); + // ray, start_time, max_time + + if (range <= threshold) + { + openvdb::math::DDA, 0> dda(ray, 0., range); + // want to stop before dda.time == dda.maxTime + do + { + openvdb::Coord ijk(dda.voxel()); + float ll = acc.getValue(ijk); + if (ll < visited_cleared_logocc_min_thresh) + { + acc.setValue(ijk, std::max(l_min, ll + l_free)); + } + dda.step(); + } while (dda.time() < dda.maxTime()); + + // post condition, at hit voxel + for (int i = 0; i < hit_thickness; ++i) + { + openvdb::Coord ijk(dda.voxel()); + float ll = acc.getValue(ijk); + if (ll < visited_cleared_logocc_min_thresh) + { + acc.setValue(ijk, std::min(l_max, ll + l_occ)); + } + dda.step(); + } + } + else + { + openvdb::math::DDA, 0> dda(ray, 0., std::min(range, threshold)); + do + { + openvdb::Coord ijk(dda.voxel()); + float ll = acc.getValue(ijk); + if (ll < visited_cleared_logocc_min_thresh) + { + acc.setValue(ijk, std::max(l_min, ll + l_free)); + } + dda.step(); + } while (dda.time() < dda.maxTime()); + } + } +} + +void VDBUtil::setVoxelSize(openvdb::GridBase &grid, double vs) +{ + // cell-centered + const openvdb::math::Vec3d offset(vs / 2., vs / 2., vs / 2.); + openvdb::math::Transform::Ptr tf = openvdb::math::Transform::createLinearTransform(vs); + tf->postTranslate(offset); + grid.setTransform(tf); +} + +bool VDBUtil::collCheckPointInPartialFreeSpace(const openvdb::FloatGrid::Ptr grid, + const Eigen::Vector3d &point_xyz, + const float length, + const float breadth, + const float height, + const float l_occ, + const float unknown_fraction_thresh, + const float occupied_fraction_thresh) +{ + + bool point_is_in_free_space; + if (grid->empty()) + { + std::cerr << "[VDBUtil::checkPointInFreeSpace] Grid is empty. \ + Cannot perform collision check. Exiting." + << "\n"; + return false; + } + const openvdb::math::Transform &grid_tf(grid->transform()); + openvdb::Vec3d xyz(point_xyz.x(), point_xyz.y(), point_xyz.z()); + openvdb::math::CoordBBox coord_bbox; + + VDBUtil::createCoordBBox(grid, coord_bbox, xyz, + length, breadth, height); + + long int bbox_total_vox_count = 0; + long int unknown_vox_count = 0; + long int occupied_vox_count = 0; + openvdb::FloatGrid::Accessor acc = grid->getAccessor(); + for (auto ijk = coord_bbox.beginXYZ(); ijk; ++ijk) + { + ++bbox_total_vox_count; + openvdb::Coord idx = *ijk; + float vox_val = 0.0; + bool vox_state = acc.probeValue(idx, vox_val); + if (vox_state && vox_val > l_occ) + { + ++occupied_vox_count; + } + else if (!vox_state) + { + ++unknown_vox_count; + } + } + double occupied_fraction = double(occupied_vox_count) / double(bbox_total_vox_count); + double unknown_fraction = double(unknown_vox_count) / double(bbox_total_vox_count); + // if ( occupied_fraction <= occupied_fraction_thresh && unknown_fraction <= unknown_fraction_thresh) { + // point_is_in_free_space = true; + // } else { // in collision + // point_is_in_free_space = false; + // } + + if (occupied_fraction > occupied_fraction_thresh || unknown_fraction > unknown_fraction_thresh) + { + point_is_in_free_space = false; + } + else + { // in free space + point_is_in_free_space = true; + } + + return point_is_in_free_space; +} + +bool VDBUtil::checkCollisionAlongRay(const openvdb::FloatGrid::Ptr& grid, + const openvdb::Vec3d &p1_xyz, + const openvdb::Vec3d &p2_xyz, + const float bbox_length, + const float bbox_breadth, + const float bbox_height, + const float l_occ, + openvdb::Vec3d &hit_point, + float dist_offset_from_cluster, + bool consider_unknown_occupied) +{ + + bool in_collision = false; + openvdb::Coord ray_element_ijk; + openvdb::Vec3d ray_element_xyz; + + openvdb::FloatGrid::Accessor acc = grid->getAccessor(); + const openvdb::math::Transform &grid_tf(grid->transform()); + + openvdb::Vec3d p1_ijk = grid_tf.worldToIndex(p1_xyz); + openvdb::Vec3d p2_ijk = grid_tf.worldToIndex(p2_xyz); + openvdb::Vec3d dir(p2_ijk - p1_ijk); + double distance = dir.length(); + dir.normalize(); + openvdb::math::Ray ray(p1_ijk, dir); + openvdb::math::DDA, 0> dda(ray, 0., distance); + // want to stop before dda.time == dda.maxTime + // do { + while (dda.next() < dda.maxTime()) + { + + ray_element_ijk = dda.voxel(); + // check for robot model cube occupancy + ray_element_xyz = grid->indexToWorld(ray_element_ijk); + openvdb::math::CoordBBox coord_bbox; + VDBUtil::createCoordBBox(grid, coord_bbox, ray_element_xyz, + bbox_length, bbox_breadth, bbox_height); + if (VDBUtil::checkIntersectWithCoordBBox(coord_bbox, grid, + l_occ, consider_unknown_occupied)) + { + in_collision = true; + break; + } + dda.step(); + } + // } while (dda.time() < dda.maxTime()); + // } while (dda.next() < dda.maxTime()); + + if (in_collision) + { + // openvdb::Vec3d hit_ray(ray_element_ijk - p1_ijk); + // double hit_distance = hit_ray.length(); + // double min_threshold_distance = distance - dist_offset_from_cluster; + // if (hit_distance >= min_threshold_distance) + // { + // return false; + // } + // post condition, at hit voxel + hit_point = ray_element_xyz; + return true; + } + else + { + return false; + } +} + +long int VDBUtil::countVoxelsInsideBbox(const openvdb::FloatGrid::Ptr &grid, + const Eigen::Vector3d &bbox_center, + const float length, + const float breadth, + const float height) +{ + + openvdb::CoordBBox bbox; + long int vox_count = 0; + float log_occ_val = 0.0; + const openvdb::math::Transform &grid_tf(grid->transform()); + openvdb::FloatTree::Ptr grid_tree = grid->treePtr(); + createCoordBBox(grid, + bbox, + convertEigenVecToVec3D(bbox_center), + length, + breadth, + height); + + for (auto bbox_iter = bbox.beginXYZ(); bbox_iter; ++bbox_iter) + { + openvdb::Coord ijk = *bbox_iter; + if (grid_tree->probeValue(ijk, log_occ_val)) + { + vox_count += 1; + } + } + return vox_count; +} + +// void VDBUtil::createCoordBBox(const openvdb::FloatGrid::Ptr grid, +// openvdb::math::CoordBBox &coord_bbox_out, +// const openvdb::Vec3d ¢er, +// const float bbox_length, +// const float bbox_breadth, +// const float bbox_height) +// { + +// openvdb::Vec3d bbox_min, bbox_max; +// openvdb::Coord ijk_bbox_min, ijk_bbox_max; +// openvdb::math::Transform &grid_tf(grid->transform()); + +// bbox_min.x() = center.x() - (bbox_length / 2); +// bbox_max.x() = center.x() + (bbox_length / 2); + +// bbox_min.y() = center.y() - (bbox_breadth / 2); +// bbox_max.y() = center.y() + (bbox_breadth / 2); + +// bbox_min.z() = center.z() - (bbox_height / 2); +// bbox_max.z() = center.z() + (bbox_height / 2); + +// ijk_bbox_min = grid_tf.worldToIndexCellCentered(bbox_min); +// ijk_bbox_max = grid_tf.worldToIndexCellCentered(bbox_max); + +// coord_bbox_out.reset(ijk_bbox_min, ijk_bbox_max); +// } + +// The above implementation rounds to the nearest cell center. +// If bbox_max lands just past a cell center, rounding can pull it back and +// you’ll silently drop the outermost layer of cells. +// Use a continuous index bbox and floor/ceil: +void VDBUtil::createCoordBBox(const openvdb::FloatGrid::Ptr grid, + openvdb::math::CoordBBox &coord_bbox_out, + const openvdb::Vec3d ¢er, + const float bbox_length, + const float bbox_breadth, + const float bbox_height) +{ + const auto& tf = grid->transform(); + const openvdb::Vec3d half(bbox_length*0.5, bbox_breadth*0.5, bbox_height*0.5); + + // World-space box -> continuous index-space box + const openvdb::BBoxd wbox(center - half, center + half); + const openvdb::BBoxd ibox = tf.worldToIndex(wbox); + + // Integer (cell-centered) bounds: floor(min), ceil(max) + const openvdb::Coord ijk_min( + static_cast(std::floor(ibox.min().x() + 0.5)), + static_cast(std::floor(ibox.min().y() + 0.5)), + static_cast(std::floor(ibox.min().z() + 0.5))); + + const openvdb::Coord ijk_max( + static_cast(std::ceil (ibox.max().x() - 0.5)), + static_cast(std::ceil (ibox.max().y() - 0.5)), + static_cast(std::ceil (ibox.max().z() - 0.5))); + + coord_bbox_out.reset(ijk_min, ijk_max); +} + +bool VDBUtil::checkIntersectWithCoordBBox(const openvdb::math::CoordBBox &search_bbox, + const openvdb::FloatGrid::Ptr grid, + float occupancy_threshold, + bool consider_unknown_occupied) +{ + + openvdb::math::Transform &grid_tf(grid->transform()); + const size_t count = search_bbox.volume(); + openvdb::Vec3d xyz; + openvdb::FloatGrid::Accessor acc = grid->getAccessor(); + for (auto iter = search_bbox.beginXYZ(); iter; ++iter) + { + openvdb::Coord ijk = *iter; + float vox_val = 0.0; + bool vox_state = acc.probeValue(ijk, vox_val); + if (vox_state && vox_val > occupancy_threshold) + { + return true; + } + else if (consider_unknown_occupied && std::fabs(vox_val) <= FLT_EPSILON) + { + return true; + } + } + return false; +} + +openvdb::Vec3d VDBUtil::convertEigenVecToVec3D(const Eigen::Vector3d &point) +{ + + return openvdb::Vec3d(point.x(), point.y(), point.z()); +} \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/viewpoint_sampling.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/viewpoint_sampling.cpp new file mode 100644 index 000000000..827325211 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/viewpoint_sampling.cpp @@ -0,0 +1,488 @@ +/* + * Viewpoint Sampling Module + * Copyright (C) 2020 Rohit Garg - All rights reserved + * Original author: rg1@cmu.edu / rohitgarg617@gmail.com + * Modified by junbiny@andrew.cmu.edu, removed matplotlib part and modified viewpoint sampling method + */ + +#include "../include/viewpoint_sampling.hpp" + +namespace viewpoint_sampling_ns +{ + // constructor + ViewpointSampling::ViewpointSampling(ListOfClustersT &cluster_list, + double viewing_distance, + float bbox_length, + float bbox_breadth, + float bbox_height, + float l_occ, + float vox_size, + uint16_t num_viewp_sample_per_cluster, + double viewing_distance_inner_r, + double viewing_distance_outer_r, + double viewing_distance_z_offset, + double coll_check_endpoint_offset, + double bbox_fraction_viewp_centroid_coll_check, + double viewp_bbox_unknown_frac_thresh, + double viewp_bbox_occ_frac_thresh) + { + + viewing_distance_ = viewing_distance; + bbox_length_ = bbox_length; + bbox_breadth_ = bbox_breadth; + bbox_height_ = bbox_height; + l_occ_ = l_occ; + vox_size_ = vox_size; + num_viewp_to_sample_ = num_viewp_sample_per_cluster; + viewing_distance_inner_r_ = viewing_distance_inner_r; + viewing_distance_outer_r_ = viewing_distance_outer_r; + viewing_distance_z_offset_ = viewing_distance_z_offset; + coll_check_endpoint_offset_ = coll_check_endpoint_offset; + bbox_fraction_viewp_centroid_coll_check_ = bbox_fraction_viewp_centroid_coll_check; + cluster_list_ = std::make_shared(cluster_list); + viewpoint_list_ = pcl::PointCloud::Ptr(new pcl::PointCloud()); + viewp_bbox_unknown_frac_thresh_ = viewp_bbox_unknown_frac_thresh; + viewp_bbox_occ_frac_thresh_ = viewp_bbox_occ_frac_thresh; + } + + // reset data stored by the class + void ViewpointSampling::reset(ListOfClustersT &cluster_list) + { + + // TODO -> not sure if this make_shared is required + cluster_list_ = std::make_shared(cluster_list); + xy_points_.clear(); + viewpoint_list_->clear(); + } + + // update the global map grid pointer + void ViewpointSampling::updateGridPtr(const openvdb::FloatGrid::Ptr &grid) + { + grid_map_ = grid; + } + + // update the cluster centroid list pointer + void ViewpointSampling::updateCentroidListPtr( + const pcl::PointCloud::Ptr ¢roid_list) + { + centroid_list_ = centroid_list; + } + + // main method that samples viewpoints + void ViewpointSampling::sampleViewpoints() + { + cluster_idx_ = -1; + + for (const auto &cluster : *cluster_list_) + { + + cluster_idx_ += 1; + + // current_cluster_centroid_ = computeMean(); + pcl::PointXYZI centroid = centroid_list_->at(cluster_idx_); + current_cluster_score_ = centroid.intensity; + current_cluster_centroid_.x() = centroid.x; + current_cluster_centroid_.y() = centroid.y; + current_cluster_centroid_.z() = centroid.z; + sampleViewpointsAroundCentroid(); + } + } + + // remove z component from 3D points + void ViewpointSampling::projectToXYPlane() + { + xy_points_.clear(); + for (const auto &point : cluster_list_->at(cluster_idx_)) + { + Eigen::Vector2d point_in{point.x(), point.y()}; + xy_points_.push_back(point_in); + } + } + + // logging to file + void ViewpointSampling::logDataToFile() + { + + std::string file1 = "cluster"; + std::string file_path1 = setupFilePath(cluster_idx_, file1); + writeVecToCSVfile(file_path1, xy_points_); + + std::string file2 = "fit_line_wsolver" + std::to_string(solver_type_); + std::string file_path2 = setupFilePath(cluster_idx_, file2); + Eigen::MatrixXd output_mat = fitted_line_; + writeToCSVfile(file_path2, output_mat); + } + + // generate file path + std::string ViewpointSampling::setupFilePath(int file_index, std::string file_name) + { + std::string fname = file_name + "_"; + std::string path = "/home/rohit/rWorkspace/vdb_map_ws/src/map_processor/logs/"; + std::string idx = std::to_string(file_index); + std::string fformat = ".txt"; + return path + fname + idx + fformat; + } + + // write matrix to a CSV file + void ViewpointSampling::writeToCSVfile(std::string file_name, Eigen::MatrixXd &matrix) + { + const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n"); + std::ofstream file(file_name.c_str()); + file << matrix.format(CSVFormat); + file.close(); + } + + // write vector to a CSV file + void ViewpointSampling::writeVecToCSVfile(std::string file_name, + std::vector &points) + { + const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision, 0, ", ", "\n"); + std::ofstream file(file_name.c_str()); + for (const auto &point : points) + { + file << point(0) << ", " << point(1) << "\n"; + } + file.close(); + } + + // splitting into two separate vectors + void ViewpointSampling::toXVecYVec() + { + for (auto point : xy_points_) + { + cluster_x_points_.push_back(point.x()); + cluster_y_points_.push_back(point.y()); + } + } + + // discretize line into a list of points + void ViewpointSampling::drawPointsAlongLine(float spacing, float length) + { + double start_x = current_cluster_centroid_.x(); + uint64_t n_points = std::floor(length / spacing); + for (int i = 0; i < n_points; ++i) + { + double line_x_pt = start_x + (i * spacing); + double line_y_pt = (fitted_line_(0) * line_x_pt) + fitted_line_(0); + line_x_points_.push_back(line_x_pt); + line_y_points_.push_back(line_y_pt); + } + } + + // Ax=b -> A matrix + void ViewpointSampling::createAMatrix() + { + A_mat_ = Eigen::MatrixXd::Ones(xy_points_.size(), 2); + uint32_t idx = 0; + for (const auto &point : xy_points_) + { + Eigen::Vector2d point_in{point.x(), 1}; + A_mat_.row(idx) = point_in; + idx += 1; + } + } + + // Ax=b -> b matrix + void ViewpointSampling::createBMatrix() + { + b_vec_ = Eigen::VectorXd::Zero(xy_points_.size()); + uint32_t idx = 0; + for (const auto &point : xy_points_) + { + b_vec_(idx) = point.y(); + idx += 1; + } + } + + // for getting the cluster centroid + Eigen::Vector3d ViewpointSampling::computeMean() + { + + double sum_x = 0, sum_y = 0, sum_z = 0; + for (const auto &point : cluster_list_->at(cluster_idx_)) + { + sum_x += point.x(); + sum_y += point.y(); + sum_z += point.z(); + } + uint32_t n_points = cluster_list_->at(cluster_idx_).size(); + Eigen::Vector3d point_out{sum_x / n_points, sum_y / n_points, sum_z / n_points}; + return point_out; + } + + // get two points at a perpendicular distance from a line and a point on the line + void ViewpointSampling::computePointsPerpToLine() + { + + opposing_viewpoints_ = Eigen::MatrixXd::Zero(2, 3); + + double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0; + + double x_c = current_cluster_centroid_.x(); // cluster xy coords + double y_c = current_cluster_centroid_.y(); + + double m = fitted_line_(0); // slope of the line + double r = viewing_distance_; // distance from the line + + // ref: http://www.rasmus.is/uk/t/F/Su58k05.htm#:~:text=To%20find%20a%20direction%20vector,%2B%20by%20%2B%20c%20%3D%200. + // ref: https://math.stackexchange.com/questions/2043054/find-a-point-on-a-perpendicular-line-a-given-distance-from-another-point + opposing_viewpoints_(0, 0) = x_c + + std::sqrt(std::pow(r, 2) / (1 + (1 / std::pow(m, 2)))); + + opposing_viewpoints_(1, 0) = x_c - + std::sqrt(std::pow(r, 2) / (1 + (1 / std::pow(m, 2)))); + + opposing_viewpoints_(0, 1) = y_c - + (1 / m) * std::sqrt(std::pow(r, 2) / (1 + (1 / std::pow(m, 2)))); + + opposing_viewpoints_(1, 1) = y_c + + (1 / m) * std::sqrt(std::pow(r, 2) / (1 + (1 / std::pow(m, 2)))); + + // z -> same height as the cluster centroid since the + // view plane is perpendicular to XY plane + opposing_viewpoints_(0, 2) = current_cluster_centroid_.z(); + opposing_viewpoints_(1, 2) = current_cluster_centroid_.z(); + } + + // select one out of the two points on either side of the line + void ViewpointSampling::selectOnePoint() + { + + long int active_vox_count_side1 = 0, active_vox_count_side2 = 0; + + active_vox_count_side1 = VDBUtil::countVoxelsInsideBbox(grid_map_, + opposing_viewpoints_.row(0), + bbox_length_, + bbox_breadth_, + bbox_height_); + + active_vox_count_side2 = VDBUtil::countVoxelsInsideBbox(grid_map_, + opposing_viewpoints_.row(1), + bbox_length_, + bbox_breadth_, + bbox_height_); + + // viewpoint selection is happening using the number of active voxels + // found in a bbox centered around the opposing points + // whichever bbox has a higher count is chosen + if (active_vox_count_side1 >= active_vox_count_side2) + { + selected_viewpoint_ = opposing_viewpoints_.row(0); + } + else + { + selected_viewpoint_ = opposing_viewpoints_.row(1); + } + direction_vector_ = current_cluster_centroid_ - selected_viewpoint_; + } + + // assuming x-forward, z-up convention + float ViewpointSampling::computeYawFromDirVector() + { + return std::atan2(direction_vector_(1), direction_vector_(0)); + } + + pcl::PointXYZHSV ViewpointSampling::constructViewpoint() + { + pcl::PointXYZHSV view_point; + + view_point.x = selected_viewpoint_.x(); + view_point.y = selected_viewpoint_.y(); + view_point.z = selected_viewpoint_.z(); + view_point.h = heading_; + view_point.s = current_cluster_score_; + view_point.v = 0.0; + return view_point; + } + + // line fit using linear least squares + bool ViewpointSampling::fitLineToPoints(int solver_flag) + { + + createAMatrix(); + createBMatrix(); + // ref: https://eigen.tuxfamily.org/dox/group__LeastSquares.html + switch (solver_flag) + { + case 1: + useSVDDecomposition(); + break; + case 2: + useQRDecomposition(); + break; + case 3: + useNormalEquations(); + break; + } + return true; + } + + // SVD is the most accurate but slowest + void ViewpointSampling::useSVDDecomposition() + { + fitted_line_ = Eigen::Vector2d::Zero(); + fitted_line_ = A_mat_.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b_vec_); + } + + // QR decomposition sits between SVD and normal equations + void ViewpointSampling::useQRDecomposition() + { + fitted_line_ = Eigen::Vector2d::Zero(); + fitted_line_ = A_mat_.colPivHouseholderQr().solve(b_vec_); + } + + // solving A^T A x = A^T b (linear least squares using normal equations) + // fastest but also least accurate + void ViewpointSampling::useNormalEquations() + { + // Normal Equations method + fitted_line_ = Eigen::Vector2d::Zero(); + fitted_line_ = (A_mat_.transpose() * A_mat_).ldlt().solve(A_mat_.transpose() * b_vec_); + } + + // create viewpoints on a circle centered around the centroid + void ViewpointSampling::sampleViewpointsAroundCentroid() + { + + /* viewing_distance_inner_r_ = viewing_distance_inner_r; + viewing_distance_outer_r_ = viewing_distance_outer_r; + viewing_distance_z_offset_ */ + + // successively try viewpoints on different horizontal circles until some + // are successfully found + + double angle_spacing = num_viewp_to_sample_ / (2 * M_PI); + int count_viewpoints_added = 0; + + // First, try outer circle with 0 z offset + count_viewpoints_added += sampleViewpointsAroundCentroidParamaterized( + viewing_distance_outer_r_, + 0.0, + angle_spacing); + + if (count_viewpoints_added > 1) + { + // Don't add any more + return; + } + + // Next, try inner circle with 0 z offset + count_viewpoints_added += sampleViewpointsAroundCentroidParamaterized( + viewing_distance_inner_r_, + 0.0, + angle_spacing); + + if (count_viewpoints_added > 0) + { + // Don't add any more + return; + } + + // Try outer circle with +/- z offset + count_viewpoints_added += sampleViewpointsAroundCentroidParamaterized( + viewing_distance_outer_r_, + +viewing_distance_z_offset_, + angle_spacing); + count_viewpoints_added += sampleViewpointsAroundCentroidParamaterized( + viewing_distance_outer_r_, + -viewing_distance_z_offset_, + angle_spacing); + + if (count_viewpoints_added > 0) + { + // Don't add any more + return; + } + + // Try inner circle with +/- z offset + count_viewpoints_added += sampleViewpointsAroundCentroidParamaterized( + viewing_distance_inner_r_, + +viewing_distance_z_offset_, + angle_spacing); + count_viewpoints_added += sampleViewpointsAroundCentroidParamaterized( + viewing_distance_inner_r_, + -viewing_distance_z_offset_, + angle_spacing); + + if (count_viewpoints_added > 0) + { + // Don't add any more + return; + } + } + + // create viewpoints on a circle centered around the centroid + // with preset parameters + int ViewpointSampling::sampleViewpointsAroundCentroidParamaterized( + const double viewing_distance, + const double z_offset, + const double angle_spacing) + { + // return the number of viewpoints added + + int count_viewpoints_added = 0; + double psi = 0.0; + + for (int i = 0; i < num_viewp_to_sample_; ++i) + { + + psi += angle_spacing * i; + + Eigen::Vector3d viewpoint; + viewpoint.x() = current_cluster_centroid_.x() + (viewing_distance * std::cos(psi)); + viewpoint.y() = current_cluster_centroid_.y() + (viewing_distance * std::sin(psi)); + viewpoint.z() = current_cluster_centroid_.z() + z_offset; + + // bool viewpoint_is_in_free_space = VDBUtil::checkPointInFreeSpace(grid_map_, + // viewpoint, bbox_length_, + // bbox_breadth_, bbox_height_, + // l_occ_, true); + + bool viewpoint_is_in_free_space = + VDBUtil::collCheckPointInPartialFreeSpace(grid_map_, + viewpoint, bbox_length_, + bbox_breadth_, bbox_height_, + l_occ_, + viewp_bbox_unknown_frac_thresh_, + viewp_bbox_occ_frac_thresh_); + + openvdb::Vec3d hp; + openvdb::Vec3d viewpoint_vbd(viewpoint.x(), viewpoint.y(), viewpoint.z()); + openvdb::Vec3d centroid_vdb(current_cluster_centroid_.x(), + current_cluster_centroid_.y(), + current_cluster_centroid_.z()); + + bool consider_unknown_occupied = false; + + double ray_coll_check_bbox_length = + bbox_fraction_viewp_centroid_coll_check_ * bbox_length_; + double ray_coll_check_bbox_breadth = + bbox_fraction_viewp_centroid_coll_check_ * bbox_breadth_; + double ray_coll_check_bbox_height = + bbox_fraction_viewp_centroid_coll_check_ * bbox_height_; + + bool viewpoint_to_centroid_is_free_space = !VDBUtil::checkCollisionAlongRay( + grid_map_, + viewpoint_vbd, + centroid_vdb, + ray_coll_check_bbox_length, + ray_coll_check_bbox_breadth, + ray_coll_check_bbox_height, + l_occ_, + hp, + coll_check_endpoint_offset_, + consider_unknown_occupied); + + if (viewpoint_is_in_free_space && viewpoint_to_centroid_is_free_space) + { + selected_viewpoint_ = viewpoint; + direction_vector_ = current_cluster_centroid_ - selected_viewpoint_; + heading_ = computeYawFromDirVector(); + viewpoint_list_->push_back(constructViewpoint()); + count_viewpoints_added++; + } + } + return count_viewpoints_added; + } + +} // namespace viewpoint_sampling_ns \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/waypoint_routing_node.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/waypoint_routing_node.cpp new file mode 100644 index 000000000..c83c39878 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/src/waypoint_routing_node.cpp @@ -0,0 +1,351 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "../include/exploration_node.hpp" +#include "../include/exploration_logic.hpp" + +#include +#include +#include + +#include "utils/utils.hpp" + +class WaypointRoutingNode : public ExplorationNode +{ + using ExplorationNode::ExplorationNode; +public: + void initialize() override; + +private: + std::string sub_target_path_topic_; + std::deque target_path; + + rclcpp::Subscription::SharedPtr sub_target_path; + void targetpathCallback(const geometry_msgs::msg::PoseArray::SharedPtr msg); + + void timerCallback() override; + void generate_plan() override; +}; + +void WaypointRoutingNode::initialize() +{ + this->declare_parameter("sub_target_path_topic"); + if (!this->get_parameter("sub_target_path_topic", this->sub_target_path_topic_)) + { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: sub_target_path_topic"); + return; + } + + // TF buffer and listener + tf_buffer = std::make_shared(this->get_clock()); + tf_listener = std::make_shared(*tf_buffer); + + this->sub_lidar = this->create_subscription( + sub_lidar_topic_, rclcpp::QoS(1).durability_volatile().best_effort(), std::bind(&WaypointRoutingNode::lidarCallback, this, std::placeholders::_1)); + this->sub_target_path = this->create_subscription( + sub_target_path_topic_, rclcpp::QoS(10), std::bind(&WaypointRoutingNode::targetpathCallback, this, std::placeholders::_1)); + + this->pub_global_plan = this->create_publisher(pub_global_plan_topic_, 10); + this->pub_goal_point = + this->create_publisher(pub_goal_point_viz_topic_, 10); + this->pub_trajectory_lines = + this->create_publisher(pub_trajectory_viz_topic_, 10); + this->pub_vdb = + this->create_publisher(pub_grid_viz_topic_, 10); + + // Set up the timer + this->timer = this->create_wall_timer(std::chrono::seconds(5), + std::bind(&WaypointRoutingNode::timerCallback, this)); + // Set up the service + this->srv_exploration_toggle = this->create_service( + srv_exploration_toggle_topic_, std::bind(&WaypointRoutingNode::ExplorationToggleCallback, this, + std::placeholders::_1, std::placeholders::_2)); + RCLCPP_INFO(this->get_logger(), "Exploration node initialized"); +} + +void WaypointRoutingNode::targetpathCallback(const geometry_msgs::msg::PoseArray::SharedPtr msg) +{ + RCLCPP_INFO(this->get_logger(), "Received target path with %zu poses", msg->poses.size()); + this->target_path.assign(msg->poses.begin(), msg->poses.end()); +} + +void WaypointRoutingNode::generate_plan() +{ + RCLCPP_INFO(this->get_logger(), "Starting to generate plan..."); + + // std::tuple start_loc; + // if (this->generated_paths.size() == 0) + // { + // geometry_msgs::msg::Quaternion orientation = this->current_location.rotation; + // tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); + // q.normalize(); + // double roll, pitch, yaw; + // tf2::Matrix3x3 m(q); + // m.getRPY(roll, pitch, yaw); + // start_loc = std::make_tuple(this->current_location.translation.x, + // this->current_location.translation.y, + // this->current_location.translation.z, yaw); + // } + // else + // { + // geometry_msgs::msg::Quaternion orientation = + // this->generated_paths.back().poses.back().pose.orientation; + // tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); + // q.normalize(); + // double roll, pitch, yaw; + // tf2::Matrix3x3 m(q); + // m.getRPY(roll, pitch, yaw); + // start_loc = std::make_tuple(this->generated_paths.back().poses.back().pose.position.x, + // this->generated_paths.back().poses.back().pose.position.y, + // this->generated_paths.back().poses.back().pose.position.z, yaw); + // } + + ViewPoint start_point; + + if (this->generated_paths.empty() || this->generated_paths.back().poses.empty()) + { + // Case 1: no generated path yet → use current_location + geometry_msgs::msg::Quaternion orientation = this->current_location.rotation; + + tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); + q.normalize(); + + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + + start_point.x = this->current_location.translation.x; + start_point.y = this->current_location.translation.y; + start_point.z = this->current_location.translation.z; + + start_point.orientation = orientation; + start_point.orientation_yaw = yaw; + start_point.orientation_set = true; + } + else + { + // Case 2: use last pose of last generated path + const auto &last_pose = this->generated_paths.back().poses.back().pose; + geometry_msgs::msg::Quaternion orientation = last_pose.orientation; + + tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); + q.normalize(); + + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + + start_point.x = last_pose.position.x; + start_point.y = last_pose.position.y; + start_point.z = last_pose.position.z; + + start_point.orientation = orientation; + start_point.orientation_yaw = yaw; + start_point.orientation_set = true; + } + + // convert first goal waypoint to ViewPoint + ViewPoint goal_viewpoint; + + const auto &first_pose = target_path.front(); + + goal_viewpoint.x = first_pose.position.x; + goal_viewpoint.y = first_pose.position.y; + goal_viewpoint.z = first_pose.position.z; + + // Keep quaternion + yaw consistent + tf2::Quaternion q(first_pose.orientation.x, + first_pose.orientation.y, + first_pose.orientation.z, + first_pose.orientation.w); + q.normalize(); + + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + + goal_viewpoint.orientation = first_pose.orientation; + goal_viewpoint.orientation_yaw = yaw; + goal_viewpoint.orientation_set = true; + + float timeout_duration = 5.0; + std::optional gen_path_opt = + // this->exploration_planner->generate_straight_rand_path(start_loc, timeout_duration); + // this->exploration_planner->select_viewpoint_and_plan(start_point, timeout_duration); + this->exploration_planner->plan_to_given_waypoint(start_point, goal_viewpoint); + if (gen_path_opt.has_value() && gen_path_opt.value().size() > 0) + { + RCLCPP_INFO(this->get_logger(), "Generated path with %ld points", + gen_path_opt.value().size()); + + // set the current goal location + this->current_goal_location = geometry_msgs::msg::Transform(); + this->current_goal_location.translation.x = std::get<0>(gen_path_opt.value().back()); + this->current_goal_location.translation.y = std::get<1>(gen_path_opt.value().back()); + this->current_goal_location.translation.z = std::get<2>(gen_path_opt.value().back()); + float z_rot = std::get<3>(gen_path_opt.value().back()); + tf2::Quaternion q; + q.setRPY(0, 0, z_rot); // Roll = 0, Pitch = 0, Yaw = yaw + q.normalize(); + this->current_goal_location.rotation.x = q.x(); + this->current_goal_location.rotation.y = q.y(); + this->current_goal_location.rotation.z = q.z(); + this->current_goal_location.rotation.w = q.w(); + + // publish the path + nav_msgs::msg::Path generated_single_path; + generated_single_path = nav_msgs::msg::Path(); + generated_single_path.header.stamp = this->now(); + generated_single_path.header.frame_id = world_frame_id_; + for (auto point : gen_path_opt.value()) + { + geometry_msgs::msg::PoseStamped point_msg; + point_msg.pose.position.x = std::get<0>(point); + point_msg.pose.position.y = std::get<1>(point); + point_msg.pose.position.z = std::get<2>(point); + // convert yaw rotation to quaternion + tf2::Quaternion q; + q.setRPY(0, 0, std::get<3>(point)); // Roll = 0, Pitch = 0, Yaw = yaw + q.normalize(); + point_msg.pose.orientation.x = q.x(); + point_msg.pose.orientation.y = q.y(); + point_msg.pose.orientation.z = q.z(); + point_msg.pose.orientation.w = q.w(); + point_msg.header.stamp = this->now(); + generated_single_path.poses.push_back(point_msg); + } + geometry_msgs::msg::PoseStamped last_goal_loc = generated_single_path.poses.back(); + this->current_goal_location.translation.x = last_goal_loc.pose.position.x; + this->current_goal_location.translation.y = last_goal_loc.pose.position.y; + this->current_goal_location.translation.z = last_goal_loc.pose.position.z; + this->current_goal_location.rotation.z = last_goal_loc.pose.orientation.z; + this->generated_paths.push_back(generated_single_path); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Failed to generate path, size was 0"); + } +} + +void WaypointRoutingNode::timerCallback() +{ + RCLCPP_INFO(this->get_logger(), "timer callback start"); + // get current TF to world + try + { + geometry_msgs::msg::TransformStamped transform_stamped = + this->tf_buffer->lookupTransform(this->world_frame_id_, this->robot_frame_id_, + rclcpp::Time(0)); + this->current_location = transform_stamped.transform; + if (!this->received_first_robot_tf) + { + this->received_first_robot_tf = true; + this->last_location = this->current_location; + this->last_position_change = this->now(); + RCLCPP_WARN(this->get_logger(), "Received first robot_tf"); + } + } + catch (tf2::TransformException &ex) + { + RCLCPP_ERROR(this->get_logger(), "Robot tf not received: %s", ex.what()); + } + + + // visualize + visualization_msgs::msg::Marker vdb_marker_msg; + generateVDBMarker(this->exploration_planner->occ_grid, map_frame_id_, vdb_marker_msg); + vdb_marker_msg.header.stamp = this->now(); + this->pub_vdb->publish(vdb_marker_msg); + + if (this->enable_exploration) + { + if (!this->is_path_executing) + { + if (!target_path.empty() && this->received_first_robot_tf) + { + + this->generate_plan(); + + this->publish_plan(); + this->is_path_executing = true; + this->last_position_change = this->now(); // Reset stall timer when starting new plan + } + else + { + RCLCPP_WARN(this->get_logger(), "target path size is %zu", target_path.size()); + RCLCPP_WARN(this->get_logger(), "tf received is %d", this->received_first_robot_tf); + } + } + else + { + // check if the robot has reached the goal point + std::tuple current_point = std::make_tuple( + this->current_location.translation.x, this->current_location.translation.y, + this->current_location.translation.z); + std::tuple goal_point = std::make_tuple( + this->current_goal_location.translation.x, this->current_goal_location.translation.y, + this->current_goal_location.translation.z); + if (get_point_distance(current_point, goal_point) < + this->exploration_planner->path_end_threshold_m) + { + this->is_path_executing = false; + this->generated_paths.clear(); + this->target_path.pop_front(); + RCLCPP_INFO(this->get_logger(), "Reached goal point"); + } + else + { + // Check if position has changed significantly + std::tuple last_point = std::make_tuple(this->last_location.translation.x, + this->last_location.translation.y, + this->last_location.translation.z); + + if (get_point_distance(current_point, last_point) > this->position_change_threshold) + { + this->last_location = this->current_location; + this->last_position_change = this->now(); + } + else + { + // Check if we've been stationary for too long + rclcpp::Duration stall_duration = this->now() - this->last_position_change; + if (stall_duration.seconds() > this->stall_timeout_seconds) + { + RCLCPP_INFO(this->get_logger(), "Robot stationary for %f seconds, clearing plan", + stall_duration.seconds()); + this->is_path_executing = false; + this->generated_paths.clear(); + this->target_path.pop_front(); + this->last_position_change = this->now(); // Reset timer to avoid spam + } + } + } + } + } + + RCLCPP_INFO(this->get_logger(), "timer callback end"); +} + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + node->initialize(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file