hongshaorou/YOPO
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial Commit (tested training, testing, and TRT conversion)

Browse Source
This commit is contained in:
Lu Junjie
2024-10-20 17:01:07 +08:00
parent 86d2f311f8
commit 5738088bae
221 changed files with 59249 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

321
flightlib/CMakeLists.txt Normal file
View File

@@ -0,0 +1,321 @@
cmake_minimum_required(VERSION 3.0)
project(flightlib VERSION 0.1.0)
message(STATUS "====================== !Flightmare! ======================")
# ###############################################################################
# Options
# ###############################################################################
option(ENABLE_FAST "Build with optimizations for speed" ON)
option(ENABLE_BLAS "Build using BLAS and LAPACK libraries" OFF)
option(ENABLE_PARALLEL "Build using openmp parallelization" ON)
option(EIGEN_FROM_SYSTTEM "Use the system-provided Eigen" ON)
set(
EIGEN_ALTERNATIVE "" CACHE STRING
"Path to alternative Eigen, autodownload if blank"
)
# ###############################################################################
# Finding Dependencies
# ###############################################################################
# Eigen3
message(STATUS "======> Setup Dependencies")
if(EIGEN_FROM_SYSTTEM)
find_package(Eigen3 3.3.4 QUIET)
if(EIGEN3_FOUND)
message(STATUS "Using system provided Eigen.")
message(${EIGEN3_INCLUDE_DIR})
else()
message(STATUS "No sufficient Eigen version (3.3.4) found.")
message(STATUS "Restoring to download Eigen sources.")
include(cmake/eigen.cmake)
endif()
elseif(EIGEN_ALTERNATIVE STREQUAL "")
include(cmake/eigen.cmake)
else()
set(EIGEN_INCLUDE_DIR ${EIGEN_ALTERNATIVE})
endif()
message(STATUS "Eigen3 include dir: ${EIGEN3_INCLUDE_DIR}")
# PCL (Many VTK errors are reported, but do not affect compilation)
find_package(PCL 1.10 REQUIRED)
message(STATUS "Using system provided PCL.")
message(${PCL_INCLUDE_DIRS})
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
# Including dependencies
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
# Including dependencies
include(cmake/pybind11.cmake)
include(cmake/yaml.cmake)
# Including OpenMP & CUDA
find_package(OpenMP REQUIRED)
find_package(CUDA REQUIRED)
if(ENABLE_BLAS)
set(BLA_VENDOR "Generic")
find_package(BLAS REQUIRED)
if(BLAS_FOUND)
message(STATUS "Found BLAS: ${BLAS_LIBRARIES}")
else()
message(ERROR "Could not enable BLAS because BLAS was not found")
endif()
find_package(LAPACK REQUIRED)
if(LAPACK_FOUND)
message(STATUS "Found Lapack: ${LAPACK_LIBRARIES}")
else()
message(ERROR "Could not enable LAPACK because LAPACK was not found")
endif()
endif()
# Check for ccache
if(NOT DEFINED CATKIN_DEVEL_PREFIX)
find_program(CCACHE_PROGRAM ccache)
if(CCACHE_PROGRAM)
set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "${CCACHE_PROGRAM}")
else()
message(INFO "Build time could be improved with ccache!")
message(INFO "sudo apt install ccache")
endif()
endif()
# ###############################################################################
# Setup Compilation Flag
# ###############################################################################
message(STATUS "======> Setup Compilation ")
add_definitions(-DEIGEN_STACK_ALLOCATION_LIMIT=1048576)
include_directories(${EIGEN_INCLUDE_DIR} "tests")
include_directories(${OpenCV_INCLUDE_DIRS})
# Set default build type
if(NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt)
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE)
endif()
endif()
cuda_select_nvcc_arch_flags(CUDA_ARCH_FLAGS)
message("CUDA Architecture: ${CUDA_ARCH_FLAGS}")
if(${CMAKE_BUILD_TYPE} STREQUAL Release)
SET(CUDA_NVCC_FLAGS "${CUDA_ARCH_FLAGS};-O3;-use_fast_math" CACHE STRING "nvcc flags" FORCE)
message("build CUDA with -O3")
else()
message("build CUDA with Debug")
SET(CUDA_NVCC_FLAGS "-g ;-G ;${CUDA_ARCH_FLAGS}" CACHE STRING "nvcc flags" FORCE)
SET(CUDA_VERBOSE_BUILD ON CACHE BOOL "nvcc verbose" FORCE)
endif()
# Add c++ flags
set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -fPIC -Wall -DNDEBUG -fopenmp")
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -fPIC -Wall -g -fopenmp")
set(CMAKE_CXX_STANDARD 17)
# Architectural flags
if("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "armv7l")
message(STATUS "Using ARMv7 optimized flags!")
set(CMAKE_CXX_ARCH_FLAGS " -Wno-psabi -march=armv7-a -mfpu=neon -mfloat-abi=hard -funsafe-math-optimizations")
elseif("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "aarch64")
message(STATUS "Using ARM aarch64 optimized flags!")
set(CMAKE_CXX_ARCH_FLAGS " -Wno-psabi -march=armv8-a+crypto -mcpu=cortex-a57+crypto")
else()
set(CMAKE_CXX_ARCH_FLAGS " -march=native")
endif()
# Optimized flags
if(ENABLE_FAST)
message(STATUS "Enabling fast optimization flags!")
set(CMAKE_CXX_FAST_FLAGS " -Ofast")
else()
set(CMAKE_CXX_FAST_FLAGS " -O0")
endif()
# BLAS Flags
if(BLAS_FOUND AND LAPACK_FOUND)
message(STATUS "Enabling BLAS and LAPACK")
set(CMAKE_CXX_BLAS_FLAGS " -DEIGEN_USE_BLAS -DEIGEN_USE_LAPACK -DEIGEN_USE_LAPACKE")
else()
set(CMAKE_CXX_BLAS_FLAGS "")
endif()
# Summarize Flags
set(CMAKE_CXX_FLAGS_RELEASE
"${CMAKE_CXX_FLAGS_RELEASE} ${CMAKE_CXX_FAST_FLAGS} ${CMAKE_CXX_ARCH_FLAGS} ${CMAKE_CXX_PAR_FLAGS}")
string(REPLACE "-DNDEBUG" "" CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO}")
message(STATUS "The activated CXX RELEASE configuration is:\n ${CMAKE_CXX_FLAGS_RELEASE}")
message(STATUS "The activated CXX DEBUG configuration is:\n ${CMAKE_CXX_FLAGS_DEBUG}")
# ###############################################################################
# Specify Build Resources
# ###############################################################################
message(STATUS "======> Setup Build ")
add_subdirectory(${PROJECT_SOURCE_DIR}/third_party/arc_utilities)
add_subdirectory(${PROJECT_SOURCE_DIR}/third_party/sdf_tools)
# Create file lists for flightlib source
file(GLOB_RECURSE FLIGHTLIB_SOURCES
src/bridges/*.cpp
src/dynamics/*.cpp
src/objects/*.cpp
src/sensors/*.cpp
src/sensors/*.cu
src/envs/*.cpp
src/common/*.cpp
src/grad_traj_optimization/*.cpp
)
# Create file lists for flightlib_gym source
file(GLOB_RECURSE FLIGHTLIB_GYM_SOURCES
src/wrapper/*.cpp
)
# ###############################################################################
# Optional Catkin Build
# ###############################################################################
if(DEFINED CATKIN_DEVEL_PREFIX)
message(STATUS "======> Building with -- catkin -- ")
include(cmake/catkin.cmake)
return()
endif()
find_package(catkin REQUIRED COMPONENTS
roscpp
sdf_tools
pcl_ros
pcl_conversions
sensor_msgs
)
# ###############################################################################
# Setup Build
# ###############################################################################
# Library and Executables
include_directories(include)
include_directories(${PROJECT_SOURCE_DIR}/third_party/sdf_tools/include)
include_directories(${PROJECT_SOURCE_DIR}/third_party/arc_utilities/include)
# 1. The Training Lib
if(NOT FLIGHTLIB_SOURCES)
set(LIBRARY_NAME)
else()
# flightlib -CUDA
cuda_add_library(${PROJECT_NAME} ${FLIGHTLIB_SOURCES})
target_link_libraries(${PROJECT_NAME} # PRIVATE
${OpenCV_LIBRARIES}
${PCL_LIBRARIES}
${CUDA_curand_LIBRARY}
${CUDA_cublas_LIBRARY}
${CUDA_LIBRARIES}
${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PROJECT_SOURCE_DIR}
yaml-cpp
zmq
zmqpp
stdc++fs)
set(LIBRARY_NAME ${PROJECT_NAME})
set_target_properties(${PROJECT_NAME} PROPERTIES POSITION_INDEPENDENT_CODE TRUE)
endif()
if(FLIGHTLIB_GYM_SOURCES)
# flightlib_gym (python3 binding with Pybind11)
pybind11_add_module(flightgym MODULE
${FLIGHTLIB_GYM_SOURCES})
if(EIGEN3_FOUND)
target_include_directories(flightgym PRIVATE
# ${PROJECT_SOURCE_DIR}/externals/pybind11-src/include
${PYBIND11_INCLUDE_DIR}
${PROJECT_SOURCE_DIR}/include
${EIGEN3_INCLUDE_DIR})
else()
target_include_directories(flightgym PRIVATE
# ${PROJECT_SOURCE_DIR}/externals/pybind11-src/include
${PYBIND11_INCLUDE_DIR}
${PROJECT_SOURCE_DIR}/externals/eigen/eigen3 # pybind11 use #include <Eigen/Core>, however, flightmare use #include <eigen3/Eigen/Core>
${PROJECT_SOURCE_DIR}/include)
endif()
target_link_libraries(flightgym PRIVATE ${LIBRARY_NAME})
endif()
if(ENABLE_BLAS AND BLAS_FOUND AND LAPACK_FOUND)
message(STATUS "Linking standard BLAS ${BLAS_LIBRARIES}")
target_link_libraries(${LIBRARY_NAME}
${BLAS_LIBRARIES}
${LAPACK_LIBRARIES}
${LAPACKE_LIBRARIES}
)
endif()
# Build Other ROS PKG
find_package(catkin REQUIRED COMPONENTS
tf
nav_msgs
cv_bridge
)
include_directories(
${catkin_INCLUDE_DIRS}
${PROJECT_SOURCE_DIR}/include
)
# 2. ROS Simulation Node
add_executable(flightros_node
src/ros_nodes/flight_pilot.cpp
src/ros_nodes/flight_pilot_node.cpp
)
target_link_libraries(flightros_node
${LIBRARY_NAME}
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
zmqpp
stdc++fs
)
# 3. The Planner
add_executable(yopo_planner_node
src/ros_nodes/yopo_planner_node.cpp
)
target_link_libraries(yopo_planner_node
${LIBRARY_NAME}
${catkin_LIBRARIES}
)
# 4. Tool: Trajectory Evaluation
add_executable(traj_eval_node
src/ros_nodes/traj_eval_node.cpp
)
target_link_libraries(traj_eval_node
${LIBRARY_NAME}
${catkin_LIBRARIES}
)
# 5. Tool: Map Visualization
add_executable(map_visual_node
src/ros_nodes/map_visual_node.cpp
)
target_link_libraries(map_visual_node
${LIBRARY_NAME}
${catkin_LIBRARIES}
)
message(STATUS "================ !Done. No more nightmare! ================")

4
flightlib/build/.clang-format Normal file
View File

@@ -0,0 +1,4 @@
---
DisableFormat: true
SortIncludes: false
---

6
flightlib/build/.gitignore vendored Normal file
View File

@@ -0,0 +1,6 @@
# Ignore everything in this directory
*
# Except this file
!.gitignore
!.clang-format
!setup.py

15
flightlib/build/setup.py Normal file
View File

@@ -0,0 +1,15 @@
from setuptools import setup, Extension
#
setup(name='flightgym',
version='0.0.1',
author="Junjie Lu / Yunlong Song",
author_email='lqzx1998@tju.edu.cn / song@ifi.uzh.ch',
description="Flightmare: A Quadrotor Simulator",
long_description='This project is modified based on Flightmare by Yunlong Song, Thanks for his excellent work!',
packages=[''],
package_dir={'': './'},
package_data={'': ['flightgym.cpython-36m-x86_64-linux-gnu.so']},
zip_fase=True,
url=None,
)

29
flightlib/cmake/catkin.cmake Normal file
View File

@@ -0,0 +1,29 @@
# Setup catkin simple
find_package(catkin_simple REQUIRED)
catkin_simple()
add_definitions(-std=c++17)
# Library and Executables
cs_add_library(${PROJECT_NAME} ${FLIGHTLIB_SOURCES})
target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES}
${BLAS_LIBRARIES}
${LAPACK_LIBRARIES}
${LAPACKE_LIBRARIES}
${OpenCV_LIBRARIES}
yaml-cpp
zmq
zmqpp
)
# Build tests
if(BUILD_TESTS)
catkin_add_gtest(flightlib_tests ${FLIGHTLIB_TEST_SOURCES})
target_link_libraries(flightlib_tests ${PROJECT_NAME} gtest gtest_main)
endif()
# Finish catkin simple
cs_install()
cs_export()

25
flightlib/cmake/eigen.cmake Normal file
View File

@@ -0,0 +1,25 @@
# Download and unpack eigen at configure time
message(STATUS "Getting Eigen...")
configure_file(
cmake/eigen_download.cmake
${PROJECT_SOURCE_DIR}/externals/eigen/CMakeLists.txt)
execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .
RESULT_VARIABLE result
WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/eigen
OUTPUT_QUIET)
if(result)
message(FATAL_ERROR "Download of Eigen failed: ${result}")
endif()
execute_process(COMMAND ${CMAKE_COMMAND} --build .
RESULT_VARIABLE result
WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/eigen
OUTPUT_QUIET)
if(result)
message(FATAL_ERROR "Build step for eigen failed: ${result}")
endif()
message(STATUS "Eigen downloaded!")
set(EIGEN_INCLUDE_DIR ${PROJECT_SOURCE_DIR}/externals/eigen)

15
flightlib/cmake/eigen_download.cmake Normal file
View File

@@ -0,0 +1,15 @@
cmake_minimum_required(VERSION 3.0.0)
project(eigen-external)
include(ExternalProject)
ExternalProject_Add(eigen
GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git
GIT_TAG 3.3.4
SOURCE_DIR "${PROJECT_SOURCE_DIR}/externals/eigen/eigen3"
CONFIGURE_COMMAND ""
BUILD_COMMAND ""
INSTALL_COMMAND ""
TEST_COMMAND ""
UPDATE_DISCONNECTED ON
)

37
flightlib/cmake/gtest.cmake Normal file
View File

@@ -0,0 +1,37 @@
# Download and unpack googletest at configure time
message(STATUS "Getting gtests...")
configure_file(cmake/gtest_download.cmake ${PROJECT_SOURCE_DIR}/externals/googletest-download/CMakeLists.txt)
execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .
RESULT_VARIABLE result
WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/googletest-download
OUTPUT_QUIET)
if(result)
message(FATAL_ERROR "CMake step for googletest failed: ${result}")
endif()
execute_process(COMMAND ${CMAKE_COMMAND} --build .
RESULT_VARIABLE result
WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/googletest-download
OUTPUT_QUIET)
if(result)
message(FATAL_ERROR "Build step for googletest failed: ${result}")
endif()
message(STATUS "gtests downloaded!")
# Prevent overriding the parent project's compiler/linker
# settings on Windows
set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
# Add googletest directly to our build. This defines
# the gtest and gtest_main targets.
add_subdirectory(${PROJECT_SOURCE_DIR}/externals/googletest-src
${PROJECT_SOURCE_DIR}/externals/googletest-build
EXCLUDE_FROM_ALL)
# The gtest/gtest_main targets carry header search path
# dependencies automatically when using CMake 2.8.11 or
# later. Otherwise we have to add them here ourselves.
if (CMAKE_VERSION VERSION_LESS 2.8.11)
include_directories("${gtest_SOURCE_DIR}/include")
endif()

16
flightlib/cmake/gtest_download.cmake Normal file
View File

@@ -0,0 +1,16 @@
cmake_minimum_required(VERSION 3.0.0)
project(googletest-download NONE)
include(ExternalProject)
ExternalProject_Add(googletest
GIT_REPOSITORY https://github.com/google/googletest.git
GIT_TAG main
SOURCE_DIR "${PROJECT_SOURCE_DIR}/externals/googletest-src"
BINARY_DIR "${PROJECT_SOURCE_DIR}/externals/googletest-build"
CONFIGURE_COMMAND ""
BUILD_COMMAND ""
INSTALL_COMMAND ""
TEST_COMMAND ""
UPDATE_DISCONNECTED ON
)

33
flightlib/cmake/pybind11.cmake Normal file
View File

@@ -0,0 +1,33 @@
# Download and unpack pybind11 at configure time
message(STATUS "Getting Pybind11...")
# set(PYBIND11_PYTHON_VERSION 3.6)
set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION_STRING})
configure_file(
cmake/pybind11_download.cmake
${PROJECT_SOURCE_DIR}/externals/pybind11-download/CMakeLists.txt)
execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .
RESULT_VARIABLE result
WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/pybind11-download
OUTPUT_QUIET)
if(result)
message(FATAL_ERROR "Cmake Step for Pybind11 failed: ${result}")
endif()
execute_process(COMMAND ${CMAKE_COMMAND} --build .
RESULT_VARIABLE result
WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/pybind11-download
OUTPUT_QUIET)
if(result)
message(FATAL_ERROR "Build step for eigen failed: ${result}")
endif()
message(STATUS "Pybind11 downloaded!")
set(PYBIND11_INCLUDE_DIR ${PROJECT_SOURCE_DIR}/externals/pybind11-src/include)
add_subdirectory(${PROJECT_SOURCE_DIR}/externals/pybind11-src
EXCLUDE_FROM_ALL)
include_directories(SYSTEM "${PROJECT_SOURCE_DIR}/externals/pybind11-src/include")

17
flightlib/cmake/pybind11_download.cmake Normal file
View File

@@ -0,0 +1,17 @@
cmake_minimum_required(VERSION 3.0.0)
project(pybind11-download)
include(ExternalProject)
ExternalProject_Add(pybind11
GIT_REPOSITORY https://cf.ghproxy.cc/https://github.com/pybind/pybind11
GIT_TAG master
SOURCE_DIR "${PROJECT_SOURCE_DIR}/externals/pybind11-src"
BINARY_DIR "${PROJECT_SOURCE_DIR}/externals/pybind11-bin"
CONFIGURE_COMMAND ""
CMAKE_ARGS ""
BUILD_COMMAND ""
INSTALL_COMMAND ""
TEST_COMMAND ""
UPDATE_DISCONNECTED ON
)

33
flightlib/cmake/yaml.cmake Normal file
View File

@@ -0,0 +1,33 @@
# Download and unpack eigen at configure time
message(STATUS "Getting yaml-cpp...")
configure_file(
cmake/yaml_download.cmake
${PROJECT_SOURCE_DIR}/externals/yaml-download/CMakeLists.txt)
execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .
RESULT_VARIABLE result
WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/yaml-download
OUTPUT_QUIET
ERROR_QUIET)
if(result)
message(FATAL_ERROR "CMake step for yaml-cpp failed: ${result}")
endif()
execute_process(COMMAND ${CMAKE_COMMAND} --build .
RESULT_VARIABLE result
WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/yaml-download
OUTPUT_QUIET
ERROR_QUIET)
if(result)
message(FATAL_ERROR "Build step for yaml failed: ${result}")
endif()
message(STATUS "Yaml downloaded!")
add_subdirectory(${PROJECT_SOURCE_DIR}/externals/yaml-src
${PROJECT_SOURCE_DIR}/externals/yaml-build
EXCLUDE_FROM_ALL)
target_compile_options(yaml-cpp PUBLIC -fPIC -w)
include_directories(SYSTEM "${PROJECT_SOURCE_DIR}/externals/yaml-src/include")
link_directories("${PROJECT_SOURCE_DIR}/externals/yaml-build")

18
flightlib/cmake/yaml_download.cmake Normal file
View File

@@ -0,0 +1,18 @@
cmake_minimum_required(VERSION 3.0.0)
project(yaml-download)
include(ExternalProject)
ExternalProject_Add(yaml
GIT_REPOSITORY https://cf.ghproxy.cc/https://github.com/jbeder/yaml-cpp
GIT_TAG master
SOURCE_DIR "${PROJECT_SOURCE_DIR}/externals/yaml-src"
BINARY_DIR "${PROJECT_SOURCE_DIR}/externals/yaml-bin"
CONFIGURE_COMMAND ""
CMAKE_ARGS "-DBUILD_TESTING=OFF -DYAML_CPP_INSTALL=OFF"
CMAKE_CACHE_ARGS -DBUILD_TESTING:BOOL=OFF -DYAML_CPP_INSTALLC:BOOL=ON
BUILD_COMMAND ""
INSTALL_COMMAND ""
TEST_COMMAND ""
UPDATE_DISCONNECTED ON
)

26
flightlib/configs/quadrotor_env.yaml Executable file
View File

@@ -0,0 +1,26 @@
quadrotor_env:
collect_data: yes # yes in Data Collection (random init state); no in Imitation Learning and Testing
bounding_box: [60.0, 60.0, 2.0] # spawn quadrotor within this bounding box
bounding_box_origin: [-10, 20, 2.5]
sim_dt: 0.1 # sim_dt in imitation learning and testing
data_collection:
roll_var: 0.01
pitch_var: 0.01
rgb_camera_left:
on: yes
t_BC: [0.0, 0.0, 0.1] # translational vector of the camera with repect to the body frame
r_BC: [0.0, 0.0, -90] # rotational angle (roll, pitch, yaw) of the camera in degree.
width: 160
height: 90
fov: 90.0 # Horizontal FOV
enable_depth: yes
enable_segmentation: no # not used
enable_opticalflow: no # not used
# Enable Stereo depth when rgb_camera_right on.
rgb_camera_right:
on: no
t_BC: [0.0, -0.2, 0.1] # translational vector of the camera with repect to the body frame
r_BC: [0.0, 0.0, -90] # rotational angle (roll, pitch, yaw) of the camera in degree.

30
flightlib/configs/quadrotor_ros.yaml Normal file
View File

@@ -0,0 +1,30 @@
main_loop_freq: 30
unity_render: yes
scene_id: 4 # 0 wasteland, 1 japanese street, 4 emptyforest in standalone (SR)
odom_topic: /juliett/ground_truth/odom
quad_size: 0.1
ply_path: "/flightrender/RPG_Flightmare/pointcloud_data/"
rgb_camera_left:
on: yes
t_BC: [0.0, 0.0, 0.1] # translational vector of the camera with repect to the body frame
r_BC: [0.0, -5.0, -90] # rotational angle (roll, pitch, yaw) of the camera in degree.
width: 160
height: 90
fov: 90.0 # Horizontal FOV
enable_depth: yes
enable_segmentation: no
enable_opticalflow: no
# enable stereo depth when rgb_camera_right on (If use, please use larger resolution (e.g., 640x360)).
rgb_camera_right:
on: no
t_BC: [0.0, -0.2, 0.1] # translational vector of the camera with repect to the body frame
r_BC: [0.0, -5.0, -90] # rotational angle (roll, pitch, yaw) of the camera in degree.
unity:
spawn_trees: true
avg_tree_spacing: 4.0
bounding_box: [80.0, 80.0, 11.0] # spawn objects within this bounding box
bounding_box_origin: [-10, 20, 2.5]
pointcloud_resolution: 0.2

64
flightlib/configs/traj_opt.yaml Normal file
View File

@@ -0,0 +1,64 @@
vel_max: 6.0
# segment_time = 2 * radio / vel_max
# IMPORTANT PARAM: weight of penalties (6m/s)
ws: 0.00004
wc: 0.001
wl: 0.00
wg: 0.0002
#ws: 0.00004
#wc: 0.001
#wl: 0.02
#wg: 0.0001
# trajectory and primitive parma
horizon_num: 5
vertical_num: 3
horizon_camera_fov: 90.0
vertical_camera_fov: 60.0
horizon_anchor_fov: 30
vertical_anchor_fov: 30
goal_length: 10
radio_range: 4.0 # planning horizon: 2 * radio_range
vel_fov: 90.0 # not use currently
radio_num: 1 # 1 just ok
vel_num: 1 # 1 just ok
vel_prefile: 0.0 # 0 just ok
# For data efficiency, we randomly sample multiple vel and acc for each depth image with the following the distribution.
# values at normalized speed (actual speed can be denormalized by multiplying v_multiple)
# 单位数据倍数： v_multiple = 0.5 * v_max = radio / time
# v数据的均值： v_mean = v_multiple * v_mean_unit
# v数据的方差： v_var = v_multiple^2 * v_var_unit
# a数据的均值： v_mean = v_multiple^2 * a_mean_unit
# a数据的方差： v_var = v_multiple^4 * a_var_unit
vx_mean_unit: 1.5
vy_mean_unit: 0.0
vz_mean_unit: 0.0
vx_var_unit: 0.15
vy_var_unit: 0.45
vz_var_unit: 0.1
ax_mean_unit: 0.0
ay_mean_unit: 0.0
az_mean_unit: 0.0
ax_var_unit: 0.0278
ay_var_unit: 0.05
az_var_unit: 0.0278
# penalties
alpha: 10.0
d0: 1.2
r: 0.6
alphav: 2.0
v0: 3.5
rv: 1.5
alphaa: 2.0
a0: 3.5
ra: 1.5
# deprecated weight
wv: 0.0
wa: 0.0

15
flightlib/configs/vec_env.yaml Normal file
View File

@@ -0,0 +1,15 @@
env:
seed: 1
scene_id: 4 # 0 wasteland, 1 japanese street, 4 emptyforest in SR standalone
num_envs: 16 # Important: same to batch size!
num_threads: 16
dataset_path: "/run/yopo_sim/"
ply_path: "/run/yopo_sim/"
unity:
spawn_trees: true
avg_tree_spacing: 4
# larger than the position to generate the drone to ensure the completeness of the point cloud in edge.
bounding_box: [80.0, 80.0, 11] # spawn objects within this bounding box
bounding_box_origin: [-10, 20, 2.5]
pointcloud_resolution: 0.2

115
flightlib/include/flightlib/bridges/unity_bridge.hpp Normal file
View File

@@ -0,0 +1,115 @@
//
// This bridge was originally from FlightGoggles.
// We made several changes on top of it.
//
#pragma once
// std libs
#include <opencv2/imgproc/types_c.h>
#include <sys/time.h>
#include <unistd.h>
#include <experimental/filesystem>
#include <fstream>
#include <map>
#include <random>
#include <string>
#include <thread>
#include <unordered_map>
// Include ZMQ bindings for communications with Unity.
#include <zmqpp/zmqpp.hpp>
// flightlib
#include "flightlib/bridges/unity_message_types.hpp"
#include "flightlib/common/logger.hpp"
#include "flightlib/common/math.hpp"
#include "flightlib/common/quad_state.hpp"
#include "flightlib/common/types.hpp"
#include "flightlib/objects/quadrotor.hpp"
#include "flightlib/objects/static_object.hpp"
#include "flightlib/objects/unity_camera.hpp"
#include "flightlib/sensors/rgb_camera.hpp"
using json = nlohmann::json;
namespace flightlib {
class UnityBridge {
public:
// constructor & destructor
UnityBridge();
~UnityBridge() {};
// connect function
bool initializeConnections(void);
bool connectUnity(const SceneID scene_id);
bool disconnectUnity(void);
// public get functions
bool getRender(const FrameID frame_id);
bool handleOutput(FrameID &frameID);
void refreshUnity(FrameID id);
void generatePointcloud(const Ref<Vector<3>> &min_corner, const Ref<Vector<3>> &max_corner, int ply_idx, std::string path, SceneID scene_id,
Scalar pc_resolution_ = 0.2);
// public set functions
bool setScene(const SceneID &scene_id);
// add object
bool addQuadrotor(std::shared_ptr<Quadrotor> quad);
bool addCamera(std::shared_ptr<UnityCamera> unity_camera);
bool addStaticObject(std::shared_ptr<StaticObject> static_object);
// public auxiliary functions
inline void setPubPort(const std::string &pub_port) { pub_port_ = pub_port; };
inline void setSubPort(const std::string &sub_port) { sub_port_ = sub_port; };
// create unity bridge
static std::shared_ptr<UnityBridge> getInstance(void) {
static std::shared_ptr<UnityBridge> bridge_ptr = std::make_shared<UnityBridge>();
return bridge_ptr;
};
// add tree
bool placeTrees(TreeMessage_t &tree_msg);
void rmTrees();
void pointCloudGenerator(PointCloudMessage_t &pcd_msg);
bool spawnTrees(Ref<Vector<3>> bounding_box_, Ref<Vector<3>> bounding_box_origin_, Scalar avg_tree_spacing_);
private:
template<typename T>
std::vector<double> convertToDoubleVector(const std::vector<T> &input);
//
SettingsMessage_t settings_;
PubMessage_t pub_msg_;
Logger logger_{"UnityBridge"};
std::vector<std::shared_ptr<Quadrotor>> unity_quadrotors_;
std::vector<std::shared_ptr<RGBCamera>> rgb_cameras_;
std::vector<std::shared_ptr<StaticObject>> static_objects_;
// ZMQ variables and functions
std::string client_address_;
std::string pub_port_;
std::string sub_port_;
zmqpp::context context_;
zmqpp::socket pub_{context_, zmqpp::socket_type::publish};
zmqpp::socket sub_{context_, zmqpp::socket_type::subscribe};
bool sendInitialSettings(void);
bool handleSettings(void);
// timing variables
int64_t num_frames_;
int64_t last_downloaded_utime_;
int64_t last_download_debug_utime_;
int64_t u_packet_latency_;
// axuiliary variables
const Scalar unity_connection_time_out_{60.0};
bool unity_ready_{false};
// Used for trees
std::random_device random_device_;
std::default_random_engine generator_;
};
} // namespace flightlib

309
flightlib/include/flightlib/bridges/unity_message_types.hpp Normal file
View File

@@ -0,0 +1,309 @@
//
// This bridge message types was originally from FlightGoggles.
// We made several changes on top of it.
//
#pragma once
// std
#include <string>
// opencv
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
// flightlib
#include "flightlib/common/types.hpp"
#include "flightlib/json/json.hpp"
using json = nlohmann::json;
namespace flightlib {
enum UnityScene {
WAREHOUSE = 0,
NATUREFOREST = 3,
SCENE_EMPTYFOREST = 4,
SCENE_GRANDCANYON = 5,
SCENE_WASTELAND = 6,
SCENE_JAPANESE = 7,
// total number of environment
SceneNum = 6
};
struct Camera_t {
std::string ID;
// frame Metadata
// int64_t ntime; // deprecated
int channels{3};
int width{1024};
int height{768};
double fov{70.0f};
double depth_scale{0.20}; // 0.xx corresponds to xx cm resolution
// metadata
bool is_depth{false};
int output_index{0};
//
std::vector<std::string> post_processing;
// Transformation matrix from camera to vehicle body 4 x 4
// use 1-D vector for json convention
std::vector<double> T_BC{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
};
struct Lidar_t {
std::string ID;
int num_beams{10};
double max_distance{10.0};
double start_scan_angle{-M_PI / 2};
double end_scan_angle{M_PI / 2};
// Transformation matrix from lidar to vehicle body 4 x 4
// use 1-D vector for json convention
std::vector<double> T_BS{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
};
// Unity Vechicle, e.g., quadrotor
struct Vehicle_t {
std::string ID;
// unity coordinate system left-handed, y up
std::vector<double> position{0.0, 0.0, 0.0};
// unity quaternion (x, y, z, w)
std::vector<double> rotation{0.0, 0.0, 0.0, 1.0};
std::vector<double> size{1.0, 1.0, 1.0}; // scale
// sensors attached on the vehicle
std::vector<Camera_t> cameras;
std::vector<Lidar_t> lidars;
// collision check
bool has_collision_check = true;
};
// Unity object, e.g., gate, light, etc...
struct Object_t {
std::string ID;
std::string prefab_ID;
// unity coordinate system left hand, y up
std::vector<double> position{0.0, 0.0, 0.0};
// unity quaternion (x, y, z, w)
std::vector<double> rotation{0.0, 0.0, 0.0, 1.0};
std::vector<double> size{1.0, 1.0, 1.0}; // scale
};
struct SettingsMessage_t {
// scene/render settings
size_t scene_id = UnityScene::WAREHOUSE;
//
std::vector<Vehicle_t> vehicles;
std::vector<Object_t> objects;
};
struct PubMessage_t {
FrameID ntime{0};
std::vector<Vehicle_t> vehicles;
std::vector<Object_t> objects;
};
//
struct Sub_Vehicle_t {
bool collision;
std::vector<double> lidar_ranges;
};
struct SubMessage_t {
//
FrameID ntime{0};
//
std::vector<Sub_Vehicle_t> sub_vehicles;
};
struct PointCloudMessage_t {
int scene_id{0};
std::vector<double> bounding_box_scale{100.0, 100.0, 100.0};
std::vector<double> bounding_box_origin{0.0, 0.0, 0.0};
double resolution_z{0.1};
double ground_z_limit{0.2};
double resolution_above_ground{0.1};
double resolution_below_ground{0.1};
std::string path{"point_clouds_data/"};
std::string file_name{"default"};
double unity_ground_offset{0.3};
};
struct TreeMessage_t {
std::vector<double> bounding_area{25.0, 25.0};
std::vector<double> bounding_origin{0.0, 0.0};
double density{4.0};
int seed{-1};
};
struct ObjectMessage_t {
std::vector<std::string> name = {"Cube"}; // Cube, Sphere, Cylinder
std::vector<double> bounding_area{25.0, 25.0, 25.0};
std::vector<double> bounding_origin{0.0, 0.0, 0.0};
double density{4.0};
double rand_size{0.0};
std::vector<double> scale_min{
0.1,
0.3,
2.0,
};
std::vector<double> scale_max{1.0, 0.3, 10.0};
std::vector<double> rot_min{0.0, 0.0, 0.0};
std::vector<double> rot_max{360.0, 360.0, 360.0};
int seed{-1};
};
struct FixRatioObjectMessage_t {
std::string name = "Cube"; // Cube, Sphere, Cylinder
std::vector<double> bounding_area{25.0, 25.0};
std::vector<double> bounding_origin{0.0, 0.0};
double density{4.0};
double scale_min{0.1};
double scale_max{2.0};
double rot_min{0.0}; // rotation around z-axix
double rot_max{360.0};
int seed{-1};
};
struct LightMessage_t {
double red{0};
double green{0};
double blue{0};
double intensity{1.0};
};
/*********************
* JSON constructors *
*********************/
// Camera_t
inline void to_json(json &j, const Camera_t &o) {
j = json{{"ID", o.ID},
{"channels", o.channels},
{"width", o.width},
{"height", o.height},
{"fov", o.fov},
{"T_BC", o.T_BC},
{"isDepth", o.is_depth},
{"post_processing", o.post_processing},
{"depthScale", o.depth_scale},
{"outputIndex", o.output_index}};
}
// Lidar
inline void to_json(json &j, const Lidar_t &o) {
j = json{{"ID", o.ID},
{"num_beams", o.num_beams},
{"max_distance", o.max_distance},
{"start_angle", o.start_scan_angle},
{"end_angle", o.end_scan_angle},
{"T_BS", o.T_BS}};
}
// Vehicle_t
inline void to_json(json &j, const Vehicle_t &o) {
j = json{{"ID", o.ID},
{"position", o.position},
{"rotation", o.rotation},
{"size", o.size},
{"cameras", o.cameras},
{"lidars", o.lidars},
{"hasCollisionCheck", o.has_collision_check}};
}
// Object_t
inline void to_json(json &j, const Object_t &o) {
j = json{{"ID", o.ID},
{"prefabID", o.prefab_ID},
{"position", o.position},
{"rotation", o.rotation},
{"size", o.size}};
}
// Setting messages, pub to unity
inline void to_json(json &j, const SettingsMessage_t &o) {
j = json{{"scene_id", o.scene_id},
{"vehicles", o.vehicles},
{"objects", o.objects}};
}
// Publish messages to unity
inline void to_json(json &j, const PubMessage_t &o) {
j = json{
{"ntime", o.ntime}, {"vehicles", o.vehicles}, {"objects", o.objects}};
}
// Publish messages to unity
inline void from_json(const json &j, Sub_Vehicle_t &o) {
o.collision = j.at("collision").get<bool>();
o.lidar_ranges = j.at("lidar_ranges").get<std::vector<double>>();
}
// json to our sub message data type
// note: pub_vechicles is defined in Unity which corresponding
// to our sub_vehicles in ROS.
inline void from_json(const json &j, SubMessage_t &o) {
o.ntime = j.at("ntime").get<uint64_t>();
o.sub_vehicles = j.at("pub_vehicles").get<std::vector<Sub_Vehicle_t>>();
}
inline void to_json(json &j, const PointCloudMessage_t &o) {
j = json{{"scene_id", o.scene_id},
{"bounding_box_scale", o.bounding_box_scale},
{"bounding_box_origin", o.bounding_box_origin},
{"resolution_z", o.resolution_z},
{"ground_z_limit", o.ground_z_limit},
{"resolution_above_ground", o.resolution_above_ground},
{"resolution_below_ground", o.resolution_below_ground},
{"path", o.path},
{"file_name", o.file_name},
{"unity_ground_offset", o.unity_ground_offset}};
}
inline void to_json(json &j, const TreeMessage_t &o) {
j = json{{"bounding_area", o.bounding_area},
{"bounding_origin", o.bounding_origin},
{"density", o.density},
{"seed", o.seed}};
}
inline void to_json(json &j, const ObjectMessage_t &o) {
j = json{{"bounding_area", o.bounding_area},
{"bounding_origin", o.bounding_origin},
{"density", o.density},
{"rand_size", o.rand_size},
{"seed", o.seed},
{"name", o.name},
{"scale_min", o.scale_min},
{"scale_max", o.scale_max},
{"rot_min", o.rot_min},
{"rot_max", o.rot_max}};
}
inline void to_json(json &j, const FixRatioObjectMessage_t &o) {
j = json{{"bounding_area", o.bounding_area},
{"bounding_origin", o.bounding_origin},
{"density", o.density},
{"seed", o.seed},
{"name", o.name},
{"scale_min", o.scale_min},
{"scale_max", o.scale_max},
{"rot_min", o.rot_min},
{"rot_max", o.rot_max}};
}
inline void to_json(json &j, const LightMessage_t &o) {
j = json{{"red", o.red},
{"green", o.green},
{"blue", o.blue},
{"intensity", o.intensity}};
}
// Struct for outputting parsed received messages to handler functions
struct RenderMessage_t {
SubMessage_t sub_msg;
std::vector<cv::Mat> images;
};
} // namespace flightlib

37
flightlib/include/flightlib/common/command.hpp Normal file
View File

@@ -0,0 +1,37 @@
#pragma once
#include <cmath>
#include "flightlib/common/types.hpp"
namespace flightlib {
struct Command {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Command();
Command(const Scalar t, const Scalar thrust, const Vector<3>& omega);
Command(const Scalar t, const Vector<4>& thrusts);
bool valid() const;
bool isSingleRotorThrusts() const;
bool isRatesThrust() const;
/// time in [s]
Scalar t{NAN};
/// Collective mass-normalized thrust in [m/s^2]
Scalar collective_thrust{NAN};
/// Bodyrates in [rad/s]
Vector<3> omega{NAN, NAN, NAN};
/// Single rotor thrusts in [N]
Vector<4> thrusts{NAN, NAN, NAN, NAN};
};
} // namespace flightlib

32
flightlib/include/flightlib/common/integrator_base.hpp Normal file
View File

@@ -0,0 +1,32 @@
#pragma once
#include <functional>
#include "flightlib/common/quad_state.hpp"
#include "flightlib/common/types.hpp"
namespace flightlib {
class IntegratorBase {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
using DynamicsFunction =
std::function<bool(const Ref<const Vector<>>, Ref<Vector<>>)>;
IntegratorBase(DynamicsFunction function, const Scalar dt_max = 1e-3);
bool integrate(const QuadState& initial, QuadState* const final) const;
bool integrate(const Ref<const Vector<>> initial, const Scalar dt,
Ref<Vector<>> final) const;
virtual bool step(const Ref<const Vector<>> initial, const Scalar dt,
Ref<Vector<>> final) const = 0;
Scalar dtMax() const;
protected:
DynamicsFunction dynamics_;
Scalar dt_max_;
};
} // namespace flightlib

17
flightlib/include/flightlib/common/integrator_euler.hpp Normal file
View File

@@ -0,0 +1,17 @@
#pragma once
#include "flightlib/common/integrator_base.hpp"
namespace flightlib {
class IntegratorEuler : public IntegratorBase {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
using IntegratorBase::DynamicsFunction;
using IntegratorBase::IntegratorBase;
bool step(const Ref<const Vector<>> initial, const Scalar dt,
Ref<Vector<>> final) const;
};
} // namespace flightlib

17
flightlib/include/flightlib/common/integrator_rk4.hpp Normal file
View File

@@ -0,0 +1,17 @@
#pragma once
#include "flightlib/common/integrator_base.hpp"
namespace flightlib {
class IntegratorRK4 : public IntegratorBase {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
using IntegratorBase::DynamicsFunction;
using IntegratorBase::IntegratorBase;
bool step(const Ref<const Vector<>> initial, const Scalar dt,
Ref<Vector<>> final) const;
};
} // namespace flightlib

102
flightlib/include/flightlib/common/logger.hpp Normal file
View File

@@ -0,0 +1,102 @@
// """credit: Philipp Foehn """
#pragma once
#include <cstdio>
#include <fstream>
#include <iostream>
#include <string>
namespace flightlib {
class Logger {
public:
Logger(const std::string& name, const bool color = true);
Logger(const std::string& name, const std::string& filename);
~Logger();
inline std::streamsize precision(const std::streamsize n);
inline void scientific(const bool on = true);
template<class... Args>
void info(const std::string& message, const Args&... args) const;
void info(const std::string& message) const;
template<class... Args>
void warn(const std::string& message, const Args&... args) const;
void warn(const std::string& message) const;
template<class... Args>
void error(const std::string& message, const Args&... args) const;
void error(const std::string& message) const;
template<class... Args>
void fatal(const std::string& message, const Args&... args) const;
void fatal(const std::string& message) const;
template<typename T>
std::ostream& operator<<(const T& printable) const;
static constexpr int MAX_CHARS = 256;
private:
static constexpr int DEFAULT_PRECISION = 3;
static constexpr int NAME_PADDING = 15;
static constexpr char RESET[] = "\033[0m";
static constexpr char RED[] = "\033[31m";
static constexpr char YELLOW[] = "\033[33m";
static constexpr char INFO[] = "Info: ";
static constexpr char WARN[] = "Warning: ";
static constexpr char ERROR[] = "Error: ";
static constexpr char FATAL[] = "Fatal: ";
//
std::string name_;
mutable std::ostream sink_;
const bool colored_;
};
template<class... Args>
void Logger::info(const std::string& message, const Args&... args) const {
char buf[MAX_CHARS];
const int n = std::snprintf(buf, MAX_CHARS, message.c_str(), args...);
if (n >= 0 && n < MAX_CHARS)
info(buf);
else
error("=== Logging error ===\n");
}
template<class... Args>
void Logger::warn(const std::string& message, const Args&... args) const {
char buf[MAX_CHARS];
const int n = std::snprintf(buf, MAX_CHARS, message.c_str(), args...);
if (n >= 0 && n < MAX_CHARS)
warn(buf);
else
error("=== Logging error ===\n");
}
template<class... Args>
void Logger::error(const std::string& message, const Args&... args) const {
char buf[MAX_CHARS];
const int n = std::snprintf(buf, MAX_CHARS, message.c_str(), args...);
if (n >= 0 && n < MAX_CHARS)
error(buf);
else
error("=== Logging error ===\n");
}
template<class... Args>
void Logger::fatal(const std::string& message, const Args&... args) const {
char buf[MAX_CHARS];
const int n = std::snprintf(buf, MAX_CHARS, message.c_str(), args...);
if (n >= 0 && n < MAX_CHARS)
fatal(buf);
else
fatal("=== Logging error ===\n");
}
template<typename T>
std::ostream& Logger::operator<<(const T& printable) const {
return sink_ << name_ << printable;
}
} // namespace flightlib

54
flightlib/include/flightlib/common/math.hpp Executable file
View File

@@ -0,0 +1,54 @@
#pragma once
#include "flightlib/common/types.hpp"
namespace flightlib {
Matrix<3, 3> skew(const Vector<3>& v);
Matrix<4, 4> Q_left(const Quaternion& q);
Matrix<4, 4> Q_right(const Quaternion& q);
Matrix<4, 3> qFromQeJacobian(const Quaternion& q);
Matrix<4, 4> qConjugateJacobian();
Matrix<3, 3> qeRotJacobian(const Quaternion& q, const Matrix<3, 1>& t);
Matrix<3, 3> qeInvRotJacobian(const Quaternion& q, const Matrix<3, 1>& t);
void matrixToTripletList(const SparseMatrix& matrix,
std::vector<SparseTriplet>* const list,
const int row_offset = 0, const int col_offset = 0);
void matrixToTripletList(const Matrix<>& matrix,
std::vector<SparseTriplet>* const list,
const int row_offset = 0, const int col_offset = 0);
void insert(const SparseMatrix& from, SparseMatrix* const into,
const int row_offset = 0, const int col_offset = 0);
void insert(const Matrix<>& from, SparseMatrix* const into,
const int row_offset = 0, const int col_offset = 0);
void insert(const Matrix<>& from, Matrix<>* const into,
const int row_offset = 0, const int col_offset = 0);
void quaternionToEuler(const Quaternion& quat, Ref<Vector<3>> euler);
std::vector<Scalar> transformationRos2Unity(const Matrix<4, 4>& ros_tran_mat);
std::vector<Scalar> positionRos2Unity(const Vector<3>& ros_pos_vec);
std::vector<Scalar> quaternionRos2Unity(const Quaternion& ros_quat);
std::vector<Scalar> scalarRos2Unity(const Vector<3>& ros_scale);
void get_euler_from_R(Vector<3> &e, const Matrix<3,3> &R);
float calculate_yaw(float yaw_cur, float yaw_ref, float sim_t);
double wrapMinusPiToPi(const double angle);
double wrapZeroToTwoPi(const double angle);
} // namespace flightlib

21
flightlib/include/flightlib/common/parameter_base.hpp Normal file
View File

@@ -0,0 +1,21 @@
#pragma once
#include <yaml-cpp/yaml.h>
namespace flightlib {
struct ParameterBase {
ParameterBase();
ParameterBase(const std::string& cfg_path);
ParameterBase(const YAML::Node& cfg_node);
virtual ~ParameterBase();
virtual bool valid() = 0;
virtual bool loadParam() = 0;
YAML::Node cfg_node_;
};
} // namespace flightlib

92
flightlib/include/flightlib/common/pend_state.hpp Normal file
View File

@@ -0,0 +1,92 @@
#pragma once
#include <memory>
#include "flightlib/common/types.hpp"
namespace flightlib {
struct PendState {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
enum IDX : int {
POS = 0,
POSX = 0,
POSY = 1,
POSZ = 2,
NPOS = 3,
ATT = 3,
ATTW = 3,
ATTX = 4,
ATTY = 5,
ATTZ = 6,
NATT = 4,
VEL = 7,
VELX = 7,
VELY = 8,
VELZ = 9,
NVEL = 3,
OME = 10,
OMEX = 10,
OMEY = 11,
OMEZ = 12,
NOME = 3,
ACC = 13,
ACCX = 13,
ACCY = 14,
ACCZ = 15,
NACC = 3,
TAU = 16,
TAUX = 16,
TAUY = 17,
TAUZ = 18,
NTAU = 3,
BOME = 19,
BOMEX = 19,
BOMEY = 20,
BOMEZ = 21,
NBOME = 3,
BACC = 22,
BACCX = 22,
BACCY = 23,
BACCZ = 24,
NBACC = 3,
SIZE = 25,
DYN = 19
};
PendState();
PendState(const Vector<IDX::SIZE>& x, const Scalar t = NAN);
PendState(const PendState& state);
~PendState();
inline static int size() { return SIZE; }
Quaternion q() const;
void q(const Quaternion quaternion);
Matrix<3, 3> R() const;
void setZero();
inline bool valid() const { return x.allFinite() && std::isfinite(t); }
Vector<IDX::SIZE> x = Vector<IDX::SIZE>::Constant(NAN);
Scalar t{NAN};
Ref<Vector<3>> p{x.segment<IDX::NPOS>(IDX::POS)};
Ref<Vector<4>> qx{x.segment<IDX::NATT>(IDX::ATT)};
Ref<Vector<3>> v{x.segment<IDX::NVEL>(IDX::VEL)};
Ref<Vector<3>> w{x.segment<IDX::NOME>(IDX::OME)};
Ref<Vector<3>> a{x.segment<IDX::NACC>(IDX::ACC)};
Ref<Vector<3>> tau{x.segment<IDX::NTAU>(IDX::TAU)};
Ref<Vector<3>> bw{x.segment<IDX::NBOME>(IDX::BOME)};
Ref<Vector<3>> ba{x.segment<IDX::NBACC>(IDX::BACC)};
bool operator==(const PendState& rhs) const {
return t == rhs.t && x.isApprox(rhs.x, 1e-5);
}
friend std::ostream& operator<<(std::ostream& os, const PendState& state);
};
using PS = PendState;
} // namespace flightlib

109
flightlib/include/flightlib/common/quad_state.hpp Normal file
View File

@@ -0,0 +1,109 @@
#pragma once
#include <memory>
#include "flightlib/common/types.hpp"
namespace flightlib {
struct QuadState {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
enum IDX : int {
// position
POS = 0,
POSX = 0,
POSY = 1,
POSZ = 2,
NPOS = 3,
// quaternion
ATT = 3,
ATTW = 3,
ATTX = 4,
ATTY = 5,
ATTZ = 6,
NATT = 4,
// linear velocity
VEL = 7,
VELX = 7,
VELY = 8,
VELZ = 9,
NVEL = 3,
// body rate
OME = 10,
OMEX = 10,
OMEY = 11,
OMEZ = 12,
NOME = 3,
// linear acceleration
ACC = 13,
ACCX = 13,
ACCY = 14,
ACCZ = 15,
NACC = 3,
// body-torque
TAU = 16,
TAUX = 16,
TAUY = 17,
TAUZ = 18,
NTAU = 3,
//
BOME = 19,
BOMEX = 19,
BOMEY = 20,
BOMEZ = 21,
NBOME = 3,
//
BACC = 22,
BACCX = 22,
BACCY = 23,
BACCZ = 24,
NBACC = 3,
//
SIZE = 25,
NDYM = 19
};
QuadState();
QuadState(const Vector<IDX::SIZE>& x, const Scalar t = NAN);
QuadState(const QuadState& state);
~QuadState();
inline static int size() { return SIZE; }
Quaternion q() const;
void q(const Quaternion quaternion);
Matrix<3, 3> R() const;
void setZero();
inline bool valid() const { return x.allFinite() && std::isfinite(t); }
Vector<IDX::SIZE> x = Vector<IDX::SIZE>::Constant(NAN);
Scalar t{NAN};
// position
Ref<Vector<3>> p{x.segment<IDX::NPOS>(IDX::POS)};
// orientation (quaternion)
Ref<Vector<4>> qx{x.segment<IDX::NATT>(IDX::ATT)};
// linear velocity
Ref<Vector<3>> v{x.segment<IDX::NVEL>(IDX::VEL)};
// angular velocity
Ref<Vector<3>> w{x.segment<IDX::NOME>(IDX::OME)};
// linear accleration
Ref<Vector<3>> a{x.segment<IDX::NACC>(IDX::ACC)};
// body torque
Ref<Vector<3>> tau{x.segment<IDX::NTAU>(IDX::TAU)};
//
Ref<Vector<3>> bw{x.segment<IDX::NBOME>(IDX::BOME)};
//
Ref<Vector<3>> ba{x.segment<IDX::NBACC>(IDX::BACC)};
bool operator==(const QuadState& rhs) const {
return t == rhs.t && x.isApprox(rhs.x, 1e-5);
}
friend std::ostream& operator<<(std::ostream& os, const QuadState& state);
};
using QS = QuadState;
} // namespace flightlib

87
flightlib/include/flightlib/common/timer.hpp Normal file
View File

@@ -0,0 +1,87 @@
#pragma once
#include <chrono>
#include <iostream>
#include <string>
#include "flightlib/common/types.hpp"
namespace flightlib {
/*
* Timer class to perform runtime analytics.
*
* This timer class provides a simple solution to time code.
* Simply construct a timer and call it's `tic()` and `toc()` functions to time
* code. It is intended to be used to time multiple calls of a function and not
* only reports the `last()` timing, but also statistics such as the `mean()`,
* `min()`, `max()` time, the `count()` of calls to the timer , and even
* standard deviation `std()`.
*
* The constructor can take a name for the timer (like "update") and a name for
* the module (like "Filter").
* After construction it can be `reset()` if needed.
*
* A simple way to get the timing and stats is `std::cout << timer;` which can
* output to arbitrary streams, overloading the stream operator,
* or `print()` which always prints to console.
*
*/
class Timer {
public:
Timer(const std::string name = "", const std::string module = "");
Timer(const Timer& other);
~Timer() {}
/// Start the timer.
void tic();
/// Stops timer, calculates timing, also tics again.
Scalar toc();
/// Reset saved timings and calls;
void reset();
// Accessors
Scalar operator()() const;
Scalar mean() const;
Scalar last() const;
Scalar min() const;
Scalar max() const;
Scalar std() const;
int count() const;
/// Custom stream operator for outputs.
friend std::ostream& operator<<(std::ostream& os, const Timer& timer);
/// Print timing information to console.
void print() const;
private:
std::string name_, module_;
using TimePoint = std::chrono::high_resolution_clock::time_point;
TimePoint t_start_;
// Initialize timing to impossible values.
Scalar timing_mean_;
Scalar timing_last_;
Scalar timing_S_;
Scalar timing_min_;
Scalar timing_max_;
int n_samples_;
};
/*
* Simple class to time scopes.
*
* This effectively instantiates a timer and calls `tic()` in its constructor
* and `toc()` and ` print()` in its destructor.
*/
class ScopedTimer : public Timer {
public:
ScopedTimer(const std::string name = "", const std::string module = "");
~ScopedTimer();
};
} // namespace flightlib

101
flightlib/include/flightlib/common/types.hpp Normal file
View File

@@ -0,0 +1,101 @@
#pragma once
#include <eigen3/Eigen/Eigen>
namespace flightlib {
// ------------ General Stuff-------------
// Define the scalar type used.
using Scalar = float; // numpy float32
// Define frame id for unity
using FrameID = uint64_t;
using USecs = uint64_t;
// Define frame id for unity
using SceneID = size_t;
// ------------ Eigen Stuff-------------
// Define `Dynamic` matrix size.
static constexpr int Dynamic = Eigen::Dynamic;
// Using shorthand for 'DepthImg<ros, cols>' with scalar type.
template<int rows = Dynamic, int cols = Dynamic>
using DepthImgMatrix = Eigen::Matrix<float_t, rows, cols>;
// Using shorthand for `DepthImgVector<ros>` with scalar type.
template<int rows = Dynamic>
using DepthImgVector = DepthImgMatrix<rows, 1>;
// Using shorthand for 'DepthImgRowMajor<ros, cols>' with scalar type.
template<int rows = Dynamic, int cols = Dynamic>
using DepthImgMatrixRowMajor =
Eigen::Matrix<float_t, rows, cols, Eigen::RowMajor>;
// Using shorthand for 'ImgMatrix<ros, cols>' with scalar type.
template<int rows = Dynamic, int cols = Dynamic>
using ImgMatrix = Eigen::Matrix<uint8_t, rows, cols>;
// Using shorthand for `ImgVector<ros>` with scalar type.
template<int rows = Dynamic>
using ImgVector = ImgMatrix<rows, 1>;
// Using shorthand for 'ImgMatrixRowMajor<ros, cols>' with scalar type.
template<int rows = Dynamic, int cols = Dynamic>
using ImgMatrixRowMajor = Eigen::Matrix<uint8_t, rows, cols, Eigen::RowMajor>;
// Using shorthand for `Matrix<rows, cols>` with scalar type.
template<int rows = Dynamic, int cols = Dynamic>
using Matrix = Eigen::Matrix<Scalar, rows, cols>;
// Using shorthand for `Matrix<rows, cols>` with scalar type.
template<int rows = Dynamic, int cols = Dynamic>
using MatrixRowMajor = Eigen::Matrix<Scalar, rows, cols, Eigen::RowMajor>;
// Using shorthand for `Vector<rows>` with scalar type.
template<int rows = Dynamic>
using Vector = Matrix<rows, 1>;
// Vector bool
template<int rows = Dynamic>
using BoolVector = Eigen::Matrix<bool, -1, 1>;
// Vector int
template<int rows = Dynamic>
using IntVector = Eigen::Matrix<int, -1, 1>;
// Using shorthand for `Array<rows, cols>` with scalar type.
template<int rows = Dynamic, int cols = rows>
using Array = Eigen::Array<Scalar, rows, cols>;
// Using `SparseMatrix` with type.
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
// Using SparseTriplet with type.
using SparseTriplet = Eigen::Triplet<Scalar>;
// Using `Quaternion` with type.
using Quaternion = Eigen::Quaternion<Scalar>;
// Using 'AngleAxis' with type
using AngleAxis = Eigen::AngleAxis<Scalar>;
// Using `Ref` for modifier references.
template<class Derived>
using Ref = Eigen::Ref<Derived>;
// // Using `ConstRef` for constant references.
template<class Derived>
using ConstRef = const Eigen::Ref<const Derived>;
// Using `Map`.
template<class Derived>
using Map = Eigen::Map<Derived>;
static constexpr Scalar Gz = -9.81;
const Vector<3> GVEC{0.0, 0.0, Gz};
} // namespace flightlib

400
flightlib/include/flightlib/controller/PositionCommand.h Normal file
View File

@@ -0,0 +1,400 @@
// Generated by gencpp from file quadrotor_msgs/PositionCommand.msg
// DO NOT EDIT!
#ifndef QUADROTOR_MSGS_MESSAGE_POSITIONCOMMAND_H
#define QUADROTOR_MSGS_MESSAGE_POSITIONCOMMAND_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
#include <std_msgs/Header.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Vector3.h>
namespace quadrotor_msgs
{
template <class ContainerAllocator>
struct PositionCommand_
{
typedef PositionCommand_<ContainerAllocator> Type;
PositionCommand_()
: header()
, position()
, velocity()
, acceleration()
, yaw(0.0)
, yaw_dot(0.0)
, kx()
, kv()
, trajectory_id(0)
, trajectory_flag(0) {
kx.assign(0.0);
kv.assign(0.0);
}
PositionCommand_(const ContainerAllocator& _alloc)
: header(_alloc)
, position(_alloc)
, velocity(_alloc)
, acceleration(_alloc)
, yaw(0.0)
, yaw_dot(0.0)
, kx()
, kv()
, trajectory_id(0)
, trajectory_flag(0) {
(void)_alloc;
kx.assign(0.0);
kv.assign(0.0);
}
typedef ::std_msgs::Header_<ContainerAllocator> _header_type;
_header_type header;
typedef ::geometry_msgs::Point_<ContainerAllocator> _position_type;
_position_type position;
typedef ::geometry_msgs::Vector3_<ContainerAllocator> _velocity_type;
_velocity_type velocity;
typedef ::geometry_msgs::Vector3_<ContainerAllocator> _acceleration_type;
_acceleration_type acceleration;
typedef double _yaw_type;
_yaw_type yaw;
typedef double _yaw_dot_type;
_yaw_dot_type yaw_dot;
typedef boost::array<double, 3> _kx_type;
_kx_type kx;
typedef boost::array<double, 3> _kv_type;
_kv_type kv;
typedef uint32_t _trajectory_id_type;
_trajectory_id_type trajectory_id;
typedef uint8_t _trajectory_flag_type;
_trajectory_flag_type trajectory_flag;
// reducing the odds to have name collisions with Windows.h
#if defined(_WIN32) && defined(TRAJECTORY_STATUS_EMPTY)
#undef TRAJECTORY_STATUS_EMPTY
#endif
#if defined(_WIN32) && defined(TRAJECTORY_STATUS_READY)
#undef TRAJECTORY_STATUS_READY
#endif
#if defined(_WIN32) && defined(TRAJECTORY_STATUS_COMPLETED)
#undef TRAJECTORY_STATUS_COMPLETED
#endif
#if defined(_WIN32) && defined(TRAJECTROY_STATUS_ABORT)
#undef TRAJECTROY_STATUS_ABORT
#endif
#if defined(_WIN32) && defined(TRAJECTORY_STATUS_ILLEGAL_START)
#undef TRAJECTORY_STATUS_ILLEGAL_START
#endif
#if defined(_WIN32) && defined(TRAJECTORY_STATUS_ILLEGAL_FINAL)
#undef TRAJECTORY_STATUS_ILLEGAL_FINAL
#endif
#if defined(_WIN32) && defined(TRAJECTORY_STATUS_IMPOSSIBLE)
#undef TRAJECTORY_STATUS_IMPOSSIBLE
#endif
enum {
TRAJECTORY_STATUS_EMPTY = 0u,
TRAJECTORY_STATUS_READY = 1u,
TRAJECTORY_STATUS_COMPLETED = 3u,
TRAJECTROY_STATUS_ABORT = 4u,
TRAJECTORY_STATUS_ILLEGAL_START = 5u,
TRAJECTORY_STATUS_ILLEGAL_FINAL = 6u,
TRAJECTORY_STATUS_IMPOSSIBLE = 7u,
};
typedef boost::shared_ptr< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> const> ConstPtr;
}; // struct PositionCommand_
typedef ::quadrotor_msgs::PositionCommand_<std::allocator<void> > PositionCommand;
typedef boost::shared_ptr< ::quadrotor_msgs::PositionCommand > PositionCommandPtr;
typedef boost::shared_ptr< ::quadrotor_msgs::PositionCommand const> PositionCommandConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::quadrotor_msgs::PositionCommand_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::quadrotor_msgs::PositionCommand_<ContainerAllocator1> & lhs, const ::quadrotor_msgs::PositionCommand_<ContainerAllocator2> & rhs)
{
return lhs.header == rhs.header &&
lhs.position == rhs.position &&
lhs.velocity == rhs.velocity &&
lhs.acceleration == rhs.acceleration &&
lhs.yaw == rhs.yaw &&
lhs.yaw_dot == rhs.yaw_dot &&
lhs.kx == rhs.kx &&
lhs.kv == rhs.kv &&
lhs.trajectory_id == rhs.trajectory_id &&
lhs.trajectory_flag == rhs.trajectory_flag;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::quadrotor_msgs::PositionCommand_<ContainerAllocator1> & lhs, const ::quadrotor_msgs::PositionCommand_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace quadrotor_msgs
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsMessage< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> const>
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> const>
: TrueType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >
{
static const char* value()
{
return "4712f0609ca29a79af79a35ca3e3967a";
}
static const char* value(const ::quadrotor_msgs::PositionCommand_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0x4712f0609ca29a79ULL;
static const uint64_t static_value2 = 0xaf79a35ca3e3967aULL;
};
template<class ContainerAllocator>
struct DataType< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >
{
static const char* value()
{
return "quadrotor_msgs/PositionCommand";
}
static const char* value(const ::quadrotor_msgs::PositionCommand_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >
{
static const char* value()
{
return "Header header\n"
"geometry_msgs/Point position\n"
"geometry_msgs/Vector3 velocity\n"
"geometry_msgs/Vector3 acceleration\n"
"float64 yaw\n"
"float64 yaw_dot\n"
"float64[3] kx\n"
"float64[3] kv \n"
"\n"
"uint32 trajectory_id\n"
"\n"
"uint8 TRAJECTORY_STATUS_EMPTY = 0\n"
"uint8 TRAJECTORY_STATUS_READY = 1\n"
"uint8 TRAJECTORY_STATUS_COMPLETED = 3\n"
"uint8 TRAJECTROY_STATUS_ABORT = 4\n"
"uint8 TRAJECTORY_STATUS_ILLEGAL_START = 5\n"
"uint8 TRAJECTORY_STATUS_ILLEGAL_FINAL = 6\n"
"uint8 TRAJECTORY_STATUS_IMPOSSIBLE = 7\n"
"\n"
"# Its ID number will start from 1, allowing you comparing it with 0.\n"
"uint8 trajectory_flag\n"
"\n"
"================================================================================\n"
"MSG: std_msgs/Header\n"
"# Standard metadata for higher-level stamped data types.\n"
"# This is generally used to communicate timestamped data \n"
"# in a particular coordinate frame.\n"
"# \n"
"# sequence ID: consecutively increasing ID \n"
"uint32 seq\n"
"#Two-integer timestamp that is expressed as:\n"
"# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n"
"# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n"
"# time-handling sugar is provided by the client library\n"
"time stamp\n"
"#Frame this data is associated with\n"
"string frame_id\n"
"\n"
"================================================================================\n"
"MSG: geometry_msgs/Point\n"
"# This contains the position of a point in free space\n"
"float64 x\n"
"float64 y\n"
"float64 z\n"
"\n"
"================================================================================\n"
"MSG: geometry_msgs/Vector3\n"
"# This represents a vector in free space. \n"
"# It is only meant to represent a direction. Therefore, it does not\n"
"# make sense to apply a translation to it (e.g., when applying a \n"
"# generic rigid transformation to a Vector3, tf2 will only apply the\n"
"# rotation). If you want your data to be translatable too, use the\n"
"# geometry_msgs/Point message instead.\n"
"\n"
"float64 x\n"
"float64 y\n"
"float64 z\n"
;
}
static const char* value(const ::quadrotor_msgs::PositionCommand_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.header);
stream.next(m.position);
stream.next(m.velocity);
stream.next(m.acceleration);
stream.next(m.yaw);
stream.next(m.yaw_dot);
stream.next(m.kx);
stream.next(m.kv);
stream.next(m.trajectory_id);
stream.next(m.trajectory_flag);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct PositionCommand_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::quadrotor_msgs::PositionCommand_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::quadrotor_msgs::PositionCommand_<ContainerAllocator>& v)
{
s << indent << "header: ";
s << std::endl;
Printer< ::std_msgs::Header_<ContainerAllocator> >::stream(s, indent + " ", v.header);
s << indent << "position: ";
s << std::endl;
Printer< ::geometry_msgs::Point_<ContainerAllocator> >::stream(s, indent + " ", v.position);
s << indent << "velocity: ";
s << std::endl;
Printer< ::geometry_msgs::Vector3_<ContainerAllocator> >::stream(s, indent + " ", v.velocity);
s << indent << "acceleration: ";
s << std::endl;
Printer< ::geometry_msgs::Vector3_<ContainerAllocator> >::stream(s, indent + " ", v.acceleration);
s << indent << "yaw: ";
Printer<double>::stream(s, indent + " ", v.yaw);
s << indent << "yaw_dot: ";
Printer<double>::stream(s, indent + " ", v.yaw_dot);
s << indent << "kx[]" << std::endl;
for (size_t i = 0; i < v.kx.size(); ++i)
{
s << indent << " kx[" << i << "]: ";
Printer<double>::stream(s, indent + " ", v.kx[i]);
}
s << indent << "kv[]" << std::endl;
for (size_t i = 0; i < v.kv.size(); ++i)
{
s << indent << " kv[" << i << "]: ";
Printer<double>::stream(s, indent + " ", v.kv[i]);
}
s << indent << "trajectory_id: ";
Printer<uint32_t>::stream(s, indent + " ", v.trajectory_id);
s << indent << "trajectory_flag: ";
Printer<uint8_t>::stream(s, indent + " ", v.trajectory_flag);
}
};
} // namespace message_operations
} // namespace ros
#endif // QUADROTOR_MSGS_MESSAGE_POSITIONCOMMAND_H

290
flightlib/include/flightlib/controller/ctrl_ref.h Executable file
View File

@@ -0,0 +1,290 @@
// Generated by gencpp from file quad_pos_ctrl/ctrl_ref.msg
// DO NOT EDIT!
#ifndef QUAD_POS_CTRL_MESSAGE_CTRL_REF_H
#define QUAD_POS_CTRL_MESSAGE_CTRL_REF_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
#include <std_msgs/Header.h>
namespace quad_pos_ctrl
{
template <class ContainerAllocator>
struct ctrl_ref_
{
typedef ctrl_ref_<ContainerAllocator> Type;
ctrl_ref_()
: header()
, pos_ref()
, vel_ref()
, acc_ref()
, yaw_ref(0.0)
, ref_mask(0) {
pos_ref.assign(0.0);
vel_ref.assign(0.0);
acc_ref.assign(0.0);
}
ctrl_ref_(const ContainerAllocator& _alloc)
: header(_alloc)
, pos_ref()
, vel_ref()
, acc_ref()
, yaw_ref(0.0)
, ref_mask(0) {
(void)_alloc;
pos_ref.assign(0.0);
vel_ref.assign(0.0);
acc_ref.assign(0.0);
}
typedef ::std_msgs::Header_<ContainerAllocator> _header_type;
_header_type header;
typedef boost::array<float, 3> _pos_ref_type;
_pos_ref_type pos_ref;
typedef boost::array<float, 3> _vel_ref_type;
_vel_ref_type vel_ref;
typedef boost::array<float, 3> _acc_ref_type;
_acc_ref_type acc_ref;
typedef float _yaw_ref_type;
_yaw_ref_type yaw_ref;
typedef uint8_t _ref_mask_type;
_ref_mask_type ref_mask;
enum {
POS_CTRL_VALIED = 1u,
VEL_CTRL_VALIED = 2u,
ACC_CTRL_VALIED = 4u,
};
typedef boost::shared_ptr< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> const> ConstPtr;
}; // struct ctrl_ref_
typedef ::quad_pos_ctrl::ctrl_ref_<std::allocator<void> > ctrl_ref;
typedef boost::shared_ptr< ::quad_pos_ctrl::ctrl_ref > ctrl_refPtr;
typedef boost::shared_ptr< ::quad_pos_ctrl::ctrl_ref const> ctrl_refConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >::stream(s, "", v);
return s;
}
} // namespace quad_pos_ctrl
namespace ros
{
namespace message_traits
{
// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True}
// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'quad_pos_ctrl': ['/home/yiqianlingqi/Work/pos_px4lidar_ctrl_ws/src/quad_pos_ctrl/msg']}
// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
template <class ContainerAllocator>
struct IsFixedSize< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> const>
: FalseType
{ };
template <class ContainerAllocator>
struct IsMessage< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> const>
: TrueType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >
{
static const char* value()
{
return "c44a0c7d669f499d943b0196aea84d57";
}
static const char* value(const ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0xc44a0c7d669f499dULL;
static const uint64_t static_value2 = 0x943b0196aea84d57ULL;
};
template<class ContainerAllocator>
struct DataType< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >
{
static const char* value()
{
return "quad_pos_ctrl/ctrl_ref";
}
static const char* value(const ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >
{
static const char* value()
{
return "# enum mask\n\
uint8 POS_CTRL_VALIED = 1\n\
uint8 VEL_CTRL_VALIED = 2\n\
uint8 ACC_CTRL_VALIED = 4\n\
\n\
# \n\
Header header\n\
float32[3] pos_ref\n\
float32[3] vel_ref\n\
float32[3] acc_ref\n\
float32 yaw_ref\n\
uint8 ref_mask\n\
================================================================================\n\
MSG: std_msgs/Header\n\
# Standard metadata for higher-level stamped data types.\n\
# This is generally used to communicate timestamped data \n\
# in a particular coordinate frame.\n\
# \n\
# sequence ID: consecutively increasing ID \n\
uint32 seq\n\
#Two-integer timestamp that is expressed as:\n\
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\
# time-handling sugar is provided by the client library\n\
time stamp\n\
#Frame this data is associated with\n\
# 0: no frame\n\
# 1: global frame\n\
string frame_id\n\
";
}
static const char* value(const ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.header);
stream.next(m.pos_ref);
stream.next(m.vel_ref);
stream.next(m.acc_ref);
stream.next(m.yaw_ref);
stream.next(m.ref_mask);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct ctrl_ref_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::quad_pos_ctrl::ctrl_ref_<ContainerAllocator>& v)
{
s << indent << "header: ";
s << std::endl;
Printer< ::std_msgs::Header_<ContainerAllocator> >::stream(s, indent + " ", v.header);
s << indent << "pos_ref[]" << std::endl;
for (size_t i = 0; i < v.pos_ref.size(); ++i)
{
s << indent << " pos_ref[" << i << "]: ";
Printer<float>::stream(s, indent + " ", v.pos_ref[i]);
}
s << indent << "vel_ref[]" << std::endl;
for (size_t i = 0; i < v.vel_ref.size(); ++i)
{
s << indent << " vel_ref[" << i << "]: ";
Printer<float>::stream(s, indent + " ", v.vel_ref[i]);
}
s << indent << "acc_ref[]" << std::endl;
for (size_t i = 0; i < v.acc_ref.size(); ++i)
{
s << indent << " acc_ref[" << i << "]: ";
Printer<float>::stream(s, indent + " ", v.acc_ref[i]);
}
s << indent << "yaw_ref: ";
Printer<float>::stream(s, indent + " ", v.yaw_ref);
s << indent << "ref_mask: ";
Printer<uint8_t>::stream(s, indent + " ", v.ref_mask);
}
};
} // namespace message_operations
} // namespace ros
#endif // QUAD_POS_CTRL_MESSAGE_CTRL_REF_H

21
flightlib/include/flightlib/dynamics/dynamics_base.hpp Normal file
View File

@@ -0,0 +1,21 @@
#pragma once
#include "flightlib/common/types.hpp"
namespace flightlib {
class DynamicsBase {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
using DynamicsFunction = std::function<bool(const Ref<const Vector<>>, Ref<Vector<>>)>;
DynamicsBase();
virtual ~DynamicsBase();
// public get function
virtual DynamicsFunction getDynamicsFunction() const = 0;
private:
};
} // namespace flightlib

87
flightlib/include/flightlib/dynamics/quadrotor_dynamics.hpp Normal file
View File

@@ -0,0 +1,87 @@
#pragma once
#include <yaml-cpp/yaml.h>
#include <iostream>
#include <memory>
#include "flightlib/common/logger.hpp"
#include "flightlib/common/math.hpp"
#include "flightlib/common/quad_state.hpp"
#include "flightlib/dynamics/dynamics_base.hpp"
namespace flightlib {
class QuadrotorDynamics : DynamicsBase {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
QuadrotorDynamics(const Scalar mass = 1.0, const Scalar arm_l = 0.2);
~QuadrotorDynamics();
// dynamics function
bool dState(const QuadState& state, QuadState* derivative) const;
bool dState(const Ref<const Vector<QuadState::SIZE>> state, Ref<Vector<QuadState::SIZE>> derivative) const;
// public get function
DynamicsFunction getDynamicsFunction() const;
// help functions
bool valid() const;
bool updateParams(const YAML::Node& params);
// Helpers to apply limits.
Vector<4> clampThrust(const Vector<4> thrusts) const;
Scalar clampThrust(const Scalar thrust) const;
Scalar clampCollectiveThrust(const Scalar thrust) const;
Vector<4> clampMotorOmega(const Vector<4>& omega) const;
Vector<3> clampBodyrates(const Vector<3>& omega) const;
inline Scalar collective_thrust_min() const { return 4.0 * thrust_min_; }
inline Scalar collective_thrust_max() const { return 4.0 * thrust_max_; }
// Helpers for conversion
Vector<4> motorOmegaToThrust(const Vector<4>& omega) const;
Vector<4> motorThrustToOmega(const Vector<4>& thrusts) const;
Matrix<4, 4> getAllocationMatrix() const;
//
inline Scalar getMass(void) const { return mass_; };
inline Scalar getArmLength(void) const { return arm_l_; };
inline Scalar getMotorTauInv() const { return motor_tau_inv_; };
inline Matrix<3, 3> getJ(void) const { return J_; };
inline Matrix<3, 3> getJInv(void) const { return J_inv_; };
inline Vector<3> getOmegaMax(void) const { return omega_max_; };
bool setMass(const Scalar mass);
bool setArmLength(const Scalar arm_length);
bool setMotortauInv(const Scalar tau_inv);
friend std::ostream& operator<<(std::ostream& os, const QuadrotorDynamics& quad_dymaics);
private:
bool updateInertiaMarix();
Scalar mass_;
Scalar arm_l_;
Matrix<3, 4> t_BM_;
Matrix<3, 3> J_;
Matrix<3, 3> J_inv_;
// motors
Scalar motor_omega_min_;
Scalar motor_omega_max_;
Scalar motor_tau_inv_;
// Propellers
Vector<3> thrust_map_;
Scalar kappa_;
Scalar thrust_min_;
Scalar thrust_max_;
Scalar collective_thrust_min_;
Scalar collective_thrust_max_;
// Quadrotor limits
Vector<3> omega_max_;
};
} // namespace flightlib

70
flightlib/include/flightlib/envs/env_base.hpp Normal file
View File

@@ -0,0 +1,70 @@
//
// This is inspired by RaiGym, thanks.
//
#pragma once
#include <unistd.h>
#include <yaml-cpp/yaml.h>
#include <memory>
#include <random>
#include <unordered_map>
#include <vector>
#include "flightlib/common/types.hpp"
namespace flightlib {
class EnvBase {
public:
EnvBase() : obs_dim_(0), act_dim_(0), rew_dim_(0), sim_dt_(0.0), img_width_(0), img_height_(0) {};
virtual ~EnvBase() {};
// (pure virtual) public methods (has to be implemented by child classes)
virtual bool reset(Ref<Vector<>> obs, const bool random = true) = 0;
virtual bool step(const Ref<Vector<>> act, Ref<Vector<>> obs, Ref<Vector<>> reward) = 0;
virtual bool getObs(Ref<Vector<>> obs) = 0;
// (virtual) public methods (implementations are optional.)
virtual bool getDepthImage(Ref<DepthImgVector<>> img) { return false; }
virtual void close() {};
// virtual void render();
virtual void updateExtraInfo() {};
virtual bool isTerminalState(Scalar &reward) {
reward = 0.f;
return false;
}
// auxilirary functions
inline void setSeed(const int seed) { std::srand(seed); };
inline int getObsDim() { return obs_dim_; };
inline int getActDim() { return act_dim_; };
inline int getRewDim() { return rew_dim_; };
inline int getImgWidth() { return img_width_; };
inline int getImgHeight() { return img_height_; };
inline Scalar getSimTimeStep() { return sim_dt_; };
inline int getExtraInfoDim() { return extra_info_.size(); };
// public variables
std::unordered_map<std::string, float> extra_info_;
protected:
// observation and action dimenstions (for Reinforcement learning)
int obs_dim_;
int act_dim_;
int rew_dim_;
int img_width_;
int img_height_;
// control time step
Scalar sim_dt_{0.02};
// random variable generator
std::normal_distribution<Scalar> norm_dist_{0.0, 1.0};
std::uniform_real_distribution<Scalar> uniform_dist_{-1.0, 1.0};
std::random_device rd_;
std::mt19937 random_gen_{rd_()};
};
} // namespace flightlib

119
flightlib/include/flightlib/envs/quadrotor_env.hpp Executable file
View File

@@ -0,0 +1,119 @@
#pragma once
// std lib
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>
#include <stdlib.h>
#include <time.h>
#include <unistd.h>
#include <yaml-cpp/yaml.h>
#include <cmath>
#include <iostream>
// flightlib
#include "flightlib/bridges/unity_bridge.hpp"
#include "flightlib/common/command.hpp"
#include "flightlib/common/logger.hpp"
#include "flightlib/common/math.hpp"
#include "flightlib/common/quad_state.hpp"
#include "flightlib/common/types.hpp"
#include "flightlib/envs/env_base.hpp"
#include "flightlib/grad_traj_optimization/traj_optimization_bridge.h"
#include "flightlib/objects/quadrotor.hpp"
#include "flightlib/sensors/sgm_gpu/sgm_gpu.h"
namespace flightlib {
enum Ctl : int {
kNObs = 13, // observation dim
kNAct = 9, // action dim
};
class QuadrotorEnv final : public EnvBase {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
QuadrotorEnv();
QuadrotorEnv(const std::string &cfg_path);
~QuadrotorEnv();
/* set functions */
bool reset(Ref<Vector<>> obs, const bool random = true) override;
void setState(ConstRef<Vector<>> state);
void setGoal(ConstRef<Vector<>> goal);
void setMapID(int id);
void setDAggerMode(bool dagger_mode) { dagger_mode_ = dagger_mode; }
bool step(const Ref<Vector<>> act, Ref<Vector<>> obs, Ref<Vector<>> reward) override;
void setInputCloud(const pcl::PointCloud<pcl::PointXYZ> &point_in);
void setESDF(const std::vector<float> &esdf_map, const pcl::PointCloud<pcl::PointXYZ> &point_in);
void addKdtree(std::shared_ptr<pcl::search::KdTree<pcl::PointXYZ>> kdtree) { kdtrees.push_back(kdtree); }
void addESDFMap(std::shared_ptr<sdf_tools::SignedDistanceField> esdf_map) { esdf_maps.push_back(esdf_map); }
void addMapSize(Eigen::Vector3d map_boundary_min, Eigen::Vector3d map_boundary_max) {
min_map_boundaries.push_back(map_boundary_min);
max_map_boundaries.push_back(map_boundary_max);
}
/* get functions */
bool getObs(Ref<Vector<>> obs) override;
bool getAct(Ref<Vector<>> act) const;
bool getRGBImage(Ref<ImgVector<>> img, const bool rgb);
bool getDepthImage(Ref<DepthImgVector<>> img) override;
bool getStereoImage(Ref<DepthImgVector<>> img);
int getMapNum() { return kdtrees.size(); }
void getCostAndGradient(ConstRef<Vector<>> dp, int id, float &cost, Ref<Vector<>> grad);
inline std::vector<std::string> getRewardNames() { return reward_names_; }
void getWorldBox(Ref<Vector<>> world_box) {
world_box << world_box_(0), world_box_(1), world_box_(2), world_box_(3), world_box_(4), world_box_(5); // xyz_min, xyz_max
};
/* other functions */
void collisionCheck(float dis = 0.2);
bool configCamera(const YAML::Node &cfg_node);
bool loadParam(const YAML::Node &cfg);
void computeDepthImage(const cv::Mat &left_frame, const cv::Mat &right_frame, cv::Mat *const depth);
bool isTerminalState(Scalar &reward) override;
void addObjectsToUnity(std::shared_ptr<UnityBridge> bridge) { bridge->addQuadrotor(quadrotor_ptr_); }
void runControlAndUpdateState(Eigen::Vector3f p_ref, Eigen::Vector3f v_ref, Eigen::Vector3f a_ref);
private:
// quadrotor state and observation
std::shared_ptr<Quadrotor> quadrotor_ptr_;
QuadState quad_state_;
Vector<kNObs> quad_obs_;
Vector<kNAct> quad_act_;
Logger logger_{"QaudrotorEnv"};
Eigen::Vector3f desired_p_, desired_v_, desired_a_;
// map
Matrix<3, 2> world_box_;
Vector<3> center_, scale_;
std::vector<std::shared_ptr<pcl::search::KdTree<pcl::PointXYZ>>> kdtrees;
std::vector<std::shared_ptr<sdf_tools::SignedDistanceField>> esdf_maps;
std::vector<Eigen::Vector3d> min_map_boundaries, max_map_boundaries;
// camera params
Scalar fov_;
int width_, height_;
Scalar stereo_baseline_;
std::shared_ptr<sgm_gpu::SgmGpu> sgm_;
cv::Mat rgb_img_, gray_img_, depth_img_;
std::shared_ptr<RGBCamera> rgb_camera_left, rgb_camera_right;
// trajectory optimization
int map_idx_{0};
Vector<3> goal_;
TrajOptimizationBridge *traj_opt_bridge;
// data collection
Scalar roll_var_, pitch_var_;
// others
int steps_;
YAML::Node cfg_;
bool is_collision_;
Scalar nearest_obstacle{10};
std::vector<std::string> reward_names_;
bool collect_data_, dagger_mode_{false};
};
} // namespace flightlib

106
flightlib/include/flightlib/envs/vec_env.hpp Normal file
View File

@@ -0,0 +1,106 @@
//
// This is inspired by RaiGym, thanks.
//
#pragma once
// std
#include <omp.h>
#include <time.h>
#include <filesystem>
#include <memory>
#include <regex>
// pcl
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>
// flightlib
#include "flightlib/bridges/unity_bridge.hpp"
#include "flightlib/common/logger.hpp"
#include "flightlib/common/types.hpp"
#include "flightlib/envs/env_base.hpp"
#include "flightlib/envs/quadrotor_env.hpp"
#include "flightlib/grad_traj_optimization/traj_optimization_bridge.h"
namespace flightlib {
template<typename EnvBase>
class VecEnv {
public:
VecEnv();
VecEnv(const std::string& cfgs, const bool from_file = true);
VecEnv(const YAML::Node& cfgs_node);
~VecEnv();
/* unity functions */
bool connectUnity();
void disconnectUnity();
bool setUnity(bool render);
void render();
bool spawnTrees();
bool savePointcloud(int ply_id);
bool spawnTreesAndSavePointcloud(int ply_id_in = -1, float spacing = -1);
void close();
void setSeed(const int seed);
/* set functions */
bool reset(Ref<MatrixRowMajor<>> obs);
bool step(Ref<MatrixRowMajor<>> act, Ref<MatrixRowMajor<>> obs, Ref<MatrixRowMajor<>> reward, Ref<BoolVector<>> done);
void perAgentStep(int agent_id, Ref<MatrixRowMajor<>> act, Ref<MatrixRowMajor<>> obs, Ref<MatrixRowMajor<>> reward, Ref<BoolVector<>> done);
bool setState(ConstRef<MatrixRowMajor<>> state); // World Frame
bool setGoal(ConstRef<MatrixRowMajor<>> goal); // World Frame
void setMapID(ConstRef<IntVector<>> id);
/* get functions */
inline int getObsDim(void) { return obs_dim_; };
inline int getActDim(void) { return act_dim_; };
inline int getRewDim(void) const { return rew_dim_; };
inline int getImgHeight(void) const { return img_height_; };
inline int getImgWidth(void) const { return img_width_; };
inline int getNumOfEnvs(void) { return envs_.size(); };
inline std::vector<std::string> getRewardNames(void) { return envs_[0]->getRewardNames(); };
inline void getWorldBox(Ref<Vector<>> world_box) { envs_[0]->getWorldBox(world_box); };
void getObs(Ref<MatrixRowMajor<>> obs);
bool getRGBImage(Ref<ImgMatrixRowMajor<>> img, const bool rgb_image);
bool getStereoImage(Ref<DepthImgMatrixRowMajor<>> img);
bool getDepthImage(Ref<DepthImgMatrixRowMajor<>> img);
void getCostAndGradient(ConstRef<MatrixRowMajor<>> dp, ConstRef<IntVector<>> traj_id, Ref<Vector<>> cost,
Ref<MatrixRowMajor<>> grad); // Body Frame
/* other functions */
void init(void);
void generateMaps();
int extract_number(const std::string& filename);
private:
// create objects
Logger logger_{"VecEnv"};
std::vector<std::unique_ptr<EnvBase>> envs_;
// Flightmare(Unity3D)
std::shared_ptr<UnityBridge> unity_bridge_ptr_;
SceneID scene_id_{UnityScene::WAREHOUSE};
bool unity_ready_{false};
bool unity_render_{false};
FrameID frameID{1};
RenderMessage_t unity_output_;
uint16_t receive_id_{0};
// scenario generation
Scalar avg_tree_spacing_;
Vector<3> bounding_box_, bounding_box_origin_;
Scalar pointcloud_resolution_;
// other variables
std::string ply_path_;
bool dagger_mode_{false}, supervised_mode_{false};
int seed_, num_envs_, obs_dim_, act_dim_, rew_dim_, num_threads_;
int img_width_, img_height_;
// yaml configurations
YAML::Node cfg_;
};
} // namespace flightlib

115
flightlib/include/flightlib/grad_traj_optimization/grad_traj_optimizer.h Normal file
View File

@@ -0,0 +1,115 @@
/*
This code is modified from https://github.com/HKUST-Aerial-Robotics/grad_traj_optimization, thanks to their excellent work.
*/
#ifndef _GRAD_TRAJ_OPTIMIZER_H_
#define _GRAD_TRAJ_OPTIMIZER_H_
#include <yaml-cpp/yaml.h>
#include <Eigen/Eigen>
#include "flightlib/grad_traj_optimization/qp_generator.h"
#include "sdf_tools/collision_map.hpp"
#include "sdf_tools/sdf.hpp"
#define GDTB getDistanceToBoundary
using namespace std;
class GradTrajOptimizer {
public:
GradTrajOptimizer(const YAML::Node &cfg);
void getCoefficient(Eigen::MatrixXd &coeff) { coeff = this->coeff; };
double getCost() { return this->min_f; }
void getSegmentTime(Eigen::VectorXd &T) { T = this->Time; }
void setSignedDistanceField(std::shared_ptr<sdf_tools::SignedDistanceField> s, double res);
void setGoal(Eigen::Vector3d goal);
void setBoundary(Eigen::Vector3d min, Eigen::Vector3d max);
void getCostAndGradient(const std::vector<double> &df, const std::vector<double> &dp, double &cost, std::vector<double> &grad) const;
/** convert derivatives of end points to polynomials coefficient */
void getCoefficientFromDerivative(Eigen::MatrixXd &coeff, const std::vector<double> &_dp) const;
private:
/** signed distance field */
double resolution;
Eigen::Vector3d map_boundary_min, map_boundary_max;
std::shared_ptr<sdf_tools::SignedDistanceField> sdf;
mutable Eigen::VectorXd boundary; // min x max x... min z,max z
/** coefficient of polynomials*/
mutable Eigen::MatrixXd coeff;
/** important matrix and variables*/
Eigen::MatrixXd A;
Eigen::MatrixXd C;
Eigen::MatrixXd L;
Eigen::MatrixXd R;
Eigen::MatrixXd Rff;
Eigen::MatrixXd Rpp;
Eigen::MatrixXd Rpf;
Eigen::MatrixXd Rfp;
Eigen::VectorXd Time;
Eigen::MatrixXd V;
mutable Eigen::MatrixXd Df;
Eigen::MatrixXd Dp;
Eigen::MatrixXd path;
Eigen::Vector3d goal;
/** tractory params */
double sgm_time;
int num_dp;
int num_df;
int num_point;
double min_f;
// weight of cost
double w_smooth;
double w_collision;
double w_vel;
double w_acc;
double w_goal;
double w_long;
// params of cost
double d0;
double r;
double alpha;
double v0;
double rv;
double alphav;
double a0;
double ra;
double alphaa;
/** get distance and gradient in signed distance field ,by position query*/
void getDistanceAndGradient(Eigen::Vector3d &pos, double &dist, Eigen::Vector3d &grad) const;
void getPositionFromCoeff(Eigen::Vector3d &pos, const Eigen::MatrixXd &coeff, const int &index, const double &time) const;
void getVelocityFromCoeff(Eigen::Vector3d &vel, const Eigen::MatrixXd &coeff, const int &index, const double &time) const;
void getAccelerationFromCoeff(Eigen::Vector3d &acc, const Eigen::MatrixXd &coeff, const int &index, const double &time) const;
/** penalty and gradient */
void getDistancePenalty(const double &distance, double &cost) const;
void getDistancePenaltyGradient(const double &distance, double &grad) const;
void getVelocityPenalty(const double &distance, double &cost) const;
void getVelocityPenaltyGradient(const double &vel, double &grad) const;
void getAccelerationPenalty(const double &distance, double &cost) const;
void getAccelerationPenaltyGradient(const double &acc, double &grad) const;
void getTimeMatrix(const double &t, Eigen::MatrixXd &T) const;
void constrains(double &n, double min, double max) const;
double getDistanceToBoundary(const double &x, const double &y, const double &z) const;
void recaluculateGradient(const double &x, const double &y, const double &z, Eigen ::Vector3d &grad) const;
};
#endif

18
flightlib/include/flightlib/grad_traj_optimization/opt_utile.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef _LATTICE_NODE_H_
#define _LATTICE_NODE_H_
#include <Eigen/Eigen>
// body frame
void getLatticeGuiding(std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> &lattice_nodes, int horizon_num, int vertical_num, int radio_num,
int vel_num, double horizon_fov, double vertical_fov, double radio_range, double vel_fov, double vel_prefile);
void getPositionFromCoeff(Eigen::Vector3d &pos, Eigen::MatrixXd coeff, int index, double time);
void getVelocityFromCoeff(Eigen::Vector3d &vel, Eigen::MatrixXd coeff, int index, double time);
void getAccelerationFromCoeff(Eigen::Vector3d &acc, Eigen::MatrixXd coeff, int index, double time);
Eigen::MatrixXd solveCoeffFromBoundaryState(const Eigen::Vector3d &Pos_init, const Eigen::Vector3d &Vel_init, const Eigen::Vector3d &Acc_init,
const Eigen::Vector3d &Pos_end, const Eigen::Vector3d &Vel_end, const Eigen::Vector3d &Acc_end,
double Time);
#endif

53
flightlib/include/flightlib/grad_traj_optimization/qp_generator.h Normal file
View File

@@ -0,0 +1,53 @@
#ifndef _TRAJECTORY_GENERATOR_H_
#define _TRAJECTORY_GENERATOR_H_
#include <Eigen/Eigen>
#include <vector>
class TrajectoryGenerator {
private:
int m = 1; // number of segments in the trajectory
Eigen::MatrixXd _A; // Mapping matrix
Eigen::MatrixXd _Q; // Hessian matrix
Eigen::MatrixXd _C; // Selection matrix
Eigen::MatrixXd _L; // A.inv() * C.transpose()
Eigen::MatrixXd _R;
Eigen::MatrixXd _Rff;
Eigen::MatrixXd _Rpp;
Eigen::MatrixXd _Rpf;
Eigen::MatrixXd _Rfp;
Eigen::VectorXd _Pxi;
Eigen::VectorXd _Pyi;
Eigen::VectorXd _Pzi;
Eigen::VectorXd _Dx;
Eigen::VectorXd _Dy;
Eigen::VectorXd _Dz;
public:
Eigen::MatrixXd _Path;
Eigen::VectorXd _Time;
TrajectoryGenerator();
~TrajectoryGenerator();
void QPGeneration(const Eigen::VectorXd &Time);
void StackOptiDep(); // Stack the optimization's dependency, the intermediate matrix and initial derivatives
Eigen::MatrixXd getA();
Eigen::MatrixXd getQ();
Eigen::MatrixXd getC();
Eigen::MatrixXd getL();
Eigen::MatrixXd getR();
Eigen::MatrixXd getRpp();
Eigen::MatrixXd getRff();
Eigen::MatrixXd getRfp();
Eigen::MatrixXd getRpf();
};
#endif

71
flightlib/include/flightlib/grad_traj_optimization/traj_optimization_bridge.h Normal file
View File

@@ -0,0 +1,71 @@
#ifndef _TRAJ_OPTIMIZATION_BRIDGE_H_
#define _TRAJ_OPTIMIZATION_BRIDGE_H_
#include <pcl/common/common.h>
#include <pcl/io/ply_io.h>
#include <pcl_ros/point_cloud.h>
#include <sensor_msgs/PointCloud.h>
#include <stdlib.h>
#include <yaml-cpp/yaml.h>
#include <Eigen/Eigen>
#include "flightlib/grad_traj_optimization/grad_traj_optimizer.h"
#include "flightlib/grad_traj_optimization/opt_utile.h"
namespace traj_opt {
std::shared_ptr<sdf_tools::SignedDistanceField> SdfConstruction(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Eigen::Vector3d &map_boundary_min_sdf,
Eigen::Vector3d &map_boundary_max_sdf);
}
class TrajOptimizationBridge {
private:
YAML::Node cfg_;
// state of uav in world frame
Eigen::Vector3d pos_;
Eigen::Vector3d vel_;
Eigen::Vector3d acc_;
Eigen::Quaterniond q_wb_;
Eigen::Vector3d goal_;
Eigen::MatrixXd pred_coeff_;
// std::string ply_file;
double resolution;
std::shared_ptr<sdf_tools::SignedDistanceField> sdf_;
Eigen::Vector3d map_boundary_min_, map_boundary_max_;
// x_pva, y_pva, z_pva in world frame
std::vector<double> dp_; // df refers to the initial_state currently
std::vector<double> df_; // dp refers to the end_state currently
double goal_length;
int horizon_num, vertical_num, radio_num, vel_num;
double horizon_fov, vertical_fov, radio_range, vel_fov, vel_prefile;
std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> lattice_nodes;
void loadParam(YAML::Node &cfg);
public:
TrajOptimizationBridge();
~TrajOptimizationBridge();
void setMap(std::shared_ptr<sdf_tools::SignedDistanceField> sdf_for_traj_optimization, Eigen::Vector3d &map_boundary_min,
Eigen::Vector3d &map_boundary_max);
// in world frame
void setState(Eigen::Vector3d pos, Eigen::Quaterniond q, Eigen::Vector3d vel, Eigen::Vector3d acc);
void setGoal(Eigen::Vector3d goal);
// dp in body frame, grad in body frame
void getCostAndGradient(const std::vector<double> &dp, int id, double &cost, std::vector<double> &grad);
void getNextStateAndCost(const std::vector<double> &dp, double &cost, Eigen::Vector3d &pos, Eigen::Vector3d &vel, Eigen::Vector3d &acc,
double sim_t);
void solveBVP(const std::vector<double> &dp);
void getNextState(Eigen::Vector3d &pos, Eigen::Vector3d &vel, Eigen::Vector3d &acc, double sim_t);
};
#endif

11843
flightlib/include/flightlib/json/json.hpp Normal file
View File

File diff suppressed because it is too large Load Diff

19
flightlib/include/flightlib/objects/object_base.hpp Normal file
View File

@@ -0,0 +1,19 @@
#pragma once
#include "flightlib/common/types.hpp"
namespace flightlib {
class ObjectBase {
public:
ObjectBase();
virtual ~ObjectBase();
// reset
virtual bool reset(void) = 0;
// run
virtual bool run(const Scalar dt) = 0;
};
} // namespace flightlib

110
flightlib/include/flightlib/objects/quadrotor.hpp Normal file
View File

@@ -0,0 +1,110 @@
#pragma once
/*
Explanation:
We retain this class but do not use most of its functionalities.
For efficiency, we do not use the built-in dynamics model and instead
directly use the desired attitude given by the controller as actual state.
Because the proposed approach (YOPO) only focuses on the trajectory performance,
while control is preformed by an external controller.
*/
#include <stdlib.h>
#include "flightlib/common/command.hpp"
#include "flightlib/common/integrator_rk4.hpp"
#include "flightlib/common/types.hpp"
#include "flightlib/dynamics/quadrotor_dynamics.hpp"
#include "flightlib/objects/object_base.hpp"
#include "flightlib/sensors/imu.hpp"
#include "flightlib/sensors/rgb_camera.hpp"
namespace flightlib {
class Quadrotor : ObjectBase {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Quadrotor(const std::string& cfg_path);
Quadrotor(const QuadrotorDynamics& dynamics = QuadrotorDynamics(1.0, 0.25));
~Quadrotor();
// reset
bool reset(void) override;
bool reset(const QuadState& state);
void init(void);
// run the quadrotor
bool setState(const Ref<Vector<>> p, const Ref<Vector<>> v, const Quaternion q_, const Ref<Vector<>> a_, const Scalar ctl_dt);
bool run(const Scalar dt) override;
bool run(const Command& cmd, const Scalar dt);
void runSimpleFlight(const Eigen::Vector3f& ref_acc, float ref_yaw, Eigen::Quaternionf& quat_des);
// public get functions
bool getState(QuadState* const state) const;
bool getMotorThrusts(Ref<Vector<4>> motor_thrusts) const;
bool getMotorOmega(Ref<Vector<4>> motor_omega) const;
bool getDynamics(QuadrotorDynamics* const dynamics) const;
const QuadrotorDynamics& getDynamics();
Vector<3> getSize(void) const;
Vector<3> getPosition(void) const;
Quaternion getQuaternion(void) const;
std::vector<std::shared_ptr<RGBCamera>> getCameras(void) const;
bool getCamera(const size_t cam_id, std::shared_ptr<RGBCamera> camera) const;
int getNumCamera() const;
bool getCollision() const;
// public set functions
bool setState(const QuadState& state);
bool setCommand(const Command& cmd);
bool updateDynamics(const QuadrotorDynamics& dynamics);
bool addRGBCamera(std::shared_ptr<RGBCamera> camera);
// low-level controller
Vector<4> runFlightCtl(const Scalar sim_dt, const Vector<3>& omega, const Command& cmd);
// simulate motors
void runMotors(const Scalar sim_dt, const Vector<4>& motor_thrust_des);
// constrain world box
bool setWorldBox(const Ref<Matrix<3, 2>> box);
bool constrainInWorldBox(const QuadState& old_state);
//
inline Scalar getMass(void) { return dynamics_.getMass(); };
inline void setSize(const Ref<Vector<3>> size) { size_ = size; };
inline void setCollision(const bool collision) { collision_ = collision; };
float getSimT() { return state_.t; }
private:
// quadrotor dynamics, integrators
QuadrotorDynamics dynamics_;
IMU imu_;
std::unique_ptr<IntegratorRK4> integrator_ptr_;
std::vector<std::shared_ptr<RGBCamera>> rgb_cameras_;
// quad control command
Command cmd_;
// quad state
QuadState state_;
Vector<3> size_;
bool collision_;
// auxiliar variablers
Vector<4> motor_omega_;
Vector<4> motor_thrusts_;
Matrix<4, 4> B_allocation_;
Matrix<4, 4> B_allocation_inv_;
// P gain for body-rate control
const Matrix<3, 3> Kinv_ang_vel_tau_ = Vector<3>(16.6, 16.6, 5.0).asDiagonal();
// gravity
const Vector<3> gz_{0.0, 0.0, Gz};
// auxiliary variables
Matrix<3, 2> world_box_;
};
} // namespace flightlib

13
flightlib/include/flightlib/objects/static_gate.hpp Normal file
View File

@@ -0,0 +1,13 @@
#pragma once
#include "flightlib/objects/static_object.hpp"
namespace flightlib {
class StaticGate : public StaticObject {
public:
StaticGate(const std::string& id, const std::string& prefab_id = "rpg_gate")
: StaticObject(id, prefab_id) {}
~StaticGate() {}
};
} // namespace flightlib

36
flightlib/include/flightlib/objects/static_object.hpp Normal file
View File

@@ -0,0 +1,36 @@
#pragma once
#include "flightlib/common/types.hpp"
namespace flightlib {
class StaticObject {
public:
StaticObject(const std::string& id, const std::string& prefab_id)
: id_(id), prefab_id_(prefab_id){};
virtual ~StaticObject(){};
// public set functions
virtual void setPosition(const Vector<3>& position) { position_ = position; };
virtual void setQuaternion(const Quaternion& quaternion) {
quat_ = quaternion;
};
virtual void setSize(const Vector<3>& size) { size_ = size; };
// public get functions
virtual Vector<3> getPosition(void) { return position_; };
virtual Quaternion getQuaternion(void) { return quat_; };
virtual Vector<3> getSize(void) { return size_; };
const std::string& getID(void) { return id_; };
const std::string& getPrefabID(void) { return prefab_id_; };
private:
std::string id_;
std::string prefab_id_;
protected:
Vector<3> position_{0.0, 0.0, 0.0};
Quaternion quat_{1.0, 0.0, 0.0, 0.0};
Vector<3> size_{1.0, 1.0, 1.0};
};
} // namespace flightlib

15
flightlib/include/flightlib/objects/unity_camera.hpp Normal file
View File

@@ -0,0 +1,15 @@
#pragma once
#include "flightlib/common/types.hpp"
#include "flightlib/sensors/rgb_camera.hpp"
namespace flightlib {
class UnityCamera : RGBCamera {
public:
UnityCamera();
~UnityCamera();
private:
};
} // namespace flightlib

92
flightlib/include/flightlib/ros_nodes/flight_pilot.hpp Normal file
View File

@@ -0,0 +1,92 @@
#pragma once
#include <cv_bridge/cv_bridge.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <sensor_msgs/CameraInfo.h>
#include <yaml-cpp/yaml.h>
#include <cmath>
#include <filesystem>
#include <memory>
#include "std_msgs/Empty.h"
// flightlib
#include "flightlib/bridges/unity_bridge.hpp"
#include "flightlib/common/quad_state.hpp"
#include "flightlib/common/types.hpp"
#include "flightlib/objects/quadrotor.hpp"
#include "flightlib/sensors/rgb_camera.hpp"
#include "flightlib/sensors/sgm_gpu/sgm_gpu.h"
using namespace flightlib;
namespace flightros {
class FlightPilot {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
FlightPilot(const ros::NodeHandle& nh, const ros::NodeHandle& pnh);
~FlightPilot();
// callbacks
void mainLoopCallback(const ros::TimerEvent& event);
void spawnTreeCallback(const std_msgs::Empty::ConstPtr& msg);
void clearTreeCallback(const std_msgs::Empty::ConstPtr& msg);
void savePointcloudCallback(const std_msgs::Empty::ConstPtr& msg);
void poseCallback(const nav_msgs::Odometry::ConstPtr& msg);
// unity
bool setUnity(const bool render);
bool connectUnity(void);
bool disconnectUnity();
bool loadParams(const YAML::Node& cfg);
bool configCamera(const YAML::Node& cfg);
void computeDepthImage(const cv::Mat& left_frame, const cv::Mat& right_frame, cv::Mat* const depth);
bool spawnTreesAndSavePointCloud();
private:
// ros nodes
ros::NodeHandle nh_;
ros::NodeHandle pnh_;
// publisher & subscriber
std::string odom_topic_;
ros::Publisher stereo_pub, left_img_pub, depth_pub, cam_info_pub;
ros::Subscriber state_est_sub_, spawn_tree_sub_, clear_tree_sub_, save_pc_sub_;
// main loop timer
ros::Timer timer_main_loop_;
ros::Time timestamp;
Scalar main_loop_freq_{50.0};
// unity & quadrotor
Vector<3> quad_size_;
QuadState quad_state_;
std::shared_ptr<Quadrotor> quad_ptr_;
std::shared_ptr<UnityBridge> unity_bridge_ptr_;
SceneID scene_id_{UnityScene::WAREHOUSE};
bool unity_ready_{false};
bool unity_render_{false};
RenderMessage_t unity_output_;
uint16_t receive_id_{0};
FrameID frameID{0};
// camera param
Scalar stereo_baseline_;
Scalar fov_;
int width_;
int height_;
bool use_depth, use_stereo;
std::shared_ptr<RGBCamera> rgb_camera_left, rgb_camera_right;
std::shared_ptr<sgm_gpu::SgmGpu> sgm_;
// tree generation
int ply_id_{0};
Scalar avg_tree_spacing_;
Vector<3> bounding_box_, bounding_box_origin_;
Scalar pointcloud_resolution_;
std::string ply_path_;
};
} // namespace flightros

16
flightlib/include/flightlib/sensors/imu.hpp Normal file
View File

@@ -0,0 +1,16 @@
#pragma once
#include "flightlib/common/types.hpp"
#include "flightlib/sensors/sensor_base.hpp"
namespace flightlib {
class IMU : SensorBase {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
IMU();
~IMU();
private:
};
} // namespace flightlib

126
flightlib/include/flightlib/sensors/rgb_camera.hpp Normal file
View File

@@ -0,0 +1,126 @@
#pragma once
#include <yaml-cpp/yaml.h>
#include <deque>
#include <functional>
#include <memory>
#include <mutex>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "flightlib/common/logger.hpp"
#include "flightlib/common/types.hpp"
#include "flightlib/sensors/sensor_base.hpp"
namespace flightlib {
enum CameraLayer { RGB = 0, DepthMap = 1, Segmentation = 2, OpticalFlow = 3 };
namespace RGBCameraTypes {
typedef int8_t Intensity_t;
typedef cv::Mat Image_t;
struct RGBImage_t {
Image_t image;
USecs elapsed_useconds;
};
struct Depthmap_t {
Image_t image;
USecs elapsed_useconds;
};
struct Segement_t {
Image_t image;
USecs elapsed_useconds;
};
struct OpticFlow_t {
Image_t image;
USecs elapsed_useconds;
};
typedef Eigen::Matrix4d Mat4_t;
typedef Eigen::Vector3d Vec3_t;
typedef std::function<Eigen::Vector3d()> GetPos_t;
typedef std::function<Eigen::Vector3d()> GetVel_t;
typedef std::function<Eigen::Vector3d()> GetAcc_t;
typedef std::function<Eigen::Quaterniond()> GetQuat_t;
typedef std::function<Eigen::Vector3d()> GetOmega_t;
typedef std::function<Eigen::Vector3d()> GetPsi_t;
const std::string RGB = "rgb";
// image post processing
typedef std::string PostProcessingID;
const PostProcessingID Depth = "depth";
const PostProcessingID OpticalFlow = "optical_flow";
const PostProcessingID ObjectSegment = "object_segment"; // object segmentation
const PostProcessingID CategorySegment = "category_segment"; // category segmentation
} // namespace RGBCameraTypes
class RGBCamera : SensorBase {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
RGBCamera();
~RGBCamera();
// public set functions
bool setRelPose(const Ref<Vector<3>> B_r_BC, const Ref<Matrix<3, 3>> R_BC);
bool setWidth(const int width);
bool setHeight(const int height);
bool setFOV(const Scalar fov);
bool setDepthScale(const Scalar depth_scale);
bool setPostProcesscing(const std::vector<bool>& enabled_layers);
bool feedImageQueue(const int image_layer, const cv::Mat& image_mat);
void clearImageQueue();
// public get functions
std::vector<bool> getEnabledLayers(void) const;
std::vector<std::string> GetPostProcessing(void);
Matrix<4, 4> getRelPose(void) const;
int getChannels(void) const;
int getWidth(void) const;
int getHeight(void) const;
Scalar getFOV(void) const;
Scalar getDepthScale(void) const;
bool getRGBImage(cv::Mat& rgb_img);
bool getDepthMap(cv::Mat& depth_map);
bool getSegmentation(cv::Mat& segmentation);
bool getOpticalFlow(cv::Mat& opticalflow);
// auxiliary functions
void enableDepth(const bool on);
void enableOpticalFlow(const bool on);
void enableSegmentation(const bool on);
private:
Logger logger_{"RBGCamera"};
// camera parameters
int channels_;
int width_;
int height_;
Scalar fov_;
Scalar depth_scale_;
// Camera relative
Vector<3> B_r_BC_;
Matrix<4, 4> T_BC_;
// image data buffer
std::mutex queue_mutex_;
const int queue_size_ = 0; // 1
// TODO：不要用队列，就单纯的Mat就好了，也不会有滞留的问题，也不会有弹空的问题；先不改了省的出错
std::deque<cv::Mat> rgb_queue_;
std::deque<cv::Mat> depth_queue_;
std::deque<cv::Mat> opticalflow_queue_;
std::deque<cv::Mat> segmentation_queue_;
// [rgb, depth, segmentation, optical flow]
std::vector<bool> enabled_layers_;
// [depth, optical flow, segmentation, segmentation]
std::unordered_map<RGBCameraTypes::PostProcessingID, bool> post_processing_;
};
} // namespace flightlib

14
flightlib/include/flightlib/sensors/sensor_base.hpp Normal file
View File

@@ -0,0 +1,14 @@
#pragma once
#include "flightlib/common/types.hpp"
namespace flightlib {
class SensorBase {
public:
SensorBase();
virtual ~SensorBase();
private:
};
} // namespace flightlib

39
flightlib/include/flightlib/sensors/sgm_gpu/configuration.h Normal file
View File

@@ -0,0 +1,39 @@
/***********************************************************************
Copyright (C) 2020 Hironori Fujimoto
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
***********************************************************************/
#ifndef SGM_GPU__CONFIGURATION_H_
#define SGM_GPU__CONFIGURATION_H_
#include <stdint.h>
#define MAX_DISPARITY 128
#define CENSUS_WIDTH 9
#define CENSUS_HEIGHT 7
#define TOP (CENSUS_HEIGHT-1)/2
#define LEFT (CENSUS_WIDTH-1)/2
namespace sgm_gpu
{
typedef uint32_t cost_t;
}
#define COSTAGG_BLOCKSIZE GPU_THREADS_PER_BLOCK
#define COSTAGG_BLOCKSIZE_HORIZ GPU_THREADS_PER_BLOCK
#endif // SGM_GPU__CONFIGURATION_H_

511
flightlib/include/flightlib/sensors/sgm_gpu/cost_aggregation.h Normal file
View File

@@ -0,0 +1,511 @@
/***********************************************************************
Copyright (C) 2019 Hironori Fujimoto
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
***********************************************************************/
#include "flightlib/sensors/sgm_gpu/util.h"
#ifndef COST_AGGREGATION_H_
#define COST_AGGREGATION_H_
#define ITER_COPY 0
#define ITER_NORMAL 1
#define MIN_COMPUTE 0
#define MIN_NOCOMPUTE 1
#define DIR_UPDOWN 0
#define DIR_DOWNUP 1
#define DIR_LEFTRIGHT 2
#define DIR_RIGHTLEFT 3
namespace sgm_gpu
{
template<int add_col, bool recompute, bool join_dispcomputation>
__device__ __forceinline__ void CostAggregationGenericIndexesIncrement(int *index, int *index_im, int *col, const int add_index, const int add_imindex) {
*index += add_index;
if(recompute || join_dispcomputation) {
*index_im += add_imindex;
if(recompute) {
*col += add_col;
}
}
}
template<int add_index, bool recompute, bool join_dispcomputation>
__device__ __forceinline__ void CostAggregationDiagonalGenericIndexesIncrement(int *index, int *index_im, int *col, const int cols, const int initial_row, const int i, const int dis) {
*col += add_index;
if(add_index > 0 && *col >= cols) {
*col = 0;
} else if(*col < 0) {
*col = cols-1;
}
*index = abs(initial_row-i)*cols*MAX_DISPARITY+*col*MAX_DISPARITY+dis;
if(recompute || join_dispcomputation) {
*index_im = abs(initial_row-i)*cols+*col;
}
}
template<class T, int iter_type, int min_type, int dir_type, bool first_iteration, bool recompute, bool join_dispcomputation>
__device__ __forceinline__ void CostAggregationGenericIteration(int index, int index_im, int col, uint32_t *old_values, int *old_value1, int *old_value2, int *old_value3, int *old_value4, uint32_t *min_cost, uint32_t *min_cost_p2, uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int p1_vector, const int p2_vector, const T *_d_transform0, const T *_d_transform1, const int lane, const int MAX_PAD, const int dis, T *rp0, T *rp1, T *rp2, T *rp3, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
const T __restrict__ *d_transform0 = _d_transform0;
const T __restrict__ *d_transform1 = _d_transform1;
uint32_t costs, next_dis, prev_dis;
if(iter_type == ITER_NORMAL) {
// First shuffle
int prev_dis1 = shfl_up_32(*old_value4, 1);
if(lane == 0) {
prev_dis1 = MAX_PAD;
}
// Second shuffle
int next_dis4 = shfl_down_32(*old_value1, 1);
if(lane == 31) {
next_dis4 = MAX_PAD;
}
// Shift + rotate
//next_dis = __funnelshift_r(next_dis4, *old_values, 8);
next_dis = __byte_perm(*old_values, next_dis4, 0x4321);
prev_dis = __byte_perm(*old_values, prev_dis1, 0x2104);
next_dis = next_dis + p1_vector;
prev_dis = prev_dis + p1_vector;
}
if(recompute) {
const int dif = col - dis;
if(dir_type == DIR_LEFTRIGHT) {
if(lane == 0) {
// lane = 0 is dis = 0, no need to subtract dis
*rp0 = d_transform1[index_im];
}
} else if(dir_type == DIR_RIGHTLEFT) {
// First iteration, load D pixels
if(first_iteration) {
const uint4 right = reinterpret_cast<const uint4*>(&d_transform1[index_im-dis-3])[0];
*rp3 = right.x;
*rp2 = right.y;
*rp1 = right.z;
*rp0 = right.w;
} else if(lane == 31 && dif >= 3) {
*rp3 = d_transform1[index_im-dis-3];
}
} else {
/*
__shared__ T right_p[MAX_DISPARITY+32];
const int warp_id = threadIdx.x / WARP_SIZE;
if(warp_id < 5) {
const int block_imindex = index_im - warp_id + 32;
const int rp_index = warp_id*WARP_SIZE+lane;
const int col_cpy = col-warp_id+32;
right_p[rp_index] = ((col_cpy-(159-rp_index)) >= 0) ? ld_gbl_cs(&d_transform1[block_imindex-(159-rp_index)]) : 0;
}*/
__shared__ T right_p[128+32];
const int warp_id = threadIdx.x / WARP_SIZE;
const int block_imindex = index_im - warp_id + 2;
const int rp_index = warp_id*WARP_SIZE+lane;
const int col_cpy = col-warp_id+2;
right_p[rp_index] = ((col_cpy-(129-rp_index)) >= 0) ? d_transform1[block_imindex-(129-rp_index)] : 0;
right_p[rp_index+64] = ((col_cpy-(129-rp_index-64)) >= 0) ? d_transform1[block_imindex-(129-rp_index-64)] : 0;
//right_p[rp_index+128] = ld_gbl_cs(&d_transform1[block_imindex-(129-rp_index-128)]);
if(warp_id == 0) {
right_p[128+lane] = ld_gbl_cs(&d_transform1[block_imindex-(129-lane)]);
}
__syncthreads();
const int px = MAX_DISPARITY+warp_id-dis-1;
*rp0 = right_p[px];
*rp1 = right_p[px-1];
*rp2 = right_p[px-2];
*rp3 = right_p[px-3];
}
const T left_pixel = d_transform0[index_im];
*old_value1 = popcount(left_pixel ^ *rp0);
*old_value2 = popcount(left_pixel ^ *rp1);
*old_value3 = popcount(left_pixel ^ *rp2);
*old_value4 = popcount(left_pixel ^ *rp3);
if(iter_type == ITER_COPY) {
*old_values = uchars_to_uint32(*old_value1, *old_value2, *old_value3, *old_value4);
} else {
costs = uchars_to_uint32(*old_value1, *old_value2, *old_value3, *old_value4);
}
// Prepare for next iteration
if(dir_type == DIR_LEFTRIGHT) {
*rp3 = shfl_up_32(*rp3, 1);
} else if(dir_type == DIR_RIGHTLEFT) {
*rp0 = shfl_down_32(*rp0, 1);
}
} else {
if(iter_type == ITER_COPY) {
*old_values = ld_gbl_ca(reinterpret_cast<const uint32_t*>(&d_cost[index]));
} else {
costs = ld_gbl_ca(reinterpret_cast<const uint32_t*>(&d_cost[index]));
}
}
if(iter_type == ITER_NORMAL) {
const uint32_t min1 = __vminu4(*old_values, prev_dis);
const uint32_t min2 = __vminu4(next_dis, *min_cost_p2);
const uint32_t min_prev = __vminu4(min1, min2);
*old_values = costs + (min_prev - *min_cost);
}
if(iter_type == ITER_NORMAL || !recompute) {
uint32_to_uchars(*old_values, old_value1, old_value2, old_value3, old_value4);
}
if(join_dispcomputation) {
const uint32_t L0_costs = *((uint32_t*) (d_L0+index));
const uint32_t L1_costs = *((uint32_t*) (d_L1+index));
const uint32_t L2_costs = *((uint32_t*) (d_L2+index));
const uint32_t L3_costs = *((uint32_t*) (d_L3+index));
const uint32_t L4_costs = *((uint32_t*) (d_L4+index));
const uint32_t L5_costs = *((uint32_t*) (d_L5+index));
const uint32_t L6_costs = *((uint32_t*) (d_L6+index));
int l0_x, l0_y, l0_z, l0_w;
int l1_x, l1_y, l1_z, l1_w;
int l2_x, l2_y, l2_z, l2_w;
int l3_x, l3_y, l3_z, l3_w;
int l4_x, l4_y, l4_z, l4_w;
int l5_x, l5_y, l5_z, l5_w;
int l6_x, l6_y, l6_z, l6_w;
uint32_to_uchars(L0_costs, &l0_x, &l0_y, &l0_z, &l0_w);
uint32_to_uchars(L1_costs, &l1_x, &l1_y, &l1_z, &l1_w);
uint32_to_uchars(L2_costs, &l2_x, &l2_y, &l2_z, &l2_w);
uint32_to_uchars(L3_costs, &l3_x, &l3_y, &l3_z, &l3_w);
uint32_to_uchars(L4_costs, &l4_x, &l4_y, &l4_z, &l4_w);
uint32_to_uchars(L5_costs, &l5_x, &l5_y, &l5_z, &l5_w);
uint32_to_uchars(L6_costs, &l6_x, &l6_y, &l6_z, &l6_w);
const uint16_t val1 = l0_x + l1_x + l2_x + l3_x + l4_x + l5_x + l6_x + *old_value1;
const uint16_t val2 = l0_y + l1_y + l2_y + l3_y + l4_y + l5_y + l6_y + *old_value2;
const uint16_t val3 = l0_z + l1_z + l2_z + l3_z + l4_z + l5_z + l6_z + *old_value3;
const uint16_t val4 = l0_w + l1_w + l2_w + l3_w + l4_w + l5_w + l6_w + *old_value4;
int min_idx1 = dis;
uint16_t min1 = val1;
if(val1 > val2) {
min1 = val2;
min_idx1 = dis+1;
}
int min_idx2 = dis+2;
uint16_t min2 = val3;
if(val3 > val4) {
min2 = val4;
min_idx2 = dis+3;
}
uint16_t minval = min1;
int min_idx = min_idx1;
if(min1 > min2) {
minval = min2;
min_idx = min_idx2;
}
const int min_warpindex = warpReduceMinIndex(minval, min_idx);
if(lane == 0) {
d_disparity[index_im] = min_warpindex;
}
// Save smoothed cost to obtain right disparity
d_s[index] = val1;
d_s[index+1] = val2;
d_s[index+2] = val3;
d_s[index+3] = val4;
} else {
st_gbl_cs(reinterpret_cast<uint32_t*>(&d_L[index]), *old_values);
}
if(min_type == MIN_COMPUTE) {
int min_cost_scalar = min(min(*old_value1, *old_value2), min(*old_value3, *old_value4));
*min_cost = uchar_to_uint32(warpReduceMin(min_cost_scalar));
*min_cost_p2 = *min_cost + p2_vector;
}
}
template<class T, int add_col, int dir_type, bool recompute, bool join_dispcomputation>
__device__ __forceinline__ void CostAggregationGeneric(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int initial_row, const int initial_col, const int max_iter, const int cols, int add_index, const T *_d_transform0, const T *_d_transform1, const int add_imindex, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
const int lane = threadIdx.x % WARP_SIZE;
const int dis = 4*lane;
int index = initial_row*cols*MAX_DISPARITY+initial_col*MAX_DISPARITY+dis;
int col, index_im;
if(recompute || join_dispcomputation) {
if(recompute) {
col = initial_col;
}
index_im = initial_row*cols+initial_col;
}
const int MAX_PAD = UCHAR_MAX-P1;
const uint32_t p1_vector = uchars_to_uint32(P1, P1, P1, P1);
const uint32_t p2_vector = uchars_to_uint32(P2, P2, P2, P2);
int old_value1;
int old_value2;
int old_value3;
int old_value4;
uint32_t min_cost, min_cost_p2, old_values;
T rp0, rp1, rp2, rp3;
if(recompute) {
if(dir_type == DIR_LEFTRIGHT) {
CostAggregationGenericIteration<T, ITER_COPY, MIN_COMPUTE, dir_type, true, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp3, &rp0, &rp1, &rp2, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp2, &rp3, &rp0, &rp1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp1, &rp2, &rp3, &rp0, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
for(int i = 4; i < max_iter-3; i+=4) {
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp3, &rp0, &rp1, &rp2, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp2, &rp3, &rp0, &rp1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp1, &rp2, &rp3, &rp0, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
}
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp3, &rp0, &rp1, &rp2, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp2, &rp3, &rp0, &rp1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_NOCOMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp1, &rp2, &rp3, &rp0, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
} else if(dir_type == DIR_RIGHTLEFT) {
CostAggregationGenericIteration<T, ITER_COPY, MIN_COMPUTE, dir_type, true, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp1, &rp2, &rp3, &rp0, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp2, &rp3, &rp0, &rp1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp3, &rp0, &rp1, &rp2, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
for(int i = 4; i < max_iter-3; i+=4) {
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp1, &rp2, &rp3, &rp0, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp2, &rp3, &rp0, &rp1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp3, &rp0, &rp1, &rp2, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
}
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp1, &rp2, &rp3, &rp0, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp2, &rp3, &rp0, &rp1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_NOCOMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp3, &rp0, &rp1, &rp2, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
} else {
CostAggregationGenericIteration<T, ITER_COPY, MIN_COMPUTE, dir_type, true, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
for(int i = 1; i < max_iter; i++) {
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
}
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_NOCOMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
}
} else {
CostAggregationGenericIteration<T, ITER_COPY, MIN_COMPUTE, dir_type, true, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
for(int i = 1; i < max_iter; i++) {
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
}
CostAggregationGenericIndexesIncrement<add_col, recompute, join_dispcomputation>(&index, &index_im, &col, add_index, add_imindex);
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_NOCOMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
}
}
template<int add_index, class T, int dir_type, bool recompute, bool join_dispcomputation>
__device__ __forceinline__ void CostAggregationDiagonalGeneric(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int initial_row, const int initial_col, const int max_iter, const int col_nomin, const int col_copycost, const int cols, const T *_d_transform0, const T *_d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
const int lane = threadIdx.x % WARP_SIZE;
const int dis = 4*lane;
int col = initial_col;
int index = initial_row*cols*MAX_DISPARITY+initial_col*MAX_DISPARITY+dis;
int index_im;
if(recompute || join_dispcomputation) {
index_im = initial_row*cols+col;
}
const int MAX_PAD = UCHAR_MAX-P1;
const uint32_t p1_vector = uchars_to_uint32(P1, P1, P1, P1);
const uint32_t p2_vector = uchars_to_uint32(P2, P2, P2, P2);
int old_value1;
int old_value2;
int old_value3;
int old_value4;
uint32_t min_cost, min_cost_p2, old_values;
T rp0, rp1, rp2, rp3;
CostAggregationGenericIteration<T, ITER_COPY, MIN_COMPUTE, dir_type, true, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
for(int i = 1; i < max_iter; i++) {
CostAggregationDiagonalGenericIndexesIncrement<add_index, recompute, join_dispcomputation>(&index, &index_im, &col, cols, initial_row, i, dis);
if(col == col_copycost) {
CostAggregationGenericIteration<T, ITER_COPY, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
} else {
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_COMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
}
}
CostAggregationDiagonalGenericIndexesIncrement<add_index, recompute, join_dispcomputation>(&index, &index_im, &col, cols, max_iter, initial_row, dis);
if(col == col_copycost) {
CostAggregationGenericIteration<T, ITER_COPY, MIN_NOCOMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
} else {
CostAggregationGenericIteration<T, ITER_NORMAL, MIN_NOCOMPUTE, dir_type, false, recompute, join_dispcomputation>(index, index_im, col, &old_values, &old_value1, &old_value2, &old_value3, &old_value4, &min_cost, &min_cost_p2, d_cost, d_L, d_s, p1_vector, p2_vector, _d_transform0, _d_transform1, lane, MAX_PAD, dis, &rp0, &rp1, &rp2, &rp3, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
}
}
template<class T>
__global__ void CostAggregationKernelDiagonalDownUpRightLeft(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int rows, const int cols, const T *d_transform0, const T *d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
const int initial_col = cols - (blockIdx.x*(blockDim.x/WARP_SIZE) + (threadIdx.x / WARP_SIZE)) - 1;
if(initial_col < cols) {
const int initial_row = rows-1;
const int add_index = -1;
const int col_nomin = 0;
const int col_copycost = cols-1;
const int max_iter = rows-1;
const bool recompute = false;
const bool join_dispcomputation = false;
CostAggregationDiagonalGeneric<add_index, T, DIR_DOWNUP, recompute, join_dispcomputation>(d_cost, d_L, d_s, P1, P2, initial_row, initial_col, max_iter, col_nomin, col_copycost, cols, d_transform0, d_transform1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
}
}
template<class T>
__global__ void CostAggregationKernelDiagonalDownUpLeftRight(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int rows, const int cols, const T *d_transform0, const T *d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
const int initial_col = cols - (blockIdx.x*(blockDim.x/WARP_SIZE) + (threadIdx.x / WARP_SIZE)) - 1;
if(initial_col >= 0) {
const int initial_row = rows-1;
const int add_index = 1;
const int col_nomin = cols-1;
const int col_copycost = 0;
const int max_iter = rows-1;
const bool recompute = false;
const bool join_dispcomputation = false;
CostAggregationDiagonalGeneric<add_index, T, DIR_DOWNUP, recompute, join_dispcomputation>(d_cost, d_L, d_s, P1, P2, initial_row, initial_col, max_iter, col_nomin, col_copycost, cols, d_transform0, d_transform1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
}
}
template<class T>
__global__ void CostAggregationKernelDiagonalUpDownRightLeft(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int rows, const int cols, const T *d_transform0, const T *d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
const int initial_col = blockIdx.x*(blockDim.x/WARP_SIZE) + (threadIdx.x / WARP_SIZE);
if(initial_col < cols) {
const int initial_row = 0;
const int add_index = -1;
const int col_nomin = 0;
const int col_copycost = cols-1;
const int max_iter = rows-1;
const bool recompute = false;
const bool join_dispcomputation = true;
CostAggregationDiagonalGeneric<add_index, T, DIR_UPDOWN, recompute, join_dispcomputation>(d_cost, d_L, d_s, P1, P2, initial_row, initial_col, max_iter, col_nomin, col_copycost, cols, d_transform0, d_transform1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
}
}
template<class T>
__global__ void CostAggregationKernelDiagonalUpDownLeftRight(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int rows, const int cols, const T *d_transform0, const T *d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
const int initial_col = blockIdx.x*(blockDim.x/WARP_SIZE) + (threadIdx.x / WARP_SIZE);
if(initial_col < cols) {
const int initial_row = 0;
const int add_index = 1;
const int col_nomin = cols-1;
const int col_copycost = 0;
const int max_iter = rows-1;
const bool recompute = false;
const bool join_dispcomputation = false;
CostAggregationDiagonalGeneric<add_index, T, DIR_UPDOWN, recompute, join_dispcomputation>(d_cost, d_L, d_s, P1, P2, initial_row, initial_col, max_iter, col_nomin, col_copycost, cols, d_transform0, d_transform1, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
}
}
template<class T>
__global__ void CostAggregationKernelLeftToRight(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int rows, const int cols, const T *d_transform0, const T *d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
const int initial_row = blockIdx.x*(blockDim.x/WARP_SIZE) + (threadIdx.x / WARP_SIZE);
if(initial_row < rows) {
const int initial_col = 0;
const int add_index = MAX_DISPARITY;
const int add_imindex = 1;
const int max_iter = cols-1;
const int add_col = 1;
const bool recompute = true;
const bool join_dispcomputation = false;
CostAggregationGeneric<T, add_col, DIR_LEFTRIGHT, recompute, join_dispcomputation>(d_cost, d_L, d_s, P1, P2, initial_row, initial_col, max_iter, cols, add_index, d_transform0, d_transform1, add_imindex, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
}
}
template<class T>
__global__ void CostAggregationKernelRightToLeft(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int rows, const int cols, const T *d_transform0, const T *d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
const int initial_row = blockIdx.x*(blockDim.x/WARP_SIZE) + (threadIdx.x / WARP_SIZE);
if(initial_row < rows) {
const int initial_col = cols-1;
const int add_index = -MAX_DISPARITY;
const int add_imindex = -1;
const int max_iter = cols-1;
const int add_col = -1;
const bool recompute = true;
const bool join_dispcomputation = false;
CostAggregationGeneric<T, add_col, DIR_RIGHTLEFT, recompute, join_dispcomputation>(d_cost, d_L, d_s, P1, P2, initial_row, initial_col, max_iter, cols, add_index, d_transform0, d_transform1, add_imindex, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
}
}
template<class T>
__global__ void CostAggregationKernelDownToUp(uint8_t* d_cost, uint8_t *d_L, uint16_t *d_s, const int P1, const int P2, const int rows, const int cols, const T *d_transform0, const T *d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
const int initial_col = blockIdx.x*(blockDim.x/WARP_SIZE) + (threadIdx.x / WARP_SIZE);
if(initial_col < cols) {
const int initial_row = rows-1;
const int add_index = -cols*MAX_DISPARITY;
const int add_imindex = -cols;
const int max_iter = rows-1;
const int add_col = 0;
const bool recompute = false;
const bool join_dispcomputation = false;
CostAggregationGeneric<T, add_col, DIR_DOWNUP, recompute, join_dispcomputation>(d_cost, d_L, d_s, P1, P2, initial_row, initial_col, max_iter, cols, add_index, d_transform0, d_transform1, add_imindex, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
}
}
template<class T>
//__launch_bounds__(64, 16)
__global__ void CostAggregationKernelUpToDown(uint8_t* d_cost, uint8_t *d_L, uint16_t* d_s, const int P1, const int P2, const int rows, const int cols, const T *d_transform0, const T *d_transform1, uint8_t* __restrict__ d_disparity, const uint8_t* d_L0, const uint8_t* d_L1, const uint8_t* d_L2, const uint8_t* d_L3, const uint8_t* d_L4, const uint8_t* d_L5, const uint8_t* d_L6) {
const int initial_col = blockIdx.x*(blockDim.x/WARP_SIZE) + (threadIdx.x / WARP_SIZE);
if(initial_col < cols) {
const int initial_row = 0;
const int add_index = cols*MAX_DISPARITY;
const int add_imindex = cols;
const int max_iter = rows-1;
const int add_col = 0;
const bool recompute = false;
const bool join_dispcomputation = false;
CostAggregationGeneric<T, add_col, DIR_UPDOWN, recompute, join_dispcomputation>(d_cost, d_L, d_s, P1, P2, initial_row, initial_col, max_iter, cols, add_index, d_transform0, d_transform1, add_imindex, d_disparity, d_L0, d_L1, d_L2, d_L3, d_L4, d_L5, d_L6);
}
}
} // namespace sgm_gpu
#endif /* COST_AGGREGATION_H_ */

30
flightlib/include/flightlib/sensors/sgm_gpu/costs.h Normal file
View File

@@ -0,0 +1,30 @@
/***********************************************************************
Copyright (C) 2020 Hironori Fujimoto
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
***********************************************************************/
#ifndef SGM_GPU__COSTS_H_
#define SGM_GPU__COSTS_H_
#include <stdint.h>
#include "flightlib/sensors/sgm_gpu/configuration.h"
namespace sgm_gpu
{
__global__ void CenterSymmetricCensusKernelSM2(const uint8_t *im, const uint8_t *im2, cost_t *transform, cost_t *transform2, const uint32_t rows, const uint32_t cols);
}
#endif // SGM_GPU__COSTS_H_

33
flightlib/include/flightlib/sensors/sgm_gpu/hamming_cost.h Normal file
View File

@@ -0,0 +1,33 @@
/***********************************************************************
Copyright (C) 2020 Hironori Fujimoto
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
***********************************************************************/
#ifndef SGM_GPU__HAMMING_COST_H_
#define SGM_GPU__HAMMING_COST_H_
#include "flightlib/sensors/sgm_gpu/configuration.h"
#include "flightlib/sensors/sgm_gpu/util.h"
#include <stdint.h>
namespace sgm_gpu
{
__global__ void
HammingDistanceCostKernel(const cost_t *d_transform0, const cost_t *d_transform1, uint8_t *d_cost, const int rows, const int cols );
}
#endif // SGM_GPU__HAMMING_COST_H_

31
flightlib/include/flightlib/sensors/sgm_gpu/left_right_consistency.h Normal file
View File

@@ -0,0 +1,31 @@
/***********************************************************************
Copyright (C) 2020 Hironori Fujimoto
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
***********************************************************************/
#ifndef SGM_GPU__LEFT_RIGHT_CONSISTENCY_H_
#define SGM_GPU__LEFT_RIGHT_CONSISTENCY_H_
#include <stdint.h>
namespace sgm_gpu
{
__global__ void ChooseRightDisparity(uint8_t *right_disparity, const uint16_t *smoothed_cost, const uint32_t rows, const uint32_t cols);
__global__ void LeftRightConsistencyCheck(uint8_t *disparity, const uint8_t *disparity_right, const uint32_t rows, const uint32_t cols);
}
#endif // SGM_GPU__LEFT_RIGHT_CONSISTENCY_H_

62
flightlib/include/flightlib/sensors/sgm_gpu/median_filter.h Normal file
View File

@@ -0,0 +1,62 @@
/***********************************************************************
Copyright (C) 2020 Hironori Fujimoto
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
***********************************************************************/
#ifndef SGM_GPU__MEDIAN_FILTER_H_
#define SGM_GPU__MEDIAN_FILTER_H_
#include <stdint.h>
namespace sgm_gpu
{
__global__ void MedianFilter3x3(const uint8_t* __restrict__ d_input, uint8_t* __restrict__ d_out, const uint32_t rows, const uint32_t cols);
template<int n, typename T>
__inline__ __device__ void MedianFilter(const T* __restrict__ d_input, T* __restrict__ d_out, const uint32_t rows, const uint32_t cols) {
const uint32_t idx = blockIdx.x*blockDim.x+threadIdx.x;
const uint32_t row = idx / cols;
const uint32_t col = idx % cols;
T window[n*n];
int half = n/2;
if(row >= half && col >= half && row < rows-half && col < cols-half) {
for(uint32_t i = 0; i < n; i++) {
for(uint32_t j = 0; j < n; j++) {
window[i*n+j] = d_input[(row-half+i)*cols+col-half+j];
}
}
for(uint32_t i = 0; i < (n*n/2)+1; i++) {
uint32_t min_idx = i;
for(uint32_t j = i+1; j < n*n; j++) {
if(window[j] < window[min_idx]) {
min_idx = j;
}
}
const T tmp = window[i];
window[i] = window[min_idx];
window[min_idx] = tmp;
}
d_out[idx] = window[n*n/2];
} else if(row < rows && col < cols) {
d_out[idx] = d_input[idx];
}
}
} // namespace sgm_gpu
#endif // SGM_GPU__MEDIAN_FILTER_H_

90
flightlib/include/flightlib/sensors/sgm_gpu/sgm_gpu.h Normal file
View File

@@ -0,0 +1,90 @@
/***********************************************************************
Copyright (C) 2020 Hironori Fujimoto
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
***********************************************************************/
#ifndef SGM_GPU__SGM_GPU_H_
#define SGM_GPU__SGM_GPU_H_
#include "flightlib/sensors/sgm_gpu/configuration.h"
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/types_c.h>
namespace sgm_gpu {
class SgmGpu {
private:
/**
* @brief Parameter used in SGM algorithm
*
* See SGM paper.
*/
uint8_t p1_, p2_;
/**
* @brief Enable/disable left-right consistency check
*/
bool check_consistency_;
// Memory for disparity computation
// d_: for device
uint8_t *d_im0_;
uint8_t *d_im1_;
uint32_t *d_transform0_;
uint32_t *d_transform1_;
uint8_t *d_cost_;
uint8_t *d_disparity_;
uint8_t *d_disparity_filtered_uchar_;
uint8_t *d_disparity_right_;
uint8_t *d_disparity_right_filtered_uchar_;
uint8_t *d_L0_;
uint8_t *d_L1_;
uint8_t *d_L2_;
uint8_t *d_L3_;
uint8_t *d_L4_;
uint8_t *d_L5_;
uint8_t *d_L6_;
uint8_t *d_L7_;
uint16_t *d_s_;
bool memory_allocated_;
uint32_t cols_, rows_;
void allocateMemory(uint32_t cols, uint32_t rows);
void freeMemory();
/**
* @brief Resize images to be width and height divisible by 4 for limit of
* CUDA code
*/
void resizeToDivisibleBy4(cv::Mat &left_image, cv::Mat &right_image);
// void convertToMsg(const cv::Mat_<unsigned char> &disparity,
// const sensor_msgs::CameraInfo &left_camera_info,
// const sensor_msgs::CameraInfo &right_camera_info,
// stereo_msgs::DisparityImage &disparity_msg);
public:
/**
* @brief Constructor which use namespace <parent>/libsgm_gpu for ROS param
*/
SgmGpu(const int cols, const int rows);
~SgmGpu();
bool computeDisparity(const cv::Mat &left_image, const cv::Mat &right_image, cv::Mat &disparity_out);
};
} // namespace sgm_gpu
#endif // SGM_GPU__SGM_GPU_H_

362
flightlib/include/flightlib/sensors/sgm_gpu/util.h Normal file
View File

@@ -0,0 +1,362 @@
/***********************************************************************
Copyright (C) 2020 Hironori Fujimoto
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
***********************************************************************/
#ifndef SGM_GPU__UTIL_H_
#define SGM_GPU__UTIL_H_
#include <iostream>
#include <dirent.h>
#include <stdio.h>
#define FERMI false
#define GPU_THREADS_PER_BLOCK_FERMI 256
#define GPU_THREADS_PER_BLOCK_MAXWELL 64
/* Defines related to GPU Architecture */
#if FERMI
#define GPU_THREADS_PER_BLOCK GPU_THREADS_PER_BLOCK_FERMI
#else
#define GPU_THREADS_PER_BLOCK GPU_THREADS_PER_BLOCK_MAXWELL
#endif
#define WARP_SIZE 32
namespace sgm_gpu
{
static void CheckCudaErrorAux (const char *, unsigned, const char *, cudaError_t);
#define CUDA_CHECK_RETURN(value) CheckCudaErrorAux(__FILE__,__LINE__, #value, value)
/**
* Check the return value of the CUDA runtime API call and exit
* the application if the call has failed.
*/
static void CheckCudaErrorAux (const char *file, unsigned line, const char *statement, cudaError_t err) {
if (err == cudaSuccess)
return;
std::cerr << statement<<" returned " << cudaGetErrorString(err) << "("<<err<< ") at "<<file<<":"<<line << std::endl;
exit (1);
}
/*************************************
GPU Side defines (ASM instructions)
**************************************/
// output temporal carry in internal register
#define UADD__CARRY_OUT(c, a, b) \
asm volatile("add.cc.u32 %0, %1, %2;" : "=r"(c) : "r"(a) , "r"(b))
// add & output with temporal carry of internal register
#define UADD__IN_CARRY_OUT(c, a, b) \
asm volatile("addc.cc.u32 %0, %1, %2;" : "=r"(c) : "r"(a) , "r"(b))
// add with temporal carry of internal register
#define UADD__IN_CARRY(c, a, b) \
asm volatile("addc.u32 %0, %1, %2;" : "=r"(c) : "r"(a) , "r"(b))
// packing and unpacking: from uint64_t to uint2
#define V2S_B64(v,s) \
asm("mov.b64 %0, {%1,%2};" : "=l"(s) : "r"(v.x), "r"(v.y))
// packing and unpacking: from uint2 to uint64_t
#define S2V_B64(s,v) \
asm("mov.b64 {%0,%1}, %2;" : "=r"(v.x), "=r"(v.y) : "l"(s))
/*************************************
DEVICE side basic block primitives
**************************************/
#if FERMI
#define LDG(ptr) (* ptr)
#else
#define LDG(ptr) __ldg(ptr)
#endif
#if FERMI
__shared__ int interBuff[GPU_THREADS_PER_BLOCK];
__inline__ __device__ int __emulated_shfl(const int scalarValue, const uint32_t source_lane) {
const int warpIdx = threadIdx.x / WARP_SIZE;
const int laneIdx = threadIdx.x % WARP_SIZE;
volatile int *interShuffle = interBuff + (warpIdx * WARP_SIZE);
interShuffle[laneIdx] = scalarValue;
return(interShuffle[source_lane % WARP_SIZE]);
}
#endif
__inline__ __device__ int shfl_32(int scalarValue, const int lane) {
#if FERMI
return __emulated_shfl(scalarValue, (uint32_t)lane);
#else
return __shfl_sync(0xffffffff, scalarValue, lane);
#endif
}
__inline__ __device__ int shfl_up_32(int scalarValue, const int n) {
#if FERMI
int lane = threadIdx.x % WARP_SIZE;
lane -= n;
return shfl_32(scalarValue, lane);
#else
return __shfl_up_sync(0xffffffff, scalarValue, n);
#endif
}
__inline__ __device__ int shfl_down_32(int scalarValue, const int n) {
#if FERMI
int lane = threadIdx.x % WARP_SIZE;
lane += n;
return shfl_32(scalarValue, lane);
#else
return __shfl_down_sync(0xffffffff, scalarValue, n);
#endif
}
__inline__ __device__ int shfl_xor_32(int scalarValue, const int n) {
#if FERMI
int lane = threadIdx.x % WARP_SIZE;
lane = lane ^ n;
return shfl_32(scalarValue, lane);
#else
return __shfl_xor_sync(0xffffffff, scalarValue, n);
#endif
}
__device__ __forceinline__ uint32_t ld_gbl_ca(const __restrict__ uint32_t *addr) {
uint32_t return_value;
asm("ld.global.ca.u32 %0, [%1];" : "=r"(return_value) : "l"(addr));
return return_value;
}
__device__ __forceinline__ uint32_t ld_gbl_cs(const __restrict__ uint32_t *addr) {
uint32_t return_value;
asm("ld.global.cs.u32 %0, [%1];" : "=r"(return_value) : "l"(addr));
return return_value;
}
__device__ __forceinline__ void st_gbl_wt(const __restrict__ uint32_t *addr, const uint32_t value) {
asm("st.global.wt.u32 [%0], %1;" :: "l"(addr), "r"(value));
}
__device__ __forceinline__ void st_gbl_cs(const __restrict__ uint32_t *addr, const uint32_t value) {
asm("st.global.cs.u32 [%0], %1;" :: "l"(addr), "r"(value));
}
__device__ __forceinline__ uint32_t gpu_get_sm_idx(){
uint32_t smid;
asm volatile("mov.u32 %0, %%smid;" : "=r"(smid));
return(smid);
}
__device__ __forceinline__ void uint32_to_uchars(const uint32_t s, int *u1, int *u2, int *u3, int *u4) {
//*u1 = s & 0xff;
*u1 = __byte_perm(s, 0, 0x4440);
//*u2 = (s>>8) & 0xff;
*u2 = __byte_perm(s, 0, 0x4441);
//*u3 = (s>>16) & 0xff;
*u3 = __byte_perm(s, 0, 0x4442);
//*u4 = s>>24;
*u4 = __byte_perm(s, 0, 0x4443);
}
__device__ __forceinline__ uint32_t uchars_to_uint32(int u1, int u2, int u3, int u4) {
//return u1 | (u2<<8) | (u3<<16) | (u4<<24);
//return __byte_perm(u1, u2, 0x7740) + __byte_perm(u3, u4, 0x4077);
return u1 | (u2<<8) | __byte_perm(u3, u4, 0x4077);
}
__device__ __forceinline__ uint32_t uchar_to_uint32(int u1) {
return __byte_perm(u1, u1, 0x0);
}
__device__ __forceinline__ unsigned int vcmpgeu4(unsigned int a, unsigned int b) {
unsigned int r, c;
c = a-b;
asm ("prmt.b32 %0,%1,0,0xba98;" : "=r"(r) : "r"(c));// build mask from msbs
return r; // byte-wise unsigned gt-eq comparison with mask result
}
__device__ __forceinline__ unsigned int vminu4(unsigned int a, unsigned int b) {
unsigned int r, s;
s = vcmpgeu4 (b, a);// mask = 0xff if a >= b
r = a & s; // select a when b >= a
s = b & ~s; // select b when b < a
r = r | s; // combine byte selections
return r;
}
__device__ __forceinline__ void print_uchars(const char* str, const uint32_t s) {
int u1, u2, u3, u4;
uint32_to_uchars(s, &u1, &u2, &u3, &u4);
printf("%s: %d %d %d %d\n", str, u1, u2, u3, u4);
}
template<class T>
__device__ __forceinline__ int popcount(T n) {
#if CSCT or CSCT_RECOMPUTE
return __popc(n);
#else
return __popcll(n);
#endif
}
__inline__ __device__ uint8_t minu8_index4(int *min_idx, const uint8_t val1, const int dis, const uint8_t val2, const int dis2, const uint8_t val3, const int dis3, const uint8_t val4, const int dis4) {
int min_idx1 = dis;
uint8_t min1 = val1;
if(val1 > val2) {
min1 = val2;
min_idx1 = dis2;
}
int min_idx2 = dis3;
uint8_t min2 = val3;
if(val3 > val4) {
min2 = val4;
min_idx2 = dis4;
}
uint8_t minval = min1;
*min_idx = min_idx1;
if(min1 > min2) {
minval = min2;
*min_idx = min_idx2;
}
return minval;
}
__inline__ __device__ uint8_t minu8_index8(int *min_idx, const uint8_t val1, const int dis, const uint8_t val2, const int dis2, const uint8_t val3, const int dis3, const uint8_t val4, const int dis4, const uint8_t val5, const int dis5, const uint8_t val6, const int dis6, const uint8_t val7, const int dis7, const uint8_t val8, const int dis8) {
int min_idx1, min_idx2;
uint8_t minval1, minval2;
minval1 = minu8_index4(&min_idx1, val1, dis, val2, dis2, val3, dis3, val4, dis4);
minval2 = minu8_index4(&min_idx2, val5, dis5, val6, dis6, val7, dis7, val8, dis8);
*min_idx = min_idx1;
uint8_t minval = minval1;
if(minval1 > minval2) {
*min_idx = min_idx2;
minval = minval2;
}
return minval;
}
__inline__ __device__ int warpReduceMinIndex2(int *val, int idx) {
for(int d = 1; d < WARP_SIZE; d *= 2) {
int tmp = shfl_xor_32(*val, d);
int tmp_idx = shfl_xor_32(idx, d);
if(*val > tmp) {
*val = tmp;
idx = tmp_idx;
}
}
return idx;
}
__inline__ __device__ int warpReduceMinIndex(int val, int idx) {
for(int d = 1; d < WARP_SIZE; d *= 2) {
int tmp = shfl_xor_32(val, d);
int tmp_idx = shfl_xor_32(idx, d);
if(val > tmp) {
val = tmp;
idx = tmp_idx;
}
}
return idx;
}
__inline__ __device__ int warpReduceMin(int val) {
val = min(val, shfl_xor_32(val, 1));
val = min(val, shfl_xor_32(val, 2));
val = min(val, shfl_xor_32(val, 4));
val = min(val, shfl_xor_32(val, 8));
val = min(val, shfl_xor_32(val, 16));
return val;
}
__inline__ __device__ int blockReduceMin(int val) {
static __shared__ int shared[WARP_SIZE]; // Shared mem for WARP_SIZE partial sums
const int lane = threadIdx.x % WARP_SIZE;
const int wid = threadIdx.x / WARP_SIZE;
val = warpReduceMin(val); // Each warp performs partial reduction
if (lane==0) shared[wid]=val; // Write reduced value to shared memory
__syncthreads(); // Wait for all partial reductions
//read from shared memory only if that warp existed
val = (threadIdx.x < blockDim.x / warpSize) ? shared[lane] : INT_MAX;
if (wid==0) val = warpReduceMin(val); //Final reduce within first warp
return val;
}
__inline__ __device__ int blockReduceMinIndex(int val, int idx) {
static __shared__ int shared_val[WARP_SIZE]; // Shared mem for WARP_SIZE partial mins
static __shared__ int shared_idx[WARP_SIZE]; // Shared mem for WARP_SIZE indexes
const int lane = threadIdx.x % WARP_SIZE;
const int wid = threadIdx.x / WARP_SIZE;
idx = warpReduceMinIndex2(&val, idx); // Each warp performs partial reduction
if (lane==0) {
shared_val[wid]=val;
shared_idx[wid]=idx;
}
__syncthreads(); // Wait for all partial reductions
//read from shared memory only if that warp existed
val = (threadIdx.x < blockDim.x / WARP_SIZE) ? shared_val[lane] : INT_MAX;
idx = (threadIdx.x < blockDim.x / WARP_SIZE) ? shared_idx[lane] : INT_MAX;
if (wid==0) {
idx = warpReduceMinIndex2(&val, idx); //Final reduce within first warp
}
return idx;
}
__inline__ __device__ bool blockAny(bool local_condition) {
__shared__ bool conditions[WARP_SIZE];
const int lane = threadIdx.x % WARP_SIZE;
const int wid = threadIdx.x / WARP_SIZE;
local_condition = __any_sync(0xffffffff, local_condition); // Each warp performs __any
if (lane==0) {
conditions[wid]=local_condition;
}
__syncthreads(); // Wait for all partial __any
//read from shared memory only if that warp existed
local_condition = (threadIdx.x < blockDim.x / WARP_SIZE) ? conditions[lane] : false;
if (wid==0) {
local_condition = __any_sync(0xffffffff, local_condition); //Final __any within first warp
}
return local_condition;
}
} //namespace sgm_gpu
#endif // SGM_GPU__UTIL_H_

16
flightlib/package.xml Normal file
View File

@@ -0,0 +1,16 @@
<?xml version="1.0"?>
<package format="2">
<name>flightlib</name>
<version>0.0.1</version>
<description>Flightmare: An Open Flexible Quadrotor</description>
<maintainer email="song@ifi.uzh.ch">Yunlong Song</maintainer>
<license>GNU GPL</license>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>catkin_simple</buildtool_depend>
<depend>gtest</depend>
</package>

471
flightlib/src/bridges/unity_bridge.cpp Normal file
View File

@@ -0,0 +1,471 @@
#include "flightlib/bridges/unity_bridge.hpp"
namespace flightlib {
// constructor
UnityBridge::UnityBridge()
: client_address_("tcp://*"),
pub_port_("10253"),
sub_port_("10254"),
num_frames_(0),
last_downloaded_utime_(0),
last_download_debug_utime_(0),
u_packet_latency_(0),
unity_ready_(false) {
// initialize connections upon creating unity bridge
initializeConnections();
generator_ = std::default_random_engine(random_device_());
}
bool UnityBridge::initializeConnections() {
logger_.info("Initializing ZMQ connection...");
// create and bind an upload socket
pub_.set(zmqpp::socket_option::send_high_water_mark, 6);
pub_.bind(client_address_ + ":" + pub_port_);
std::cout << "pub_port_" << pub_port_ << std::endl;
// create and bind a download_socket
sub_.set(zmqpp::socket_option::receive_high_water_mark, 6);
sub_.bind(client_address_ + ":" + sub_port_);
std::cout << "sub_port_" << sub_port_ << std::endl;
// subscribe all messages from ZMQ
sub_.subscribe("");
logger_.info("Initializing ZMQ connections done!");
return true;
}
bool UnityBridge::connectUnity(const SceneID scene_id) {
Scalar time_out_count = 0;
Scalar sleep_useconds = 0.2 * 1e5;
setScene(scene_id);
// try to connect unity
logger_.info("Trying to Connect Unity.");
std::cout << "[";
while (!unity_ready_) {
// if time out
if (time_out_count / 1e6 > unity_connection_time_out_) {
std::cout << "]" << std::endl;
logger_.warn(
"Unity Connection time out! Make sure that Unity Standalone "
"or Unity Editor is running the Flightmare.");
return false;
}
// initialize Scene settings
sendInitialSettings();
// check if setting is done
unity_ready_ = handleSettings();
// sleep
usleep(sleep_useconds);
// increase time out counter
time_out_count += sleep_useconds;
// print something
std::cout << ".";
std::cout.flush();
}
logger_.info("Flightmare Unity is connected.");
// wait 1 seconds. until to environment is fully loaded.
usleep(1 * 1e6);
// wait until point cloud is created.
// Unity is frozen as long as point cloud is created
// check if it's possible to send and receive a frame again and then continue
FrameID send_id = 1;
getRender(send_id);
FrameID receive_id = 0;
while (send_id != receive_id) {
handleOutput(receive_id);
}
logger_.info("Flightmare Unity is ready.");
return unity_ready_;
}
bool UnityBridge::disconnectUnity() {
unity_ready_ = false;
// create new message object
std::string client_address_dis_{"tcp://*"};
std::string pub_port_dis_{"10255"};
zmqpp::context context_dis_;
zmqpp::socket pub_dis_{context_dis_, zmqpp::socket_type::publish};
pub_dis_.set(zmqpp::socket_option::send_high_water_mark, 6);
pub_dis_.bind(client_address_dis_ + ":" + pub_port_dis_);
// wait until publisher is properly connected
usleep(1000000);
zmqpp::message msg_dis_;
printf("Disconnect from Unity!\n");
msg_dis_ << "DISCONNECT";
pub_dis_.send(msg_dis_, true);
FrameID send_id = 1;
getRender(send_id);
pub_.close();
sub_.close();
pub_dis_.close();
printf("Disconnected!\n");
return true;
}
bool UnityBridge::sendInitialSettings(void) {
// create new message object
zmqpp::message msg;
// add topic header
msg << "Pose";
// create JSON object for initial settings
json json_mesg = settings_;
msg << json_mesg.dump();
// send message without blocking
pub_.send(msg, true);
return true;
};
bool UnityBridge::handleSettings(void) {
// create new message object
zmqpp::message msg;
bool done = false;
// Unpack message metadata
if (sub_.receive(msg, true)) {
std::string metadata_string = msg.get(0);
// Parse metadata
if (json::parse(metadata_string).size() > 1) {
return false; // hack
}
done = json::parse(metadata_string).at("ready").get<bool>();
}
return done;
};
bool UnityBridge::getRender(const FrameID frame_id) {
pub_msg_.ntime = frame_id;
QuadState quad_state;
for (size_t idx = 0; idx < pub_msg_.vehicles.size(); idx++) {
unity_quadrotors_[idx]->getState(&quad_state);
// 传给unity的飞机位置 = 实际飞机 - 相机和飞机的位姿差, 使得图像渲染位置=飞机位置,使视野无飞机机身遮挡。请确保第0个camera是左目
Matrix<4, 4> cam_pose = rgb_cameras_[0]->getRelPose();
Vector<3> delta_pose = cam_pose.block<3, 1>(0, 3);
// printf("camera pose to body: %f, %f, %f \n",delta_pose(0),delta_pose(1),delta_pose(2));
pub_msg_.vehicles[idx].position = convertToDoubleVector(positionRos2Unity(quad_state.p - delta_pose));
pub_msg_.vehicles[idx].rotation = convertToDoubleVector(quaternionRos2Unity(quad_state.q()));
}
for (size_t idx = 0; idx < pub_msg_.objects.size(); idx++) {
std::shared_ptr<StaticObject> gate = static_objects_[idx];
pub_msg_.objects[idx].position = convertToDoubleVector(positionRos2Unity(gate->getPosition()));
pub_msg_.objects[idx].rotation = convertToDoubleVector(quaternionRos2Unity(gate->getQuaternion()));
}
// create new message object
zmqpp::message msg;
// add topic header
msg << "Pose";
// create JSON object for pose update and append
json json_msg = pub_msg_;
msg << json_msg.dump();
// send message without blocking
pub_.send(msg, true);
return true;
}
bool UnityBridge::setScene(const SceneID& scene_id) {
if (scene_id >= UnityScene::SceneNum) {
logger_.warn("Scene ID is not defined, cannot set scene.");
return false;
}
// logger_.info("Scene ID is set to %d.", scene_id);
settings_.scene_id = scene_id;
return true;
}
bool UnityBridge::addQuadrotor(std::shared_ptr<Quadrotor> quad) {
Vehicle_t vehicle_t;
// get quadrotor state
QuadState quad_state;
if (!quad->getState(&quad_state)) {
logger_.error("Cannot get Quadrotor state.");
return false;
}
vehicle_t.ID = "quadrotor" + std::to_string(settings_.vehicles.size());
vehicle_t.position = convertToDoubleVector(positionRos2Unity(quad_state.p));
vehicle_t.rotation = convertToDoubleVector(quaternionRos2Unity(quad_state.q()));
vehicle_t.size = convertToDoubleVector(scalarRos2Unity(quad->getSize()));
// get camera
std::vector<std::shared_ptr<RGBCamera>> rgb_cameras = quad->getCameras();
for (size_t cam_idx = 0; cam_idx < rgb_cameras.size(); cam_idx++) {
std::shared_ptr<RGBCamera> cam = rgb_cameras[cam_idx];
Camera_t camera_t;
camera_t.ID = vehicle_t.ID + "_" + std::to_string(cam_idx);
std::vector<Scalar> T_BC_Scalar = transformationRos2Unity(rgb_cameras[cam_idx]->getRelPose());
std::vector<double> T_BC_double(T_BC_Scalar.begin(), T_BC_Scalar.end());
camera_t.T_BC = T_BC_double;
camera_t.channels = rgb_cameras[cam_idx]->getChannels();
camera_t.width = rgb_cameras[cam_idx]->getWidth();
camera_t.height = rgb_cameras[cam_idx]->getHeight();
camera_t.fov = rgb_cameras[cam_idx]->getFOV();
camera_t.depth_scale = rgb_cameras[cam_idx]->getDepthScale();
camera_t.post_processing = rgb_cameras[cam_idx]->GetPostProcessing();
camera_t.is_depth = false;
camera_t.output_index = cam_idx;
vehicle_t.cameras.push_back(camera_t);
// add rgb_cameras
rgb_cameras_.push_back(rgb_cameras[cam_idx]);
}
unity_quadrotors_.push_back(quad);
settings_.vehicles.push_back(vehicle_t);
pub_msg_.vehicles.push_back(vehicle_t);
return true;
}
bool UnityBridge::addStaticObject(std::shared_ptr<StaticObject> static_object) {
Object_t object_t;
object_t.ID = static_object->getID();
object_t.prefab_ID = static_object->getPrefabID();
object_t.position = convertToDoubleVector(positionRos2Unity(static_object->getPosition()));
object_t.rotation = convertToDoubleVector(quaternionRos2Unity(static_object->getQuaternion()));
object_t.size = convertToDoubleVector(scalarRos2Unity(static_object->getSize()));
static_objects_.push_back(static_object);
settings_.objects.push_back(object_t);
pub_msg_.objects.push_back(object_t);
return true;
}
bool UnityBridge::handleOutput(FrameID& frameID) {
// create new message object
zmqpp::message msg;
sub_.receive(msg);
// unpack message metadata
std::string json_sub_msg = msg.get(0);
// parse metadata
SubMessage_t sub_msg = json::parse(json_sub_msg);
frameID = sub_msg.ntime;
size_t image_i = 1;
// ensureBufferIsAllocated(sub_msg);
for (size_t idx = 0; idx < settings_.vehicles.size(); idx++) {
// update vehicle collision flag
unity_quadrotors_[idx]->setCollision(sub_msg.sub_vehicles[idx].collision);
// feed image data to RGB camera
for (const auto& cam : settings_.vehicles[idx].cameras) {
// 1、RGB图-----------------------------------------
{
uint32_t image_len = cam.width * cam.height * cam.channels;
// Get raw image bytes from ZMQ message.
// WARNING: This is a zero-copy operation that also casts the input to an array of unit8_t. when the message is deleted, this pointer
// is also dereferenced.
const uint8_t* image_data;
msg.get(image_data, image_i);
image_i = image_i + 1;
// Pack image into cv::Mat
cv::Mat new_image = cv::Mat(cam.height, cam.width, CV_MAKETYPE(CV_8U, cam.channels));
memcpy(new_image.data, image_data, image_len);
// Flip image since OpenCV origin is upper left, but Unity's is lower left.
cv::flip(new_image, new_image, 0);
// Tell OpenCv that the input is RGB.
if (cam.channels == 3) {
cv::cvtColor(new_image, new_image, CV_RGB2BGR);
}
unity_quadrotors_[idx]->getCameras()[cam.output_index]->feedImageQueue(0, new_image);
}
// 之前Flightmare的layer_idx： 0 是RGB, 1是Depth, 2是Seg, 3是光流
// 现在的post_processing： 0 是RGB, 后面按设置打开的Denpth、Seg等排列
// 2、附加开启的图-------------------------------------------
for (size_t layer_idx = 0; layer_idx < cam.post_processing.size(); layer_idx++) {
if (cam.post_processing[layer_idx] == RGBCameraTypes::Depth) {
// depth
uint32_t image_len = cam.width * cam.height * 4;
// Get raw image bytes from ZMQ message.
// WARNING: This is a zero-copy operation that also casts the input to an array of unit8_t. when the message is deleted, this
// pointer is also dereferenced.
const uint8_t* image_data;
msg.get(image_data, image_i);
image_i = image_i + 1;
// Pack image into cv::Mat
cv::Mat new_image = cv::Mat(cam.height, cam.width, CV_32FC1);
memcpy(new_image.data, image_data, image_len);
// Flip image since OpenCV origin is upper left, but Unity's is lower left.
new_image = new_image * (1.f);
cv::flip(new_image, new_image, 0);
unity_quadrotors_[idx]->getCameras()[cam.output_index]->feedImageQueue(CameraLayer::DepthMap, new_image);
}
}
}
}
return true;
}
bool UnityBridge::spawnTrees(Ref<Vector<3>> bounding_box_, Ref<Vector<3>> bounding_box_origin_, Scalar avg_tree_spacing_) {
printf("Start Spawn Trees... \n");
rmTrees();
// 循环多次避免偶尔一次没render上，后面树再也无法生成
for (size_t i = 0; i < 3; i++)
refreshUnity(10086 + i);
TreeMessage_t tree_msg;
// compute the requested tree density for Poisson
Scalar density = 1.0 / (avg_tree_spacing_ * avg_tree_spacing_);
int num_trees = static_cast<int>(bounding_box_[0] * bounding_box_[1] * density);
// draw sample from poisson distribution
std::poisson_distribution<int> poisson_dist(num_trees);
tree_msg.density = static_cast<double>(poisson_dist(generator_));
printf("Tree Spacing is %f. \n", avg_tree_spacing_);
// generate random seed
std::uniform_int_distribution<int> distribution(1, 1 << 20);
tree_msg.seed = distribution(generator_);
tree_msg.bounding_origin[0] = bounding_box_origin_[0];
tree_msg.bounding_origin[1] = bounding_box_origin_[1];
tree_msg.bounding_area[0] = bounding_box_[0];
tree_msg.bounding_area[1] = bounding_box_[1];
bool spawned = placeTrees(tree_msg);
std::cout << "Tree Spawned" << std::endl;
return spawned;
}
void UnityBridge::generatePointcloud(const Ref<Vector<3>>& min_corner, const Ref<Vector<3>>& max_corner, int ply_idx, std::string path,
SceneID scene_id, Scalar pc_resolution_) {
printf("Start creating pointcloud... \n");
PointCloudMessage_t pcd_msg;
pcd_msg.scene_id = scene_id;
pcd_msg.bounding_box_scale =
std::vector<double>{(max_corner.x() - min_corner.x()), (max_corner.y() - min_corner.y()), (max_corner.z() - min_corner.z())};
printf("Scale pointcloud: [%.2f, %.2f, %.2f]\n", pcd_msg.bounding_box_scale.at(0), pcd_msg.bounding_box_scale.at(1),
pcd_msg.bounding_box_scale.at(2));
pcd_msg.bounding_box_origin = std::vector<double>{(max_corner.x() + min_corner.x()) / 2.0, (max_corner.y() + min_corner.y()) / 2.0,
(max_corner.z() + min_corner.z()) / 2.0};
printf("Origin pointcloud: [%.2f, %.2f, %.2f]\n", pcd_msg.bounding_box_origin.at(0), pcd_msg.bounding_box_origin.at(1),
pcd_msg.bounding_box_origin.at(2));
pcd_msg.path = path;
pcd_msg.file_name = "pointcloud-" + std::to_string(ply_idx);
pcd_msg.unity_ground_offset = 0.0;
pcd_msg.resolution_above_ground = pc_resolution_;
pcd_msg.resolution_below_ground = pc_resolution_;
std::cout << "Save file name: " << pcd_msg.path + pcd_msg.file_name + ".ply" << std::endl;
while (std::experimental::filesystem::exists(pcd_msg.path + pcd_msg.file_name + ".ply")) {
std::cout << "file already exists, delete! " << std::endl;
std::experimental::filesystem::remove(pcd_msg.path + pcd_msg.file_name + ".ply");
usleep(1 * 1e6);
}
std::cout << "Pointcloud Saving...";
pointCloudGenerator(pcd_msg);
// render Unity until point cloud exists
FrameID frameID = 10086;
while (!std::experimental::filesystem::exists(pcd_msg.path + pcd_msg.file_name + ".ply")) {
std::cout << ".";
std::cout.flush();
refreshUnity(frameID); // render, not usleep (BUG: Flightmare must render continuously to refresh the sense and generate pointcloud.)
frameID++;
}
usleep(5 * 1e6);
printf("Pointcloud saved!\n");
}
template<typename T>
std::vector<double> UnityBridge::convertToDoubleVector(const std::vector<T>& input) {
std::vector<double> output(input.size());
std::transform(input.begin(), input.end(), output.begin(), [](T value) { return static_cast<double>(value); });
return output;
}
void UnityBridge::refreshUnity(FrameID id = 10086) {
FrameID frameID_test = id;
getRender(frameID_test);
FrameID frameID_rt;
handleOutput(frameID_rt);
while (frameID_test != frameID_rt)
handleOutput(frameID_rt);
}
bool UnityBridge::placeTrees(TreeMessage_t& tree_msg) {
std::string client_address_{"tcp://*"};
std::string pub_tree_port_{"10255"};
zmqpp::context context_;
zmqpp::socket pub_tree_{context_, zmqpp::socket_type::publish};
pub_tree_.set(zmqpp::socket_option::send_high_water_mark, 6);
pub_tree_.bind(client_address_ + ":" + pub_tree_port_);
std::string sub_tree_port_{"10256"};
zmqpp::socket sub_tree_{context_, zmqpp::socket_type::subscribe};
sub_tree_.set(zmqpp::socket_option::receive_high_water_mark, 6);
sub_tree_.bind(client_address_ + ":" + sub_tree_port_);
sub_tree_.subscribe("PLACETREE");
// wait until publisher is properly connected
usleep(1000000);
zmqpp::message msg;
msg << "PLACETREE";
// check if seed is not initialized
if (tree_msg.seed == -1)
tree_msg.seed = rand();
json json_msg = tree_msg;
msg << json_msg.dump();
pub_tree_.send(msg, true);
pub_tree_.close();
double sleep_useconds = 0.2 * 1e5;
FrameID frameID = 10086;
// Wait until response received
while (true) {
if (sub_tree_.receive(msg, true)) {
break;
}
// render, not usleep (BUG: Flightmare must render continuously to refresh the sense and tree.)
refreshUnity(frameID);
frameID++;
}
sub_tree_.close();
return true;
}
void UnityBridge::rmTrees() {
std::string client_address_{"tcp://*"};
std::string pub_tree_port_{"10255"};
zmqpp::context context_;
zmqpp::socket pub_tree_{context_, zmqpp::socket_type::publish};
pub_tree_.set(zmqpp::socket_option::send_high_water_mark, 6);
pub_tree_.bind(client_address_ + ":" + pub_tree_port_);
// wait until publisher is properly connected
usleep(1000000);
zmqpp::message msg;
msg << "RMTREE";
pub_tree_.send(msg, true);
pub_tree_.close();
}
void UnityBridge::pointCloudGenerator(PointCloudMessage_t& pcd_msg) {
std::string client_address_{"tcp://*"};
std::string pub_pc_port_{"10255"};
zmqpp::context context_;
zmqpp::socket pub_pc_{context_, zmqpp::socket_type::publish};
pub_pc_.set(zmqpp::socket_option::send_high_water_mark, 6);
pub_pc_.bind(client_address_ + ":" + pub_pc_port_);
// wait until publisher is properly connected
usleep(1000000);
zmqpp::message msg;
msg << "PCD";
json json_msg = pcd_msg;
msg << json_msg.dump();
pub_pc_.send(msg, true);
pub_pc_.close();
}
} // namespace flightlib

29
flightlib/src/common/command.cpp Normal file
View File

@@ -0,0 +1,29 @@
#include "flightlib/common/command.hpp"
namespace flightlib {
Command::Command() {}
Command::Command(const Scalar t, const Scalar thrust, const Vector<3>& omega)
: t(t), collective_thrust(thrust), omega(omega) {}
Command::Command(const Scalar t, const Vector<4>& thrusts)
: t(t), thrusts(thrusts) {}
bool Command::valid() const {
return std::isfinite(t) &&
((std::isfinite(collective_thrust) && omega.allFinite()) ^
thrusts.allFinite());
}
bool Command::isSingleRotorThrusts() const {
return std::isfinite(t) && thrusts.allFinite();
}
bool Command::isRatesThrust() const {
return std::isfinite(t) && std::isfinite(collective_thrust) &&
omega.allFinite();
}
} // namespace flightlib

33
flightlib/src/common/integrator_base.cpp Normal file
View File

@@ -0,0 +1,33 @@
#include "flightlib/common/integrator_base.hpp"
namespace flightlib {
IntegratorBase::IntegratorBase(IntegratorBase::DynamicsFunction function,
const Scalar dt_max)
: dynamics_(function), dt_max_(dt_max) {}
bool IntegratorBase::integrate(const QuadState& initial,
QuadState* const final) const {
if (std::isnan(initial.t) || std::isnan(final->t)) return false;
if (initial.t >= final->t) return false;
return integrate(initial.x, final->t - initial.t, final->x);
}
bool IntegratorBase::integrate(const Ref<const Vector<>> initial,
const Scalar dt, Ref<Vector<>> final) const {
Scalar dt_remaining = dt;
Vector<> state = initial;
do {
const Scalar dt_this = std::min(dt_remaining, dt_max_);
if (!step(state, dt_this, final)) return false;
state = final;
dt_remaining -= dt_this;
} while (dt_remaining > 0.0);
return true;
}
Scalar IntegratorBase::dtMax() const { return dt_max_; }
} // namespace flightlib

15
flightlib/src/common/integrator_euler.cpp Normal file
View File

@@ -0,0 +1,15 @@
#include "flightlib/common/integrator_euler.hpp"
namespace flightlib {
bool IntegratorEuler::step(const Ref<const Vector<>> initial, const Scalar dt,
Ref<Vector<>> final) const {
Vector<> derivative(initial.rows());
if (!this->dynamics_(initial, derivative)) return false;
final = initial + dt * derivative;
return true;
}
} // namespace flightlib

34
flightlib/src/common/integrator_rk4.cpp Normal file
View File

@@ -0,0 +1,34 @@
#include "flightlib/common/integrator_rk4.hpp"
namespace flightlib {
bool IntegratorRK4::step(const Ref<const Vector<>> initial, const Scalar dt,
Ref<Vector<>> final) const {
static const Vector<4> rk4_sum_vec{1.0 / 6.0, 2.0 / 6.0, 2.0 / 6.0,
1.0 / 6.0};
Matrix<> k = Matrix<>::Zero(initial.rows(), 4);
final = initial;
// k_1
if (!this->dynamics_(final, k.col(0))) return false;
// k_2
final = initial + 0.5 * dt * k.col(0);
if (!this->dynamics_(final, k.col(1))) return false;
// k_3
final = initial + 0.5 * dt * k.col(1);
if (!this->dynamics_(final, k.col(2))) return false;
// k_4
final = initial + dt * k.col(2);
if (!this->dynamics_(final, k.col(3))) return false;
final = initial + dt * k * rk4_sum_vec;
return true;
}
} // namespace flightlib

63
flightlib/src/common/logger.cpp Normal file
View File

@@ -0,0 +1,63 @@
#include "flightlib/common/logger.hpp"
namespace flightlib {
Logger::Logger(const std::string& name, const bool color)
: sink_(std::cout.rdbuf()), colored_(color) {
name_ = "[" + name + "]";
if (name_.size() < NAME_PADDING)
name_ = name_ + std::string(NAME_PADDING - name_.size(), ' ');
else
name_ = name_ + " ";
sink_.precision(DEFAULT_PRECISION);
}
Logger::Logger(const std::string& name, const std::string& filename)
: Logger(name, false) {
if (!filename.empty()) {
std::filebuf* fbuf = new std::filebuf;
if (fbuf->open(filename, std::ios::out))
sink_.rdbuf(fbuf);
else
warn("Could not open file %s. Logging to console!", filename);
}
sink_.precision(DEFAULT_PRECISION);
}
Logger::~Logger() {}
inline std::streamsize Logger::precision(const std::streamsize n) {
return sink_.precision(n);
}
inline void Logger::scientific(const bool on) {
if (on)
sink_ << std::scientific;
else
sink_ << std::fixed;
}
void Logger::info(const std::string& message) const {
if (colored_)
sink_ << name_ << message << std::endl;
else
sink_ << name_ << INFO << message << std::endl;
}
void Logger::warn(const std::string& message) const {
if (colored_)
sink_ << YELLOW << name_ << message << RESET << std::endl;
else
sink_ << name_ << WARN << message << std::endl;
}
void Logger::error(const std::string& message) const {
if (colored_)
sink_ << RED << name_ << message << RESET << std::endl;
else
sink_ << name_ << ERROR << message << std::endl;
}
} // namespace flightlib

232
flightlib/src/common/math.cpp Executable file
View File

@@ -0,0 +1,232 @@
#include "flightlib/common/math.hpp"
#include "iostream"
namespace flightlib {
Matrix<3, 3> skew(const Vector<3>& v) { return (Matrix<3, 3>() << 0, -v.z(), v.y(), v.z(), 0, -v.x(), -v.y(), v.x(), 0).finished(); }
Matrix<4, 4> Q_left(const Quaternion& q) {
return (Matrix<4, 4>() << q.w(), -q.x(), -q.y(), -q.z(), q.x(), q.w(), -q.z(), q.y(), q.y(), q.z(), q.w(), -q.x(), q.z(), -q.y(), q.x(), q.w())
.finished();
}
Matrix<4, 4> Q_right(const Quaternion& q) {
return (Matrix<4, 4>() << q.w(), -q.x(), -q.y(), -q.z(), q.x(), q.w(), q.z(), -q.y(), q.y(), -q.z(), q.w(), q.x(), q.z(), q.y(), -q.x(), q.w())
.finished();
}
Matrix<4, 3> qFromQeJacobian(const Quaternion& q) {
return (Matrix<4, 3>() << -1.0 / q.w() * q.vec().transpose(), Matrix<3, 3>::Identity()).finished();
}
Matrix<4, 4> qConjugateJacobian() { return Matrix<4, 1>(1, -1, -1, -1).asDiagonal(); }
Matrix<3, 3> qeRotJacobian(const Quaternion& q, const Matrix<3, 1>& t) {
return 2.0 * (Matrix<3, 3>() << (q.y() + q.z() * q.x() / q.w()) * t.y() + (q.z() - q.y() * q.x() / q.w()) * t.z(), // entry 0,0
-2.0 * q.y() * t.x() + (q.x() + q.z() * q.y() / q.w()) * t.y() + (q.w() - q.y() * q.y() / q.w()) * t.z(), // entry 0,1
-2.0 * q.z() * t.x() + (-q.w() + q.z() * q.z() / q.w()) * t.y() + (q.x() - q.y() * q.z() / q.w()) * t.z(), // entry 0,2
(q.y() - q.z() * q.x() / q.w()) * t.x() + (-2.0 * q.x()) * t.y() + (-q.w() + q.x() * q.x() / q.w()) * t.z(), // entry 1,0
(q.x() - q.z() * q.y() / q.w()) * t.x() + (q.z() + q.x() * q.y() / q.w()) * t.z(), // entry 1,1
(q.w() - q.z() * q.z() / q.w()) * t.x() + (-2.0 * q.z()) * t.y() + (q.y() + q.x() * q.z() / q.w()) * t.z(), // entry 1,2
(q.z() + q.y() * q.x() / q.w()) * t.x() + (q.w() - q.x() * q.x() / q.w()) * t.y() + (-2.0 * q.x()) * t.z(), // entry 2,0
(-q.w() + q.y() * q.y() / q.w()) * t.x() + (q.z() - q.x() * q.y() / q.w()) * t.y() + (-2.0 * q.y()) * t.z(), // entry 2,1
(q.x() + q.y() * q.z() / q.w()) * t.x() + (q.y() - q.x() * q.z() / q.w()) * t.y() // entry 2,2
)
.finished();
}
Matrix<3, 3> qeInvRotJacobian(const Quaternion& q, const Matrix<3, 1>& t) {
return 2.0 * (Matrix<3, 3>() << (q.y() - q.z() * q.x() / q.w()) * t.y() + (q.z() + q.y() * q.x() / q.w()) * t.z(), // entry 0,0
-2.0 * q.y() * t.x() + (q.x() - q.z() * q.y() / q.w()) * t.y() - (q.w() - q.y() * q.y() / q.w()) * t.z(), // entry 0,1
-2.0 * q.z() * t.x() + (q.w() - q.z() * q.z() / q.w()) * t.y() + (q.x() + q.y() * q.z() / q.w()) * t.z(), // entry 0,2
(q.y() + q.z() * q.x() / q.w()) * t.x() - 2.0 * q.x() * t.y() + (q.w() - q.x() * q.x() / q.w()) * t.z(), // entry 1,0
(q.x() + q.z() * q.y() / q.w()) * t.x() + (q.z() - q.x() * q.y() / q.w()) * t.z(), // entry 1,1
-(q.w() - q.z() * q.z() / q.w()) * t.x() - 2.0 * q.z() * t.y() + (q.y() - q.x() * q.z() / q.w()) * t.z(), // entry 1,2
(q.z() - q.y() * q.x() / q.w()) * t.x() - (q.w() - q.x() * q.x() / q.w()) * t.y() - 2.0 * q.x() * t.z(), // entry 2,0
(q.w() - q.y() * q.y() / q.w()) * t.x() + (q.z() + q.x() * q.y() / q.w()) * t.y() - 2.0 * q.y() * t.z(), // entry 2,1
(q.x() - q.y() * q.z() / q.w()) * t.x() + (q.y() + q.x() * q.z() / q.w()) * t.y() // entry 2,2
)
.finished();
}
void matrixToTripletList(const SparseMatrix& matrix, std::vector<SparseTriplet>* const list, const int row_offset, const int col_offset) {
list->reserve((size_t)matrix.nonZeros() + list->size());
for (int i = 0; i < matrix.outerSize(); i++) {
for (typename SparseMatrix::InnerIterator it(matrix, i); it; ++it) {
list->emplace_back(it.row() + row_offset, it.col() + col_offset, it.value());
}
}
}
void matrixToTripletList(const Matrix<Dynamic, Dynamic>& matrix, std::vector<SparseTriplet>* const list, const int row_offset, const int col_offset) {
const SparseMatrix sparse = matrix.sparseView();
matrixToTripletList(sparse, list, row_offset, col_offset);
}
void insert(const SparseMatrix& from, SparseMatrix* const into, const int row_offset, const int col_offset) {
std::vector<SparseTriplet> v;
matrixToTripletList(*into, &v);
matrixToTripletList(from, &v, row_offset, col_offset);
into->setFromTriplets(v.begin(), v.end(), [](const Scalar& older, const Scalar& newer) { return newer; });
}
void insert(const Matrix<>& from, SparseMatrix* const into, const int row_offset, const int col_offset) {
const SparseMatrix from_sparse = from.sparseView();
insert(from_sparse, into, row_offset, col_offset);
}
inline void insert(const Matrix<>& from, Matrix<>* const into, const int row_offset, const int col_offset) {
into->block(row_offset, col_offset, from.rows(), from.cols()) = from;
}
void quaternionToEuler(const Quaternion& quat, Ref<Vector<3>> euler) {
euler.x() = std::atan2(2 * quat.w() * quat.x() + 2 * quat.y() * quat.z(),
quat.w() * quat.w() - quat.x() * quat.x() - quat.y() * quat.y() + quat.z() * quat.z());
euler.y() = -std::asin(2 * quat.x() * quat.z() - 2 * quat.w() * quat.y());
euler.z() = std::atan2(2 * quat.w() * quat.z() + 2 * quat.x() * quat.y(),
quat.w() * quat.w() + quat.x() * quat.x() - quat.y() * quat.y() - quat.z() * quat.z());
}
std::vector<Scalar> transformationRos2Unity(const Matrix<4, 4>& ros_tran_mat) {
/// [ Transformation Matrix ] from ROS coordinate system (right hand)
/// to Unity coordinate system (left hand)
Matrix<4, 4> tran_mat = Matrix<4, 4>::Zero();
tran_mat(0, 0) = 1.0;
tran_mat(1, 2) = 1.0;
tran_mat(2, 1) = 1.0;
tran_mat(3, 3) = 1.0;
//
Matrix<4, 4> unity_tran_mat = tran_mat * ros_tran_mat * tran_mat.transpose();
// std::vector<Scalar> unity_tran_mat_vec(
// unity_tran_mat.data(),
// unity_tran_mat.data() + unity_tran_mat.rows() * unity_tran_mat.cols());
std::vector<Scalar> tran_unity;
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
tran_unity.push_back(unity_tran_mat(i, j));
}
}
return tran_unity;
}
std::vector<Scalar> quaternionRos2Unity(const Quaternion& ros_quat) {
/// [ Quaternion ] from ROS coordinate system (right hand)
/// to Unity coordinate system (left hand)
Matrix<3, 3> rot_mat = Matrix<3, 3>::Zero();
rot_mat(0, 0) = 1.0;
rot_mat(1, 2) = 1.0;
rot_mat(2, 1) = 1.0;
//
Matrix<3, 3> unity_rot_mat = rot_mat * ros_quat.toRotationMatrix() * rot_mat.transpose();
Quaternion unity_quat(unity_rot_mat);
std::vector<Scalar> unity_quat_vec{unity_quat.x(), unity_quat.y(), unity_quat.z(), unity_quat.w()};
return unity_quat_vec;
}
std::vector<Scalar> positionRos2Unity(const Vector<3>& ros_pos_vec) {
/// [ Position Vector ] from ROS coordinate system (right hand)
/// to Unity coordinate system (left hand)
std::vector<Scalar> unity_position{ros_pos_vec(0), ros_pos_vec(2), ros_pos_vec(1)};
return unity_position;
}
std::vector<Scalar> scalarRos2Unity(const Vector<3>& ros_scalar) {
/// [ Object Scalar Vector ] from ROS coordinate system (right hand)
/// to Unity coordinate system (left hand)
std::vector<Scalar> unity_scalar{ros_scalar(0), ros_scalar(2), ros_scalar(1)};
return unity_scalar;
}
// rpy顺序
void get_euler_from_R(Vector<3>& e, const Matrix<3, 3>& R) {
float phi = atan2(R(2, 1), R(2, 2));
float theta = asin(-R(2, 0));
float psi = atan2(R(1, 0), R(0, 0));
float pi = M_PI;
if (fabs(theta - pi / 2.0f) < 1.0e-3) {
phi = 0.0f;
psi = atan2(R(1, 2), R(0, 2));
} else if (fabs(theta + pi / 2.0f) < 1.0e-3) {
phi = 0.0f;
psi = atan2(-R(1, 2), -R(0, 2));
}
e(0) = phi;
e(1) = theta;
e(2) = psi;
}
double wrapMinusPiToPi(const double angle) {
if (angle >= -M_PIl && angle <= M_PIl) {
return angle;
}
double wrapped_angle = angle + M_PIl;
wrapped_angle = wrapZeroToTwoPi(wrapped_angle);
wrapped_angle -= M_PIl;
return wrapped_angle;
}
double wrapZeroToTwoPi(const double angle) {
if (angle >= 0.0 && angle <= 2.0 * M_PIl) {
return angle;
}
double wrapped_angle = fmod(angle, 2.0 * M_PIl);
if (wrapped_angle < 0.0) {
wrapped_angle += 2.0 * M_PIl;
}
return wrapped_angle;
}
// calculate and constrain the yaw angle per sim_t
float calculate_yaw(float yaw_cur, float yaw_ref, float sim_t) // yaw [-pi,pi]
{
float PI = 3.1415926;
float YAW_DOT_MAX_PER_SEC = 0.3 * PI;
float max_yaw_change = YAW_DOT_MAX_PER_SEC * sim_t;
float yaw_temp = yaw_ref;
float last_yaw_ = yaw_cur;
float yaw = 0;
if (yaw_temp - last_yaw_ > PI) {
if (yaw_temp - last_yaw_ - 2 * PI < -max_yaw_change) {
yaw = last_yaw_ - max_yaw_change;
if (yaw < -PI)
yaw += 2 * PI;
} else {
yaw = yaw_temp;
}
} else if (yaw_temp - last_yaw_ < -PI) {
if (yaw_temp - last_yaw_ + 2 * PI > max_yaw_change) {
yaw = last_yaw_ + max_yaw_change;
if (yaw > PI)
yaw -= 2 * PI;
} else {
yaw = yaw_temp;
}
} else {
if (yaw_temp - last_yaw_ < -max_yaw_change) {
yaw = last_yaw_ - max_yaw_change;
if (yaw < -PI)
yaw += 2 * PI;
} else if (yaw_temp - last_yaw_ > max_yaw_change) {
yaw = last_yaw_ + max_yaw_change;
if (yaw > PI)
yaw -= 2 * PI;
} else {
yaw = yaw_temp;
}
}
return yaw;
}
} // namespace flightlib

15
flightlib/src/common/parameter_base.cpp Normal file
View File

@@ -0,0 +1,15 @@
#include "flightlib/common/parameter_base.hpp"
namespace flightlib {
ParameterBase::ParameterBase() {}
ParameterBase::ParameterBase(const YAML::Node& cfg_node)
: cfg_node_(cfg_node) {}
ParameterBase::ParameterBase(const std::string& cfg_path)
: cfg_node_(YAML::Node(cfg_path)) {}
ParameterBase::~ParameterBase() {}
} // namespace flightlib

41
flightlib/src/common/pend_state.cpp Normal file
View File

@@ -0,0 +1,41 @@
#include "flightlib/common/pend_state.hpp"
namespace flightlib {
PendState::PendState() {}
PendState::PendState(const Vector<IDX::SIZE>& x, const Scalar t) : x(x), t(t) {}
PendState::PendState(const PendState& state) : x(state.x), t(state.t) {}
PendState::~PendState() {}
Quaternion PendState::q() const {
return Quaternion(x(ATTW), x(ATTX), x(ATTY), x(ATTZ));
}
void PendState::q(const Quaternion quaternion) {
x(IDX::ATTW) = quaternion.w();
x(IDX::ATTX) = quaternion.x();
x(IDX::ATTY) = quaternion.y();
x(IDX::ATTZ) = quaternion.z();
}
Matrix<3, 3> PendState::R() const {
return Quaternion(x(ATTW), x(ATTX), x(ATTY), x(ATTZ)).toRotationMatrix();
}
void PendState::setZero() {
t = 0.0;
x.setZero();
x(ATTW) = 1.0;
}
std::ostream& operator<<(std::ostream& os, const PendState& state) {
os.precision(3);
os << "State at " << state.t << "s: [" << state.x.transpose() << "]";
os.precision();
return os;
}
} // namespace flightlib

41
flightlib/src/common/quad_state.cpp Normal file
View File

@@ -0,0 +1,41 @@
#include "flightlib/common/quad_state.hpp"
namespace flightlib {
QuadState::QuadState() {}
QuadState::QuadState(const Vector<IDX::SIZE>& x, const Scalar t) : x(x), t(t) {}
QuadState::QuadState(const QuadState& state) : x(state.x), t(state.t) {}
QuadState::~QuadState() {}
Quaternion QuadState::q() const {
return Quaternion(x(ATTW), x(ATTX), x(ATTY), x(ATTZ));
}
void QuadState::q(const Quaternion quaternion) {
x(IDX::ATTW) = quaternion.w();
x(IDX::ATTX) = quaternion.x();
x(IDX::ATTY) = quaternion.y();
x(IDX::ATTZ) = quaternion.z();
}
Matrix<3, 3> QuadState::R() const {
return Quaternion(x(ATTW), x(ATTX), x(ATTY), x(ATTZ)).toRotationMatrix();
}
void QuadState::setZero() {
t = 0.0;
x.setZero();
x(ATTW) = 1.0;
}
std::ostream& operator<<(std::ostream& os, const QuadState& state) {
os.precision(3);
os << "State at " << state.t << "s: [" << state.x.transpose() << "]";
os.precision();
return os;
}
} // namespace flightlib

117
flightlib/src/common/timer.cpp Normal file
View File

@@ -0,0 +1,117 @@
#include "flightlib/common/timer.hpp"
#include <cmath>
#include <limits>
namespace flightlib {
Timer::Timer(const std::string name, const std::string module)
: name_(name),
module_(module),
timing_mean_(0.0),
timing_last_(0.0),
timing_S_(0.0),
timing_min_(std::numeric_limits<Scalar>::max()),
timing_max_(0.0),
n_samples_(0) {}
Timer::Timer(const Timer& other)
: name_(other.name_),
module_(other.module_),
t_start_(other.t_start_),
timing_mean_(other.timing_mean_),
timing_last_(other.timing_last_),
timing_S_(other.timing_S_),
timing_min_(other.timing_min_),
timing_max_(other.timing_max_),
n_samples_(other.n_samples_) {}
void Timer::tic() { t_start_ = std::chrono::high_resolution_clock::now(); }
Scalar Timer::toc() {
// Calculate timing.
const TimePoint t_end = std::chrono::high_resolution_clock::now();
timing_last_ = 1e-9 * std::chrono::duration_cast<std::chrono::nanoseconds>(
t_end - t_start_)
.count();
n_samples_++;
// Set timing, filter if already initialized.
if (timing_mean_ <= 0.0) {
timing_mean_ = timing_last_;
} else {
const Scalar timing_mean_prev = timing_mean_;
timing_mean_ =
timing_mean_prev + (timing_last_ - timing_mean_prev) / n_samples_;
timing_S_ = timing_S_ + (timing_last_ - timing_mean_prev) *
(timing_last_ - timing_mean_);
}
timing_min_ = (timing_last_ < timing_min_) ? timing_last_ : timing_min_;
timing_max_ = (timing_last_ > timing_max_) ? timing_last_ : timing_max_;
t_start_ = t_end;
return timing_mean_;
}
Scalar Timer::operator()() const { return timing_mean_; }
Scalar Timer::mean() const { return timing_mean_; }
Scalar Timer::last() const { return timing_last_; }
Scalar Timer::min() const { return timing_min_; }
Scalar Timer::max() const { return timing_max_; }
Scalar Timer::std() const { return std::sqrt(timing_S_ / n_samples_); }
int Timer::count() const { return n_samples_; }
void Timer::reset() {
n_samples_ = 0u;
t_start_ = TimePoint();
timing_mean_ = 0.0;
timing_last_ = 0.0;
timing_S_ = 0.0;
timing_min_ = std::numeric_limits<Scalar>::max();
timing_max_ = 0.0;
}
void Timer::print() const { std::cout << *this; }
std::ostream& operator<<(std::ostream& os, const Timer& timer) {
if (!timer.module_.empty()) os << "[" << timer.module_ << "] ";
if (timer.n_samples_ < 1) {
os << "Timing " << timer.name_ << " has no call yet." << std::endl;
return os;
}
const std::streamsize prec = os.precision();
os.precision(3);
os << "Timing " << timer.name_ << " in " << timer.n_samples_ << " calls"
<< std::endl;
if (!timer.module_.empty()) os << "[" << timer.module_ << "] ";
os << "mean|std: " << 1000 * timer.timing_mean_ << " | "
<< 1000 * timer.timing_S_ << " ms "
<< "[min|max: " << 1000 * timer.timing_min_ << " | "
<< 1000 * timer.timing_max_ << " ms]" << std::endl;
os.precision(prec);
return os;
}
ScopedTimer::ScopedTimer(const std::string name, const std::string module)
: Timer(name, module) {
this->tic();
}
ScopedTimer::~ScopedTimer() {
this->toc();
this->print();
}
} // namespace flightlib

9
flightlib/src/dynamics/dynamics_base.cpp Normal file
View File

@@ -0,0 +1,9 @@
#include "flightlib/dynamics/dynamics_base.hpp"
namespace flightlib {
DynamicsBase::DynamicsBase() {}
DynamicsBase::~DynamicsBase() {}
} // namespace flightlib

228
flightlib/src/dynamics/quadrotor_dynamics.cpp Normal file
View File

@@ -0,0 +1,228 @@
#include "flightlib/dynamics/quadrotor_dynamics.hpp"
namespace flightlib {
QuadrotorDynamics::QuadrotorDynamics(const Scalar mass, const Scalar arm_l)
: mass_(mass),
arm_l_(arm_l),
t_BM_(
arm_l * sqrt(0.5) *
(Matrix<3, 4>() << 1, -1, -1, 1, -1, -1, 1, 1, 0, 0, 0, 0).finished()),
J_(mass / 12.0 * arm_l * arm_l * Vector<3>(4.5, 4.5, 7).asDiagonal()),
J_inv_(J_.inverse()),
motor_omega_min_(150.0),
motor_omega_max_(2000.0),
motor_tau_inv_(1.0 / 0.05),
thrust_map_(1.3298253500372892e-06, 0.0038360810526746033,
-1.7689986848125325),
kappa_(0.016),
thrust_min_(0.0),
thrust_max_(motor_omega_max_ * motor_omega_max_ * thrust_map_(0) +
motor_omega_max_ * thrust_map_(1) + thrust_map_(2)),
collective_thrust_min_(4.0 * thrust_min_ / mass_),
collective_thrust_max_(4.0 * thrust_max_ / mass_),
omega_max_(6.0, 6.0, 2.0) {}
QuadrotorDynamics::~QuadrotorDynamics() {}
bool QuadrotorDynamics::dState(const QuadState& state,
QuadState* dstate) const {
return dState(state.x, dstate->x);
}
bool QuadrotorDynamics::dState(const Ref<const Vector<QuadState::SIZE>> state,
Ref<Vector<QuadState::SIZE>> dstate) const {
if (!state.segment<QS::NDYM>(0).allFinite()) return false;
dstate.setZero();
//
const Vector<3> omega(state(QS::OMEX), state(QS::OMEY), state(QS::OMEZ));
const Quaternion q_omega(0, omega.x(), omega.y(), omega.z());
const Vector<3> body_torque = state.segment<QS::NTAU>(QS::TAU);
// linear velocity = dx / dt
dstate.segment<QS::NPOS>(QS::POS) = state.segment<QS::NVEL>(QS::VEL);
// differentiate quaternion = dq / dt
dstate.segment<QS::NATT>(QS::ATT) =
0.5 * Q_right(q_omega) * state.segment<QS::NATT>(QS::ATT);
// linear acceleration = dv / dt
dstate.segment<QS::NVEL>(QS::VEL) = state.segment<QS::NACC>(QS::ACC);
// angular accleration = domega / dt
dstate.segment<QS::NOME>(QS::OME) =
J_inv_ * (body_torque - omega.cross(J_ * omega));
//
return true;
}
QuadrotorDynamics::DynamicsFunction QuadrotorDynamics::getDynamicsFunction()
const {
return std::bind(
static_cast<bool (QuadrotorDynamics::*)(const Ref<const Vector<QS::SIZE>>,
Ref<Vector<QS::SIZE>>) const>(
&QuadrotorDynamics::dState),
this, std::placeholders::_1, std::placeholders::_2);
}
bool QuadrotorDynamics::valid() const {
bool check = true;
check &= mass_ > 0.0;
check &= mass_ < 100.0; // limit maximum mass
check &= t_BM_.allFinite();
check &= J_.allFinite();
check &= J_inv_.allFinite();
check &= motor_omega_min_ >= 0.0;
check &= (motor_omega_max_ > motor_omega_min_);
check &= motor_tau_inv_ > 0.0;
check &= thrust_map_.allFinite();
check &= kappa_ > 0.0;
check &= thrust_min_ >= 0.0;
check &= (thrust_max_ > thrust_min_);
check &= (omega_max_.array() > 0).all();
return check;
}
Vector<4> QuadrotorDynamics::clampThrust(const Vector<4> thrusts) const {
return thrusts.cwiseMax(thrust_min_).cwiseMin(thrust_max_);
}
Scalar QuadrotorDynamics::clampThrust(const Scalar thrust) const {
return std::clamp(thrust, thrust_min_, thrust_max_);
}
Scalar QuadrotorDynamics::clampCollectiveThrust(const Scalar thrust) const {
return std::clamp(thrust, collective_thrust_min_, collective_thrust_max_);
}
Vector<4> QuadrotorDynamics::clampMotorOmega(const Vector<4>& omega) const {
return omega.cwiseMax(motor_omega_min_).cwiseMin(motor_omega_max_);
}
Vector<3> QuadrotorDynamics::clampBodyrates(const Vector<3>& omega) const {
return omega.cwiseMax(-omega_max_).cwiseMin(omega_max_);
}
Vector<4> QuadrotorDynamics::motorOmegaToThrust(const Vector<4>& omega) const {
const Matrix<4, 3> omega_poly =
(Matrix<4, 3>() << omega.cwiseProduct(omega), omega, Vector<4>::Ones())
.finished();
return omega_poly * thrust_map_;
}
Vector<4> QuadrotorDynamics::motorThrustToOmega(
const Vector<4>& thrusts) const {
const Scalar scale = 1.0 / (2.0 * thrust_map_[0]);
const Scalar offset = -thrust_map_[1] * scale;
const Array<4, 1> root =
(std::pow(thrust_map_[1], 2) -
4.0 * thrust_map_[0] * (thrust_map_[2] - thrusts.array()))
.sqrt();
return offset + scale * root;
}
Matrix<4, 4> QuadrotorDynamics::getAllocationMatrix() const {
return (Matrix<4, 4>() << Vector<4>::Ones().transpose(), t_BM_.topRows<2>(),
kappa_ * Vector<4>(1, -1, 1, -1).transpose())
.finished();
}
bool QuadrotorDynamics::setMass(const Scalar mass) {
if (mass < 0.0 || mass >= 100.) {
return false;
}
mass_ = mass;
// update inertial matrix and its inverse
updateInertiaMarix();
return true;
}
bool QuadrotorDynamics::setArmLength(const Scalar arm_length) {
if (arm_length < 0.0) {
return false;
}
arm_l_ = arm_length;
// update torque mapping matrix, inertial matrix and its inverse
updateInertiaMarix();
return true;
}
bool QuadrotorDynamics::setMotortauInv(const Scalar tau_inv) {
if (tau_inv < 1.0) {
return false;
}
motor_tau_inv_ = tau_inv;
return true;
}
bool QuadrotorDynamics::updateParams(const YAML::Node& params) {
if (params["quadrotor_dynamics"]) {
// load parameters from a yaml configuration file
mass_ = params["quadrotor_dynamics"]["mass"].as<Scalar>();
arm_l_ = params["quadrotor_dynamics"]["arm_l"].as<Scalar>();
motor_omega_min_ =
params["quadrotor_dynamics"]["motor_omega_min"].as<Scalar>();
motor_omega_max_ =
params["quadrotor_dynamics"]["motor_omega_max"].as<Scalar>();
motor_tau_inv_ =
(1.0 / params["quadrotor_dynamics"]["motor_tau"].as<Scalar>());
std::vector<Scalar> thrust_map;
thrust_map =
params["quadrotor_dynamics"]["thrust_map"].as<std::vector<Scalar>>();
thrust_map_ = Map<Vector<3>>(thrust_map.data());
kappa_ = params["quadrotor_dynamics"]["kappa"].as<Scalar>();
std::vector<Scalar> omega_max;
omega_max =
params["quadrotor_dynamics"]["omega_max"].as<std::vector<Scalar>>();
omega_max_ = Map<Vector<3>>(omega_max.data());
// update relevant variables
updateInertiaMarix();
return valid();
} else {
return false;
}
}
bool QuadrotorDynamics::updateInertiaMarix() {
if (!valid()) return false;
t_BM_ = arm_l_ * sqrt(0.5) *
(Matrix<3, 4>() << 1, -1, -1, 1, -1, -1, 1, 1, 0, 0, 0, 0).finished();
J_ = mass_ / 12.0 * arm_l_ * arm_l_ * Vector<3>(4.5, 4.5, 7).asDiagonal();
J_inv_ = J_.inverse();
return true;
}
std::ostream& operator<<(std::ostream& os, const QuadrotorDynamics& quad) {
os.precision(3);
os << "Quadrotor Dynamics:\n"
<< "mass = [" << quad.mass_ << "]\n"
<< "arm_l = [" << quad.arm_l_ << "]\n"
<< "t_BM = [" << quad.t_BM_.row(0) << "]\n"
<< " [" << quad.t_BM_.row(1) << "]\n"
<< " [" << quad.t_BM_.row(2) << "]\n"
<< "inertia = [" << quad.J_.row(0) << "]\n"
<< " [" << quad.J_.row(1) << "]\n"
<< " [" << quad.J_.row(2) << "]\n"
<< "motor_omega_min = [" << quad.motor_omega_min_ << "]\n"
<< "motor_omega_max = [" << quad.motor_omega_max_ << "]\n"
<< "motor_tau_inv = [" << quad.motor_tau_inv_ << "]\n"
<< "thrust_map = [" << quad.thrust_map_.transpose() << "]\n"
<< "kappa = [" << quad.kappa_ << "]\n"
<< "thrust_min = [" << quad.thrust_min_ << "]\n"
<< "thrust_max = [" << quad.thrust_max_ << "]\n"
<< "omega_max = [" << quad.omega_max_.transpose() << "]"
<< std::endl;
os.precision();
return os;
}
} // namespace flightlib

481
flightlib/src/envs/quadrotor_env.cpp Executable file
View File

@@ -0,0 +1,481 @@
#include "flightlib/envs/quadrotor_env.hpp"
namespace flightlib {
QuadrotorEnv::QuadrotorEnv() : QuadrotorEnv(getenv("FLIGHTMARE_PATH") + std::string("/flightlib/configs/quadrotor_env.yaml")) {}
QuadrotorEnv::QuadrotorEnv(const std::string &cfg_path) : EnvBase() {
// load configuration file
YAML::Node cfg_ = YAML::LoadFile(cfg_path);
loadParam(cfg_);
quadrotor_ptr_ = std::make_shared<Quadrotor>();
// 1、define a bounding box (z is defined manually, different from spawn box)
// x_min, x_max, y_min, y_max, z_min, z_max
world_box_ << center_(0) - 0.5 * scale_(0), center_(0) + 0.5 * scale_(0), center_(1) - 0.5 * scale_(1), center_(1) + 0.5 * scale_(1), 0.0, 5.0;
if (!quadrotor_ptr_->setWorldBox(world_box_)) {
logger_.error("cannot set wolrd box");
};
// 2、define input and output dimension for the environment
obs_dim_ = kNObs;
act_dim_ = kNAct;
rew_dim_ = 1;
// 3、define planner
traj_opt_bridge = new TrajOptimizationBridge();
// 5、add camera
sgm_.reset(new sgm_gpu::SgmGpu(width_, height_));
if (!configCamera(cfg_)) {
logger_.error("Cannot config RGB Camera. Something wrong with the config file");
}
Vector<3> quad_size(0.01, 0.01, 0.01);
quadrotor_ptr_->setSize(quad_size);
is_collision_ = false;
steps_ = 0;
}
QuadrotorEnv::~QuadrotorEnv() { delete traj_opt_bridge; }
bool QuadrotorEnv::reset(Ref<Vector<>> obs, const bool random) {
quad_state_.setZero();
quad_act_.setZero();
is_collision_ = false;
steps_ = 0;
nearest_obstacle = 10;
// Dagger Training
if (random && !collect_data_) {
// 1.reset position.
do {
is_collision_ = false;
quad_state_.x(QS::POSX) = 0.40 * scale_(0) * uniform_dist_(random_gen_) + center_(0);
quad_state_.x(QS::POSY) = 0.40 * scale_(1) * uniform_dist_(random_gen_) + center_(1);
quad_state_.x(QS::POSZ) = 0.20 * scale_(2) * uniform_dist_(random_gen_) + center_(2);
collisionCheck(1.5);
} while (is_collision_);
// 2.reset orientation
float roll = 0.0;
float pitch = 0.0;
float yaw = 3.14159 * uniform_dist_(random_gen_);
Eigen::Quaternionf q_;
q_ = Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX());
quad_state_.q(q_);
}
// Offline Data Collection
if (collect_data_) {
// 1.reset position.
do {
is_collision_ = false;
quad_state_.x(QS::POSX) = 0.5 * scale_(0) * uniform_dist_(random_gen_) + center_(0);
quad_state_.x(QS::POSY) = 0.5 * scale_(1) * uniform_dist_(random_gen_) + center_(1);
quad_state_.x(QS::POSZ) = 0.5 * scale_(2) * uniform_dist_(random_gen_) + center_(2);
collisionCheck(0.5);
} while (is_collision_);
// 2.reset orientation
float roll = (norm_dist_(random_gen_) * sqrt(roll_var_)) + 0.0;
float pitch = (norm_dist_(random_gen_) * sqrt(pitch_var_)) + 0.0;
float yaw = 3.14159 * uniform_dist_(random_gen_);
Eigen::Quaternionf q_;
q_ = Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX());
quad_state_.q(q_);
}
// reset quadrotor with random states
quadrotor_ptr_->reset(quad_state_);
// Currently, since there is no controller, the desired state is equal to the actual state.
desired_p_ = Eigen::Vector3f(quad_state_.p(0), quad_state_.p(1), quad_state_.p(2));
desired_v_ = Eigen::Vector3f(quad_state_.v(0), quad_state_.v(1), quad_state_.v(2));
desired_a_ = Eigen::Vector3f(quad_state_.a(0), quad_state_.a(1), quad_state_.a(2));
// obtain observations
getObs(obs);
return true;
}
void QuadrotorEnv::setState(ConstRef<Vector<>> state) {
if (state.rows() != 13) {
std::cout << "ERROR: state must be 13 dim (P_xyz, V_xyz, A_xyz, Q_wxyz)" << std::endl;
return;
}
quad_state_.setZero();
quad_state_.x(QS::POSX) = state(0);
quad_state_.x(QS::POSY) = state(1);
quad_state_.x(QS::POSZ) = state(2);
quad_state_.x(QS::VELX) = state(3);
quad_state_.x(QS::VELY) = state(4);
quad_state_.x(QS::VELZ) = state(5);
quad_state_.x(QS::ACCX) = state(6);
quad_state_.x(QS::ACCY) = state(7);
quad_state_.x(QS::ACCZ) = state(8);
quad_state_.x(QS::ATTW) = state(9);
quad_state_.x(QS::ATTX) = state(10);
quad_state_.x(QS::ATTY) = state(11);
quad_state_.x(QS::ATTZ) = state(12);
quadrotor_ptr_->reset(quad_state_);
}
void QuadrotorEnv::setGoal(ConstRef<Vector<>> goal) {
if (goal.rows() != 3) {
std::cout << "ERROR: goal must be 3 dim (xyz)" << std::endl;
return;
}
goal_ = goal;
}
bool QuadrotorEnv::getObs(Ref<Vector<>> obs) {
// The actual position.
Eigen::Vector3f Pw(quad_state_.p(0), quad_state_.p(1), quad_state_.p(2));
Eigen::Quaternionf Qw = quad_state_.q();
// The desired state, same with the real flight.
Eigen::Matrix3f Rwb = quad_state_.R();
Eigen::Vector3f Vw(desired_v_(0), desired_v_(1), desired_v_(2));
Eigen::Vector3f Vb = Rwb.inverse() * Vw;
Eigen::Vector3f Aw(desired_a_(0), desired_a_(1), desired_a_(2));
Eigen::Vector3f Ab = Rwb.inverse() * Aw;
// Observation: p, q_wxyz in the world frame (for training); v, a in the body frame (for testing).
quad_obs_ << Pw(0), Pw(1), Pw(2), Vb(0), Vb(1), Vb(2), Ab(0), Ab(1), Ab(2), Qw.w(), Qw.x(), Qw.y(), Qw.z();
obs.segment<kNObs>(0) = quad_obs_;
return true;
}
bool QuadrotorEnv::getDepthImage(Ref<DepthImgVector<>> depth_img) {
if (!rgb_camera_left || !rgb_camera_left->getEnabledLayers()[1]) {
logger_.error("No RGB Camera or depth map is not enabled. Cannot retrieve depth images.");
return false;
}
rgb_camera_left->getDepthMap(depth_img_);
depth_img = Map<DepthImgVector<>>((float_t *)depth_img_.data, depth_img_.rows * depth_img_.cols);
return true;
}
bool QuadrotorEnv::getStereoImage(Ref<DepthImgVector<>> stereo_img) {
if (!rgb_camera_left || !rgb_camera_right) {
logger_.error("No Stereo Camera enabled. Cannot retrieve depth map.");
return false;
}
cv::Mat left_img, right_img;
rgb_camera_left->getRGBImage(left_img);
rgb_camera_right->getRGBImage(right_img);
// compute disparity image
cv::Mat stereo_(height_, width_, CV_32FC1);
computeDepthImage(left_img, right_img, &stereo_);
// fix the nan of stereo by gt depth (make it closer to RealSense 435, as the depth from 435 is better than from SGM directly)
if (rgb_camera_left->getEnabledLayers()[1]) {
if (rgb_camera_left->getDepthMap(depth_img_)) {
cv::Mat mask, mask1, mask2;
cv::compare(stereo_, 0, mask1, cv::CMP_EQ); // 将 A 中为 0 的位置置为 255，其余位置置为 0
cv::compare(stereo_, 20, mask2, cv::CMP_GT); // 将 A 中大于 20 的位置置为 255，其余位置置为 0
mask = mask1 | mask2; // 将两个掩码进行逻辑或操作
depth_img_.copyTo(stereo_, mask); // 将 B 中 mask 为 255 的位置的值复制到 A 中
}
}
stereo_img = Map<DepthImgVector<>>((float_t *)stereo_.data, stereo_.rows * stereo_.cols);
return true;
}
void QuadrotorEnv::setMapID(int id) {
// -1 represent using the latest map in imitation learning
if (id < 0)
map_idx_ = kdtrees.size() - 1;
else
map_idx_ = id;
}
void QuadrotorEnv::getCostAndGradient(ConstRef<Vector<>> endstate, int traj_id, float &cost, Ref<Vector<>> grad) {
if (endstate.rows() != 9) {
std::cout << "ERROR: endstate must be 9 dim (X_pva, Y_pva, Z_pva)" << std::endl;
return;
}
std::vector<double> endstate_, grad_;
double cost_;
for (size_t i = 0; i < endstate.rows(); i++) {
endstate_.push_back(static_cast<double>(endstate(i)));
}
traj_opt_bridge->setMap(esdf_maps[map_idx_], min_map_boundaries[map_idx_], max_map_boundaries[map_idx_]);
traj_opt_bridge->setState(quad_state_.p.cast<double>(), quad_state_.q().cast<double>(), quad_state_.v.cast<double>(),
quad_state_.a.cast<double>());
traj_opt_bridge->setGoal(goal_.cast<double>());
traj_opt_bridge->getCostAndGradient(endstate_, traj_id, cost_, grad_);
for (size_t i = 0; i < grad_.size(); i++) {
grad(i) = static_cast<float>(grad_[i]);
}
cost = static_cast<float>(cost_);
}
bool QuadrotorEnv::step(const Ref<Vector<>> act, Ref<Vector<>> obs, Ref<Vector<>> reward) {
// python：setGoal -> step
if (!act.allFinite() || act.rows() != 9 || reward.rows() != 1) {
std::cout << "ERROR: endstate must be 9 dim" << std::endl;
return false;
}
steps_++;
std::vector<double> endstate_;
for (size_t i = 0; i < act.rows(); i++) {
endstate_.push_back(static_cast<double>(act(i)));
}
traj_opt_bridge->setMap(esdf_maps[map_idx_], min_map_boundaries[map_idx_], max_map_boundaries[map_idx_]);
traj_opt_bridge->setState(quad_state_.p.cast<double>(), quad_state_.q().cast<double>(), quad_state_.v.cast<double>(),
quad_state_.a.cast<double>());
traj_opt_bridge->setGoal(goal_.cast<double>());
double cost_;
Eigen::Vector3d next_pos, next_vel, next_acc;
traj_opt_bridge->getNextStateAndCost(endstate_, cost_, next_pos, next_vel, next_acc, sim_dt_);
desired_p_ = next_pos.cast<float>();
desired_v_ = next_vel.cast<float>();
desired_a_ = next_acc.cast<float>();
reward(0) = static_cast<float>(cost_);
// calculate the state based on the desired state.
runControlAndUpdateState(desired_p_, desired_v_, desired_a_);
quadrotor_ptr_->getState(&quad_state_);
getObs(obs);
return true;
}
bool QuadrotorEnv::isTerminalState(Scalar &reward) {
// 1.if out of boundary
if (quad_state_.x(QS::POSX) <= (world_box_(0)) || quad_state_.x(QS::POSY) <= (world_box_(1)) || quad_state_.x(QS::POSZ) <= (world_box_(2)) ||
quad_state_.x(QS::POSX) >= (world_box_(3)) || quad_state_.x(QS::POSY) >= (world_box_(4)) || quad_state_.x(QS::POSZ) >= (world_box_(5))) {
reward = 0;
// std::cout<<"越界"<<std::endl;
return true;
}
// 2.if collision
if (is_collision_) {
reward = -1;
// std::cout<<"碰撞"<<std::endl;
return true;
}
// 3.prevent uav being trapped
if (steps_ > 10 && quad_state_.v.norm() < 0.6 && dagger_mode_) {
reward = 0;
return true;
}
// 4.if reach the goal
Eigen::Vector3f Pw(quad_state_.p(0), quad_state_.p(1), quad_state_.p(2));
Eigen::Vector3f Gw(goal_(0), goal_(1), goal_(2));
float dist = (Pw - Gw).norm();
if (dist < 4) {
reward = 0;
// std::cout<<"到达"<<std::endl;
return true;
}
reward = 0.0;
return false;
}
void QuadrotorEnv::collisionCheck(float dis) {
pcl::PointXYZ drone_;
drone_.x = quad_state_.x(QS::POSX);
drone_.y = quad_state_.x(QS::POSY);
drone_.z = quad_state_.x(QS::POSZ);
int K = 1;
std::vector<int> indices(K);
std::vector<float> distances(K); // the square of the distances to the neighboring points.
kdtrees[map_idx_]->nearestKSearch(drone_, K, indices, distances);
nearest_obstacle = distances[0];
if (distances[0] < dis * dis)
is_collision_ = true;
}
bool QuadrotorEnv::loadParam(const YAML::Node &cfg) {
// camera
width_ = cfg["rgb_camera_left"]["width"].as<int>();
height_ = cfg["rgb_camera_left"]["height"].as<int>();
fov_ = cfg["rgb_camera_left"]["fov"].as<Scalar>();
// train or test
collect_data_ = cfg["quadrotor_env"]["collect_data"].as<bool>();
sim_dt_ = cfg["quadrotor_env"]["sim_dt"].as<Scalar>();
// data_collection
roll_var_ = cfg["data_collection"]["roll_var"].as<Scalar>();
pitch_var_ = cfg["data_collection"]["pitch_var"].as<Scalar>();
// world box
for (int i = 0; i < 3; ++i) {
scale_(i) = cfg["quadrotor_env"]["bounding_box"][i].as<Scalar>();
center_(i) = cfg["quadrotor_env"]["bounding_box_origin"][i].as<Scalar>();
}
return true;
}
bool QuadrotorEnv::getAct(Ref<Vector<>> act) const {
if (quad_act_.allFinite()) {
act = quad_act_;
return true;
}
return false;
}
bool QuadrotorEnv::configCamera(const YAML::Node &cfg) {
if (!cfg["rgb_camera_left"]) {
logger_.error("Cannot config RGB Camera");
return false;
}
if (!cfg["rgb_camera_left"]["on"].as<bool>()) {
logger_.warn("Camera is off. Please turn it on.");
return false;
}
if (quadrotor_ptr_->getNumCamera() >= 2) {
logger_.warn("Camera has been added. Skipping the camera configuration.");
return false;
}
// create left camera --------------------------------------------
rgb_camera_left = std::make_shared<RGBCamera>();
// load camera settings
std::vector<Scalar> t_BC_vec = cfg["rgb_camera_left"]["t_BC"].as<std::vector<Scalar>>();
std::vector<Scalar> r_BC_vec = cfg["rgb_camera_left"]["r_BC"].as<std::vector<Scalar>>();
Vector<3> t_BC(t_BC_vec.data());
Matrix<3, 3> r_BC = (AngleAxis(r_BC_vec[2] * M_PI / 180.0, Vector<3>::UnitZ()) * AngleAxis(r_BC_vec[1] * M_PI / 180.0, Vector<3>::UnitY()) *
AngleAxis(r_BC_vec[0] * M_PI / 180.0, Vector<3>::UnitX()))
.toRotationMatrix();
// Flightmare's FOV is vertical, while usually is horizontal, so convert the input horizontal FOV to vertical FOV.
Scalar rgb_fov_deg_ = cfg["rgb_camera_left"]["fov"].as<Scalar>();
double hor_fov_radians = (M_PI * (rgb_fov_deg_ / 180.0));
Scalar img_rows_ = cfg["rgb_camera_left"]["height"].as<Scalar>();
Scalar img_cols_ = cfg["rgb_camera_left"]["width"].as<Scalar>();
double flightmare_fov = 2. * std::atan(std::tan(hor_fov_radians / 2) * img_rows_ / img_cols_);
flightmare_fov = (flightmare_fov / M_PI) * 180.0;
rgb_camera_left->setFOV(flightmare_fov);
rgb_camera_left->setWidth(cfg["rgb_camera_left"]["width"].as<int>());
rgb_camera_left->setHeight(cfg["rgb_camera_left"]["height"].as<int>());
rgb_camera_left->setRelPose(t_BC, r_BC);
rgb_camera_left->enableOpticalFlow(cfg["rgb_camera_left"]["enable_opticalflow"].as<bool>());
rgb_camera_left->enableSegmentation(cfg["rgb_camera_left"]["enable_segmentation"].as<bool>());
rgb_camera_left->enableDepth(cfg["rgb_camera_left"]["enable_depth"].as<bool>());
// add camera to the quadrotor
quadrotor_ptr_->addRGBCamera(rgb_camera_left);
// create right camera --------------------------------------------
bool have_right_camera = cfg["rgb_camera_right"]["on"].as<bool>();
if (have_right_camera) {
rgb_camera_right = std::make_shared<RGBCamera>();
// load camera settings
std::vector<Scalar> t_BC_vec_r = cfg["rgb_camera_right"]["t_BC"].as<std::vector<Scalar>>();
std::vector<Scalar> r_BC_vec_r = cfg["rgb_camera_right"]["r_BC"].as<std::vector<Scalar>>();
Vector<3> t_BC_r(t_BC_vec_r.data());
Matrix<3, 3> r_BC_r =
(AngleAxis(r_BC_vec_r[2] * M_PI / 180.0, Vector<3>::UnitZ()) * AngleAxis(r_BC_vec_r[1] * M_PI / 180.0, Vector<3>::UnitY()) *
AngleAxis(r_BC_vec_r[0] * M_PI / 180.0, Vector<3>::UnitX()))
.toRotationMatrix();
rgb_camera_right->setFOV(flightmare_fov);
rgb_camera_right->setWidth(cfg["rgb_camera_left"]["width"].as<int>());
rgb_camera_right->setHeight(cfg["rgb_camera_left"]["height"].as<int>());
rgb_camera_right->setRelPose(t_BC_r, r_BC_r);
rgb_camera_right->enableOpticalFlow(false);
rgb_camera_right->enableSegmentation(false);
rgb_camera_right->enableDepth(false);
// add camera to the quadrotor
quadrotor_ptr_->addRGBCamera(rgb_camera_right);
stereo_baseline_ = fabs(t_BC(1) - t_BC_r(1));
}
// adapt parameters
img_width_ = rgb_camera_left->getWidth();
img_height_ = rgb_camera_left->getHeight();
rgb_img_ = cv::Mat::zeros(img_height_, img_width_, CV_MAKETYPE(CV_8U, rgb_camera_left->getChannels()));
depth_img_ = cv::Mat::zeros(img_height_, img_width_, CV_32FC1);
return true;
}
void QuadrotorEnv::computeDepthImage(const cv::Mat &left_frame, const cv::Mat &right_frame, cv::Mat *const depth) {
cv::Mat disparity(height_, width_, CV_8UC1);
sgm_->computeDisparity(left_frame, right_frame, disparity);
disparity.convertTo(disparity, CV_32FC1);
// compute depth from disparity
float f = (width_ / 2.0) / std::tan((M_PI * (fov_ / 180.0)) / 2.0);
// depth = stereo_baseline_ * f / disparity
for (int r = 0; r < height_; ++r) {
for (int c = 0; c < width_; ++c) {
if (disparity.at<float>(r, c) == 0.0f) {
depth->at<float>(r, c) = 0.0f;
} else if (disparity.at<float>(r, c) == 255.0f) {
depth->at<float>(r, c) = 0.0f;
} else {
depth->at<float>(r, c) = static_cast<float>(stereo_baseline_) * f / disparity.at<float>(r, c);
}
}
}
}
bool QuadrotorEnv::getRGBImage(Ref<ImgVector<>> img, const bool rgb) {
if (!rgb_camera_left) {
logger_.error("No Camera! Cannot retrieve Images.");
return false;
}
rgb_camera_left->getRGBImage(rgb_img_);
if (rgb_img_.rows != height_ || rgb_img_.cols != width_) {
logger_.error("Image resolution mismatch. Aborting.. Image rows %d != %d, Image cols %d != %d", rgb_img_.rows, height_, rgb_img_.cols,
width_);
return false;
}
if (!rgb) {
// converting rgb image to gray image
cvtColor(rgb_img_, gray_img_, CV_RGB2GRAY);
// map cv::Mat data to Eiegn::Vector
img = Map<ImgVector<>>(gray_img_.data, gray_img_.rows * gray_img_.cols);
} else {
img = Map<ImgVector<>>(rgb_img_.data, rgb_img_.rows * rgb_img_.cols * rgb_camera_left->getChannels());
}
return true;
}
void QuadrotorEnv::runControlAndUpdateState(Eigen::Vector3f p_ref, Eigen::Vector3f v_ref, Eigen::Vector3f a_ref) {
Eigen::Vector3f p_cur;
p_cur = quad_state_.p;
Eigen::Vector3f dir_vel = v_ref;
Eigen::Vector3f dir_goal = goal_ - p_cur;
Eigen::Vector3f dir_des = dir_vel.normalized() + dir_goal.normalized();
float yaw_ref = atan2(dir_des(1), dir_des(0));
Vector<3> rpy_cur;
get_euler_from_R(rpy_cur, quad_state_.R());
yaw_ref = calculate_yaw(rpy_cur(2), yaw_ref, sim_dt_); // limit the yaw (required by controller)
Eigen::Quaternionf q_ref;
quadrotor_ptr_->runSimpleFlight(a_ref, yaw_ref, q_ref);
quadrotor_ptr_->setState(p_ref, v_ref, q_ref, a_ref, sim_dt_);
}
} // namespace flightlib

406
flightlib/src/envs/vec_env.cpp Normal file
View File

@@ -0,0 +1,406 @@
#include "flightlib/envs/vec_env.hpp"
namespace flightlib {
template<typename EnvBase>
VecEnv<EnvBase>::VecEnv() : VecEnv(getenv("FLIGHTMARE_PATH") + std::string("/flightlib/configs/vec_env.yaml")) {}
template<typename EnvBase>
VecEnv<EnvBase>::VecEnv(const YAML::Node& cfg_node) : cfg_(cfg_node) {
init();
}
template<typename EnvBase>
VecEnv<EnvBase>::VecEnv(const std::string& cfgs, const bool from_file) {
// load environment configuration
if (from_file)
cfg_ = YAML::LoadFile(cfgs);
else
cfg_ = YAML::Load(cfgs);
init();
}
template<typename EnvBase>
void VecEnv<EnvBase>::init(void) {
// note that the cfg are input from python, and many have changed from vec_end.yaml
unity_render_ = cfg_["env"]["render"].as<bool>();
supervised_mode_ = cfg_["env"]["supervised"].as<bool>();
dagger_mode_ = cfg_["env"]["imitation"].as<bool>();
seed_ = cfg_["env"]["seed"].as<int>();
num_envs_ = cfg_["env"]["num_envs"].as<int>();
num_threads_ = cfg_["env"]["num_threads"].as<int>();
scene_id_ = cfg_["env"]["scene_id"].as<SceneID>();
ply_path_ = getenv("FLIGHTMARE_PATH") + cfg_["env"]["ply_path"].as<std::string>();
avg_tree_spacing_ = cfg_["unity"]["avg_tree_spacing"].as<Scalar>();
pointcloud_resolution_ = cfg_["unity"]["pointcloud_resolution"].as<Scalar>();
for (int i = 0; i < 3; ++i) {
bounding_box_(i) = cfg_["unity"]["bounding_box"][i].as<Scalar>();
bounding_box_origin_(i) = cfg_["unity"]["bounding_box_origin"][i].as<Scalar>();
}
// set threads
omp_set_num_threads(cfg_["env"]["num_threads"].as<int>());
// create & setup environments
const bool render = false;
for (int i = 0; i < num_envs_; i++) {
envs_.push_back(std::make_unique<EnvBase>());
}
// set Unity (init unity_bridge_ptr_ and add quadrotors to envs)
setUnity(unity_render_);
obs_dim_ = envs_[0]->getObsDim();
act_dim_ = envs_[0]->getActDim();
rew_dim_ = envs_[0]->getRewDim();
img_width_ = envs_[0]->getImgWidth();
img_height_ = envs_[0]->getImgHeight();
// if supervised_mode, then generate map from .ply
if (supervised_mode_)
generateMaps();
// set dagger_mode to each env
for (int i = 0; i < num_envs_; i++)
envs_[i]->setDAggerMode(dagger_mode_);
std::cout << "Vectorized Environment:\n"
<< "dagger mode = [" << dagger_mode_ << "]\n"
<< "supervised mode = [" << supervised_mode_ << "]\n"
<< "obs dim = [" << obs_dim_ << "]\n"
<< "act dim = [" << act_dim_ << "]\n"
<< "img width = [" << img_width_ << "]\n"
<< "img height = [" << img_height_ << "]\n"
<< "num_envs = [" << num_envs_ << "]\n"
<< "num_thread = [" << num_threads_ << "]\n"
<< "scene_id = [" << scene_id_ << "]" << std::endl;
}
template<typename EnvBase>
VecEnv<EnvBase>::~VecEnv() {}
// ====================== set functions ===================== //
template<typename EnvBase>
bool VecEnv<EnvBase>::reset(Ref<MatrixRowMajor<>> obs) {
if (obs.rows() != num_envs_ || obs.cols() != obs_dim_) {
logger_.error("Input matrix dimensions do not match with that of the environment.");
return false;
}
for (int i = 0; i < num_envs_; i++) {
envs_[i]->reset(obs.row(i));
}
if (unity_render_ && unity_ready_) {
frameID = 1;
FrameID frameID_rt;
unity_bridge_ptr_->getRender(frameID);
unity_bridge_ptr_->handleOutput(frameID_rt);
while (frameID != frameID_rt)
unity_bridge_ptr_->handleOutput(frameID_rt);
}
return true;
}
template<typename EnvBase>
bool VecEnv<EnvBase>::setState(ConstRef<MatrixRowMajor<>> state) {
if (state.rows() != num_envs_ || state.cols() != 13) { // 13: pvaq
logger_.error("Input state dimensions do not match with state.");
return false;
}
for (int i = 0; i < num_envs_; i++) {
envs_[i]->setState(state.row(i));
}
return true;
}
template<typename EnvBase>
bool VecEnv<EnvBase>::setGoal(ConstRef<MatrixRowMajor<>> goal) {
if (goal.rows() != num_envs_ || goal.cols() != 3) {
logger_.error("Input goal dimensions do not match with 3.");
return false;
}
for (int i = 0; i < num_envs_; i++) {
envs_[i]->setGoal(goal.row(i));
}
return true;
}
template<typename EnvBase>
bool VecEnv<EnvBase>::step(Ref<MatrixRowMajor<>> act, Ref<MatrixRowMajor<>> obs, Ref<MatrixRowMajor<>> reward, Ref<BoolVector<>> done) {
if (act.rows() != num_envs_ || act.cols() != act_dim_ || obs.rows() != num_envs_ || obs.cols() != obs_dim_ || reward.rows() != num_envs_ ||
reward.cols() != rew_dim_ || done.rows() != num_envs_ || done.cols() != 1) {
logger_.error("Input matrix dimensions do not match with that of the environment.");
return false;
}
#pragma omp parallel for schedule(dynamic)
for (int i = 0; i < num_envs_; i++) {
perAgentStep(i, act, obs, reward, done);
}
if (unity_render_ && unity_ready_) {
frameID++;
FrameID frameID_rt;
unity_bridge_ptr_->getRender(frameID);
unity_bridge_ptr_->handleOutput(frameID_rt);
while (frameID != frameID_rt)
unity_bridge_ptr_->handleOutput(frameID_rt);
}
return true;
}
template<typename EnvBase>
void VecEnv<EnvBase>::perAgentStep(int agent_id, Ref<MatrixRowMajor<>> act, Ref<MatrixRowMajor<>> obs, Ref<MatrixRowMajor<>> reward,
Ref<BoolVector<>> done) {
envs_[agent_id]->step(act.row(agent_id), obs.row(agent_id), reward.row(agent_id));
// use larger collision threshold in training and lower in testing
if (dagger_mode_)
envs_[agent_id]->collisionCheck(0.3);
else
envs_[agent_id]->collisionCheck(0.1);
Scalar terminal_reward = 0;
done(agent_id) = envs_[agent_id]->isTerminalState(terminal_reward);
if (done[agent_id]) {
envs_[agent_id]->reset(obs.row(agent_id));
}
}
template<typename EnvBase>
void VecEnv<EnvBase>::setMapID(ConstRef<IntVector<>> id) {
if (id.rows() != num_envs_) {
logger_.error("Input matrix dimensions do not match with that of the environment.");
return;
}
for (int i = 0; i < num_envs_; i++) {
envs_[i]->setMapID(id(i));
}
}
template<typename EnvBase>
void VecEnv<EnvBase>::setSeed(const int seed) {
int seed_inc = seed;
for (int i = 0; i < num_envs_; i++)
envs_[i]->setSeed(seed_inc++);
}
// ====================== set functions ===================== //
template<typename EnvBase>
void VecEnv<EnvBase>::getObs(Ref<MatrixRowMajor<>> obs) {
for (int i = 0; i < num_envs_; i++)
envs_[i]->getObs(obs.row(i));
}
template<typename EnvBase>
bool VecEnv<EnvBase>::getRGBImage(Ref<ImgMatrixRowMajor<>> img, const bool rgb_image) {
bool valid_img = true;
for (int i = 0; i < num_envs_; i++) {
valid_img &= envs_[i]->getRGBImage(img.row(i), rgb_image);
}
return valid_img;
}
template<typename EnvBase>
bool VecEnv<EnvBase>::getStereoImage(Ref<DepthImgMatrixRowMajor<>> stereo_img) {
bool valid_img = true;
for (int i = 0; i < num_envs_; i++) {
valid_img &= envs_[i]->getStereoImage(stereo_img.row(i));
}
return valid_img;
}
template<typename EnvBase>
bool VecEnv<EnvBase>::getDepthImage(Ref<DepthImgMatrixRowMajor<>> depth_img) {
bool valid_img = true;
for (int i = 0; i < num_envs_; i++) {
valid_img &= envs_[i]->getDepthImage(depth_img.row(i));
}
return valid_img;
}
template<typename EnvBase>
void VecEnv<EnvBase>::getCostAndGradient(ConstRef<MatrixRowMajor<>> dp, ConstRef<IntVector<>> traj_id, Ref<Vector<>> cost,
Ref<MatrixRowMajor<>> grad) {
#pragma omp parallel for schedule(dynamic) num_threads(num_threads_)
for (int i = 0; i < num_envs_; i++) {
envs_[i]->getCostAndGradient(dp.row(i), traj_id(i), cost(i), grad.row(i));
}
}
// ====================== unity functions ===================== //
template<typename EnvBase>
bool VecEnv<EnvBase>::setUnity(bool render) {
unity_render_ = render;
if (unity_render_ && unity_bridge_ptr_ == nullptr) {
// create unity bridge
unity_bridge_ptr_ = UnityBridge::getInstance();
// add objects to Unity
for (int i = 0; i < num_envs_; i++) {
envs_[i]->addObjectsToUnity(unity_bridge_ptr_);
}
logger_.info("Flightmare Bridge is created.");
}
return true;
}
template<typename EnvBase>
bool VecEnv<EnvBase>::spawnTrees() {
if (!unity_ready_ || unity_bridge_ptr_ == nullptr)
return false;
bool spawned = unity_bridge_ptr_->spawnTrees(bounding_box_, bounding_box_origin_, avg_tree_spacing_);
return spawned;
}
template<typename EnvBase>
bool VecEnv<EnvBase>::savePointcloud(int ply_id) {
if (!unity_ready_ || unity_bridge_ptr_ == nullptr)
return false;
Vector<3> min_corner = bounding_box_origin_ - 0.5 * bounding_box_;
Vector<3> max_corner = bounding_box_origin_ + 0.5 * bounding_box_;
unity_bridge_ptr_->generatePointcloud(min_corner, max_corner, ply_id, ply_path_, scene_id_, pointcloud_resolution_);
return true;
}
template<typename EnvBase>
bool VecEnv<EnvBase>::spawnTreesAndSavePointcloud(int ply_id_in, float spacing) {
Scalar avg_tree_spacing = avg_tree_spacing_;
if (spacing > 0)
avg_tree_spacing = spacing;
int ply_id = envs_[0]->getMapNum();
if (ply_id_in >= 0)
ply_id = ply_id_in;
if (!unity_ready_ || unity_bridge_ptr_ == nullptr)
return false;
bool spawned = unity_bridge_ptr_->spawnTrees(bounding_box_, bounding_box_origin_, avg_tree_spacing);
Vector<3> min_corner = bounding_box_origin_ - 0.5 * bounding_box_;
Vector<3> max_corner = bounding_box_origin_ + 0.5 * bounding_box_;
unity_bridge_ptr_->generatePointcloud(min_corner, max_corner, ply_id, ply_path_, scene_id_, pointcloud_resolution_);
usleep(1 * 1e6); // waitting 1s for generating completely
// KDtree, for collision detection
pcl::search::KdTree<pcl::PointXYZ> kdtree;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
std::cout << "Map Path: " << ply_path_ + "pointcloud-" + std::to_string(ply_id) + ".ply" << std::endl;
pcl::io::loadPLYFile<pcl::PointXYZ>(ply_path_ + "pointcloud-" + std::to_string(ply_id) + ".ply", *cloud);
kdtree.setInputCloud(cloud); // 0.3s
// ESDF, for gradient calculation (map_id is required)
Eigen::Vector3d map_boundary_min, map_boundary_max;
std::shared_ptr<sdf_tools::SignedDistanceField> esdf_map = traj_opt::SdfConstruction(cloud, map_boundary_min, map_boundary_max);
for (int i = 0; i < num_envs_; i++) {
envs_[i]->addKdtree(std::make_shared<pcl::search::KdTree<pcl::PointXYZ>>(kdtree));
envs_[i]->addESDFMap(esdf_map);
envs_[i]->addMapSize(map_boundary_min, map_boundary_max);
}
return true;
}
// For supervised learning
template<typename EnvBase>
void VecEnv<EnvBase>::generateMaps() {
std::vector<std::string> ply_files;
for (const auto& entry : std::filesystem::directory_iterator(ply_path_)) {
if (entry.is_regular_file() && entry.path().extension() == ".ply") {
ply_files.push_back(entry.path().string());
}
}
// Sort according to the number of the filename.
std::sort(ply_files.begin(), ply_files.end(), [this](const std::string& a, const std::string& b) {
return extract_number(std::filesystem::path(a).filename().string()) < extract_number(std::filesystem::path(b).filename().string());
});
for (auto ply_file : ply_files) {
std::cout << "load ply file: " << ply_file << std::endl;
pcl::search::KdTree<pcl::PointXYZ> kdtree;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPLYFile<pcl::PointXYZ>(ply_file, *cloud);
// KDtree, for collision detection
kdtree.setInputCloud(cloud); // 0.3s
// ESDF, for gradient calculation
Eigen::Vector3d map_boundary_min, map_boundary_max;
std::shared_ptr<sdf_tools::SignedDistanceField> esdf_map = traj_opt::SdfConstruction(cloud, map_boundary_min, map_boundary_max);
std::cout << "pc min:" << map_boundary_min.transpose() << " pc max:" << map_boundary_max.transpose() << std::endl;
for (int i = 0; i < num_envs_; i++) {
envs_[i]->addKdtree(std::make_shared<pcl::search::KdTree<pcl::PointXYZ>>(kdtree));
envs_[i]->addESDFMap(esdf_map);
envs_[i]->addMapSize(map_boundary_min, map_boundary_max);
}
}
}
template<typename EnvBase>
bool VecEnv<EnvBase>::connectUnity(void) {
if (unity_bridge_ptr_ == nullptr)
return false;
unity_ready_ = unity_bridge_ptr_->connectUnity(scene_id_);
return unity_ready_;
}
template<typename EnvBase>
void VecEnv<EnvBase>::render(void) {
if (unity_render_ && unity_ready_) {
frameID++;
FrameID frameID_rt;
unity_bridge_ptr_->getRender(frameID);
unity_bridge_ptr_->handleOutput(frameID_rt);
while (frameID != frameID_rt)
unity_bridge_ptr_->handleOutput(frameID_rt);
}
}
template<typename EnvBase>
void VecEnv<EnvBase>::disconnectUnity(void) {
if (unity_bridge_ptr_ != nullptr) {
unity_bridge_ptr_->disconnectUnity();
unity_ready_ = false;
} else {
logger_.warn("Flightmare Unity Bridge is not initialized.");
}
}
template<typename EnvBase>
void VecEnv<EnvBase>::close() {
for (int i = 0; i < num_envs_; i++) {
envs_[i]->close();
}
}
// ====================== other functions ===================== //
// Extract the number from the filename (e.g., pointcloud-1.ply)
template<typename EnvBase>
int VecEnv<EnvBase>::extract_number(const std::string& filename) {
std::regex number_regex("pointcloud-(\\d+)\\.ply");
std::smatch match;
if (std::regex_search(filename, match, number_regex)) {
return std::stoi(match[1]);
}
return -1; // If no number is found, return -1
}
// IMPORTANT. Otherwise:
// Segmentation fault (core dumped)
template class VecEnv<QuadrotorEnv>;
} // namespace flightlib

375
flightlib/src/grad_traj_optimization/grad_traj_optimizer.cpp Normal file
View File

@@ -0,0 +1,375 @@
#include "flightlib/grad_traj_optimization/grad_traj_optimizer.h"
GradTrajOptimizer::GradTrajOptimizer(const YAML::Node &cfg) {
//-------------------------get parameter from server--------------------
this->w_smooth = cfg["ws"].as<double>();
this->w_goal = cfg["wg"].as<double>();
this->w_long = cfg["wl"].as<double>();
this->w_vel = cfg["wv"].as<double>();
this->w_acc = cfg["wa"].as<double>();
this->w_collision = cfg["wc"].as<double>();
this->alpha = cfg["alpha"].as<double>();
this->d0 = cfg["d0"].as<double>();
this->r = cfg["r"].as<double>();
this->alphav = cfg["alphav"].as<double>();
this->v0 = cfg["v0"].as<double>();
this->rv = cfg["rv"].as<double>();
this->alphaa = cfg["alphaa"].as<double>();
this->a0 = cfg["a0"].as<double>();
this->ra = cfg["ra"].as<double>();
this->sgm_time = 2 * cfg["radio_range"].as<double>() / cfg["vel_max"].as<double>();
//------------------------generate optimization dependency------------------
Time = Eigen::VectorXd::Zero(1);
Time(0) = sgm_time;
TrajectoryGenerator generator;
generator.QPGeneration(Time);
R = generator.getR();
Rff = generator.getRff();
Rpp = generator.getRpp();
Rpf = generator.getRpf();
Rfp = generator.getRfp();
L = generator.getL();
A = generator.getA();
C = generator.getC();
int m = Time.size(); // number of segments in the trajectory
Dp = Eigen::MatrixXd::Zero(3, m * 3); // optimized x_pva, y_pva, z_pva (end state)
Df = Eigen::MatrixXd::Zero(3, m * 3); // fixed x_pva, y_pva, z_pva (init state)
V = Eigen::MatrixXd::Zero(6, 6);
for (int i = 0; i < 5; ++i)
V(i, i + 1) = i + 1;
num_dp = Dp.cols();
num_df = Df.cols();
num_point = Time.rows() + 1;
boundary = Eigen::VectorXd::Zero(6);
}
void GradTrajOptimizer::setSignedDistanceField(std::shared_ptr<sdf_tools::SignedDistanceField> s, double res) {
this->sdf = s;
this->resolution = res;
}
void GradTrajOptimizer::constrains(double &n, double min, double max) const {
if (n > max)
n = max;
else if (n < min)
n = min;
}
void GradTrajOptimizer::setGoal(Eigen::Vector3d goal) { this->goal = goal; }
void GradTrajOptimizer::setBoundary(Eigen::Vector3d min, Eigen::Vector3d max) {
this->map_boundary_min = min;
this->map_boundary_max = max;
boundary(0) = map_boundary_min(0);
boundary(1) = map_boundary_max(0);
boundary(2) = map_boundary_min(1);
boundary(3) = map_boundary_max(1);
boundary(4) = map_boundary_min(2);
boundary(5) = map_boundary_max(2);
}
void GradTrajOptimizer::getCoefficientFromDerivative(Eigen::MatrixXd &coefficient, const std::vector<double> &_dp) const {
coefficient.resize(num_point - 1, 18);
for (int i = 0; i < 3; ++i) {
//-----------------------merge df and dp -> d(df,dp)-----------------------
Eigen::VectorXd df(num_df);
Eigen::VectorXd dp(num_dp);
Eigen::VectorXd d(num_df + num_dp);
df = Df.row(i);
for (int j = 0; j < num_dp; j++) {
dp(j) = _dp[j + num_dp * i];
}
d.segment(0, 3) = df;
d.segment(3, num_dp) = dp;
// ----------------------convert derivative to coefficient------------------
Eigen::VectorXd coe(6 * (num_point - 1));
coe = L * d;
for (int j = 0; j < (num_point - 1); j++) {
coefficient.block(j, 6 * i, 1, 6) = coe.segment(6 * j, 6).transpose();
}
}
}
void GradTrajOptimizer::getCostAndGradient(const std::vector<double> &df, const std::vector<double> &dp, double &cost,
std::vector<double> &_grad) const {
cost = 0;
double cost_smooth = 0;
double cost_colli = 0;
double cost_goal = 0;
double cost_long = 0;
double cost_vel = 0; // deprecated
double cost_acc = 0; // deprecated
Eigen::MatrixXd gradient = Eigen::MatrixXd::Zero(3, num_dp);
Eigen::MatrixXd g_smooth = Eigen::MatrixXd::Zero(3, num_dp);
Eigen::MatrixXd g_colli = Eigen::MatrixXd::Zero(3, num_dp);
Eigen::MatrixXd g_goal = Eigen::MatrixXd::Zero(3, num_dp);
Eigen::MatrixXd g_long = Eigen::MatrixXd::Zero(3, num_dp);
Eigen::MatrixXd g_vel = Eigen::MatrixXd::Zero(3, num_dp); // deprecated
Eigen::MatrixXd g_acc = Eigen::MatrixXd::Zero(3, num_dp); // deprecated
for (int i = 0; i < num_df; ++i) {
Df(0, i) = df[i];
Df(1, i) = df[i + num_df];
Df(2, i) = df[i + 2 * num_df];
}
// ------------------------- 1. get smoothness cost -----------------------------
{
// 平滑度的Cost，基于MinimalSnap，有：Js = d * R * d，其中d = [dF,dP]
Eigen::VectorXd dfx = Df.block(0, 0, 1, 3).transpose();
Eigen::VectorXd dfy = Df.block(1, 0, 1, 3).transpose();
Eigen::VectorXd dfz = Df.block(2, 0, 1, 3).transpose();
Eigen::VectorXd dpx = Eigen::VectorXd::Zero(num_dp);
Eigen::VectorXd dpy = Eigen::VectorXd::Zero(num_dp);
Eigen::VectorXd dpz = Eigen::VectorXd::Zero(num_dp);
for (int i = 0; i < num_dp; ++i) {
dpx(i) = dp[i];
dpy(i) = dp[i + num_dp];
dpz(i) = dp[i + 2 * num_dp];
}
Eigen::VectorXd dx = Eigen::VectorXd::Zero(num_dp + num_df);
Eigen::VectorXd dy = Eigen::VectorXd::Zero(num_dp + num_df);
Eigen::VectorXd dz = Eigen::VectorXd::Zero(num_dp + num_df);
dx.segment(0, 3) = dfx;
dx.segment(3, num_dp) = dpx;
dy.segment(0, 3) = dfy;
dy.segment(3, num_dp) = dpy;
dz.segment(0, 3) = dfz;
dz.segment(3, num_dp) = dpz;
// ------------------- 1.1 get smoothness cost,fs= d'Rd ---------------------
cost_smooth = double(dx.transpose() * R * dx) + double(dy.transpose() * R * dy) + (dz.transpose() * R * dz);
//-------------------- 1.2 get smoothness gradient --------------------------
Eigen::MatrixXd gx_smooth = 2 * Rfp.transpose() * dfx + 2 * Rpp * dpx;
Eigen::MatrixXd gy_smooth = 2 * Rfp.transpose() * dfy + 2 * Rpp * dpy;
Eigen::MatrixXd gz_smooth = 2 * Rfp.transpose() * dfz + 2 * Rpp * dpz;
g_smooth.row(0) = gx_smooth.transpose();
g_smooth.row(1) = gy_smooth.transpose();
g_smooth.row(2) = gz_smooth.transpose();
}
// -------------------------- 2. get collision cost -----------------------------
{
Eigen::MatrixXd coe;
getCoefficientFromDerivative(coe, dp);
Eigen::MatrixXd Ldp(6, num_dp);
// only single-segment polynomial here
for (int s = 0; s < Time.size(); s++) {
Ldp = L.block(6 * s, 3, 6, num_dp);
// discrete time step
double dt = Time(s) / 30.0;
for (double t = 1e-3; t < Time(s); t += dt) {
// get position, velocity
Eigen::Vector3d pos, vel;
getPositionFromCoeff(pos, coe, s, t);
getVelocityFromCoeff(vel, coe, s, t);
// get information from signed distance field
double dist = 0, gd = 0, cd = 0;
Eigen::Vector3d grad;
getDistanceAndGradient(pos, dist, grad); // 在sdf地图中的距离障碍物dist和grad（梯度方向）
getDistancePenalty(dist, cd); // 计算障碍物距离惩罚cost
getDistancePenaltyGradient(dist, gd); // 计算障碍物距离惩罚的梯度值
// time Matrix T
Eigen::MatrixXd T(1, 6);
getTimeMatrix(t, T);
// ------------------------ 2.1 collision cost-------------------------
cost_colli += cd * dt; // 碰撞cost = 障碍物距离惩罚c * 速度norm * 时间间隔
// ------------------ 2.2 gradient of collision cost-------------------
for (int k = 0; k < 3; k++) { // 每一行对应xyz三个轴，一行的各列对应具体轴上对p,v,a的梯度
g_colli.row(k) = g_colli.row(k) + (gd * grad(k) * T * Ldp) * dt;
}
// ---------- 3. Deprecated: get velocity and accleration cost --------
if (0) {
double cv = 0, ca = 0, gv = 0, ga = 0;
Eigen::Vector3d acc;
getAccelerationFromCoeff(acc, coe, s, t);
for (int k = 0; k < 3; k++) {
getVelocityPenalty(vel(k), cv);
cost_vel += cv * dt;
getAccelerationPenalty(acc(k), ca);
cost_acc += ca * dt;
}
for (int k = 0; k < 3; k++) {
getVelocityPenaltyGradient(vel(k), gv);
g_vel.row(k) = g_vel.row(k) + (gv * T * V * Ldp) * dt;
getAccelerationPenaltyGradient(acc(k), ga);
g_acc.row(k) = g_acc.row(k) + (ga * T * V * V * Ldp) * dt;
}
}
}
}
}
// ---------------------------- 4. get goal cost ---------------------------------
// 4.1 make the trajectry longer
Eigen::Vector3d start_pos(df[0], df[num_dp], df[2 * num_dp]);
Eigen::Vector3d end_pos(dp[0], dp[num_dp], dp[2 * num_dp]);
Eigen::Vector3d delta_pos = end_pos - start_pos;
double goal_r = 100; // param can be moved to config
cost_long = exp(-(delta_pos(0) * delta_pos(0) + delta_pos(1) * delta_pos(1)) / goal_r);
g_long(0, 0) = -2 / goal_r * delta_pos(0) * cost_long;
g_long(1, 0) = -2 / goal_r * delta_pos(1) * cost_long;
// 4.2 make the trajectry approach the goal
cost_goal = (end_pos - this->goal).norm() * (end_pos - this->goal).norm();
g_goal(0, 0) = dp[0] - this->goal(0);
g_goal(1, 0) = dp[num_dp] - this->goal(1);
g_goal(2, 0) = dp[2 * num_dp] - this->goal(2);
g_goal = 2 * g_goal;
//------------------------ sum up all cost and gradient -----------------------------
double ws = this->w_smooth, wc = this->w_collision, wg = this->w_goal, wv = this->w_vel, wa = this->w_acc, wl = this->w_long;
cost = ws * cost_smooth + wc * cost_colli + wv * cost_vel + wa * cost_acc + wg * cost_goal + wl * cost_long + 1e-3;
gradient = ws * g_smooth + wc * g_colli + wg * g_goal + wv * g_vel + wa * g_acc + wl * g_long;
// gradient: 3x3 每一行对应xyz三个轴，一行的各列对应具体轴上对p,v,a的梯度
_grad.resize(num_dp * 3);
for (int i = 0; i < num_dp; ++i) {
_grad[i] = gradient(0, i);
_grad[i + num_dp] = gradient(1, i);
_grad[i + 2 * num_dp] = gradient(2, i);
}
// cout << "smooth cost:" << ws * cost_smooth << " collision cost:" << wc * cost_colli << " goal cost:" << wg * cost_goal << endl;
// cout << "smooth grad:" << ws * g_smooth(0) << " collision grad:" << wc * g_colli(0) << " goal grad:" << wg * g_goal(0) << endl;
}
// get position from coefficient
void GradTrajOptimizer::getPositionFromCoeff(Eigen::Vector3d &pos, const Eigen::MatrixXd &coeff, const int &index, const double &time) const {
int s = index;
double t = time;
float x = coeff(s, 0) + coeff(s, 1) * t + coeff(s, 2) * pow(t, 2) + coeff(s, 3) * pow(t, 3) + coeff(s, 4) * pow(t, 4) + coeff(s, 5) * pow(t, 5);
float y = coeff(s, 6) + coeff(s, 7) * t + coeff(s, 8) * pow(t, 2) + coeff(s, 9) * pow(t, 3) + coeff(s, 10) * pow(t, 4) + coeff(s, 11) * pow(t, 5);
float z =
coeff(s, 12) + coeff(s, 13) * t + coeff(s, 14) * pow(t, 2) + coeff(s, 15) * pow(t, 3) + coeff(s, 16) * pow(t, 4) + coeff(s, 17) * pow(t, 5);
pos(0) = x;
pos(1) = y;
pos(2) = z;
}
// get velocity from cofficient
void GradTrajOptimizer::getVelocityFromCoeff(Eigen::Vector3d &vel, const Eigen::MatrixXd &coeff, const int &index, const double &time) const {
int s = index;
double t = time;
float vx = coeff(s, 1) + 2 * coeff(s, 2) * pow(t, 1) + 3 * coeff(s, 3) * pow(t, 2) + 4 * coeff(s, 4) * pow(t, 3) + 5 * coeff(s, 5) * pow(t, 4);
float vy = coeff(s, 7) + 2 * coeff(s, 8) * pow(t, 1) + 3 * coeff(s, 9) * pow(t, 2) + 4 * coeff(s, 10) * pow(t, 3) + 5 * coeff(s, 11) * pow(t, 4);
float vz =
coeff(s, 13) + 2 * coeff(s, 14) * pow(t, 1) + 3 * coeff(s, 15) * pow(t, 2) + 4 * coeff(s, 16) * pow(t, 3) + 5 * coeff(s, 17) * pow(t, 4);
vel(0) = vx;
vel(1) = vy;
vel(2) = vz;
}
// get acceleration from coefficient
void GradTrajOptimizer::getAccelerationFromCoeff(Eigen::Vector3d &acc, const Eigen::MatrixXd &coeff, const int &index, const double &time) const {
int s = index;
double t = time;
float ax = 2 * coeff(s, 2) + 6 * coeff(s, 3) * pow(t, 1) + 12 * coeff(s, 4) * pow(t, 2) + 20 * coeff(s, 5) * pow(t, 3);
float ay = 2 * coeff(s, 8) + 6 * coeff(s, 9) * pow(t, 1) + 12 * coeff(s, 10) * pow(t, 2) + 20 * coeff(s, 11) * pow(t, 3);
float az = 2 * coeff(s, 14) + 6 * coeff(s, 15) * pow(t, 1) + 12 * coeff(s, 16) * pow(t, 2) + 20 * coeff(s, 17) * pow(t, 3);
acc(0) = ax;
acc(1) = ay;
acc(2) = az;
}
inline void GradTrajOptimizer::getDistancePenalty(const double &d, double &cost) const { cost = this->alpha * exp(-(d - this->d0) / this->r); }
inline void GradTrajOptimizer::getDistancePenaltyGradient(const double &d, double &grad) const {
grad = -(this->alpha / this->r) * exp(-(d - this->d0) / this->r);
}
inline void GradTrajOptimizer::getVelocityPenalty(const double &v, double &cost) const { cost = alphav * exp((abs(v) - v0) / rv); }
inline void GradTrajOptimizer::getVelocityPenaltyGradient(const double &v, double &grad) const { grad = (alphav / rv) * exp((abs(v) - v0) / rv); }
inline void GradTrajOptimizer::getAccelerationPenalty(const double &a, double &cost) const { cost = alphaa * exp((abs(a) - a0) / ra); }
inline void GradTrajOptimizer::getAccelerationPenaltyGradient(const double &a, double &grad) const { grad = (alphaa / ra) * exp((abs(a) - a0) / ra); }
// get distance in signed distance field ,by position query
void GradTrajOptimizer::getDistanceAndGradient(Eigen::Vector3d &pos, double &dist, Eigen::Vector3d &grad) const {
// get sdf directly from sdf_tools
Eigen::Vector3d ori_pos = pos;
// 1、限定在地图边界内 2、后面越界的惩罚回来
constrains(pos(0), map_boundary_min(0), map_boundary_max(0));
constrains(pos(1), map_boundary_min(1), map_boundary_max(1));
constrains(pos(2), map_boundary_min(2), map_boundary_max(2));
std::vector<double> location_gradient_query = this->sdf->GetGradient(pos(0), pos(1), pos(2), true);
grad(0) = location_gradient_query[0];
grad(1) = location_gradient_query[1];
grad(2) = location_gradient_query[2];
std::pair<float, bool> location_sdf_query = this->sdf->GetSafe(pos(0), pos(1), pos(2));
dist = location_sdf_query.first;
// update distance and gradient using boundary
double dtb = getDistanceToBoundary(ori_pos(0), ori_pos(1), ori_pos(2));
// 1. 点在边界内时：把点推向内部; 2. 如果在Boundary外: (dtb<0)梯度是指向Boundary的,同样推向内部
if (dtb < dist) {
dist = dtb;
recaluculateGradient(ori_pos(0), ori_pos(1), ori_pos(2), grad);
}
}
double GradTrajOptimizer::getDistanceToBoundary(const double &x, const double &y, const double &z) const {
double dist_x = std::min(x - boundary(0), boundary(1) - x);
double dist_y = std::min(y - boundary(2), boundary(3) - y);
double dist_z = std::min(z - boundary(4), boundary(5) - z);
double dtb = std::min(dist_x, dist_y);
dtb = std::min(dtb, dist_z);
return dtb;
}
void GradTrajOptimizer::recaluculateGradient(const double &x, const double &y, const double &z, Eigen::Vector3d &grad) const {
double r = this->resolution;
grad(0) = (10 * (GDTB(x + r, y, z) - GDTB(x - r, y, z)) + 3 * (GDTB(x + r, y + r, z) - GDTB(x - r, y + r, z)) +
3 * (GDTB(x + r, y - r, z) - GDTB(x - r, y - r, z))) /
(32 * r);
grad(1) = (10 * (GDTB(x, y + r, z) - GDTB(x, y - r, z)) + 3 * (GDTB(x + r, y + r, z) - GDTB(x + r, y - r, z)) +
3 * (GDTB(x - r, y + r, z) - GDTB(x - r, y - r, z))) /
(32 * r);
grad(2) = (10 * (GDTB(x, y, z + r) - GDTB(x, y, z - r)) + 3 * (GDTB(x, y + r, z + r) - GDTB(x, y + r, z - r)) +
3 * (GDTB(x, y - r, z + r) - GDTB(x, y - r, z - r))) /
(32 * r);
}
void GradTrajOptimizer::getTimeMatrix(const double &t, Eigen::MatrixXd &T) const {
T.resize(1, 6);
T.setZero();
for (int i = 0; i < 6; ++i) {
T(0, i) = pow(t, i);
}
}

145
flightlib/src/grad_traj_optimization/opt_utile.cpp Normal file
View File

@@ -0,0 +1,145 @@
#include "flightlib/grad_traj_optimization/opt_utile.h"
/*
Front-End Guiding Path:
We evenly sample vertical_num * horizon_num * radio_num * vel_num primitives here with different position, length, and velocity direction.
But in practical, only vertical_num * horizon_num primitives are sampled (radio_num = vel_num = 1).
*/
void getLatticeGuiding(std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> &lattice_nodes, int horizon_num, int vertical_num, int radio_num,
int vel_num, double horizon_fov, double vertical_fov, double radio_range, double vel_fov, double vel_prefile) {
double direction_diff, altitude_diff, radio_diff, vel_dir_diff;
if (horizon_num == 1)
direction_diff = 0;
else
direction_diff = (horizon_fov / 180.0 * M_PI) / (horizon_num - 1);
if (vertical_num == 1)
altitude_diff = 0;
else
altitude_diff = (vertical_fov / 180.0 * M_PI) / (vertical_num - 1);
radio_diff = radio_range / radio_num;
if (vel_num == 1)
vel_dir_diff = 0;
else
vel_dir_diff = (vel_fov / 180.0f * M_PI) / (vel_num - 1);
// if (vel_num == 1) // be 0 looks like better
// vel_prefile = 0.0;
lattice_nodes.clear();
for (int h = 0; h < radio_num; h++) {
for (int i = 0; i < vertical_num; i++) {
for (int j = 0; j < horizon_num; j++) {
for (int k = 0; k < vel_num; k++) {
double search_radio = (h + 1) * radio_diff;
double alpha = -direction_diff * (horizon_num - 1) / 2 + j * direction_diff; // 位置偏航角（从右往左）
double beta = -altitude_diff * (vertical_num - 1) / 2 + i * altitude_diff; // 高度偏移角（从下往上）
double gamma = -vel_dir_diff * (vel_num - 1) / 2 + k * vel_dir_diff; // 速度偏航
Eigen::Vector3d lattice_node_pos(cos(beta) * cos(alpha) * search_radio, cos(beta) * sin(alpha) * search_radio,
sin(beta) * search_radio);
Eigen::Vector3d lattice_node_vel(cos(alpha + gamma) * vel_prefile, sin(alpha + gamma) * vel_prefile, 0.0);
std::pair<Eigen::Vector3d, Eigen::Vector3d> lattice_node(lattice_node_pos, lattice_node_vel);
lattice_nodes.push_back(lattice_node);
}
}
}
}
}
// TODO: 改为硬编码
Eigen::MatrixXd solveCoeffFromBoundaryState(const Eigen::Vector3d &Pos_init, const Eigen::Vector3d &Vel_init, const Eigen::Vector3d &Acc_init,
const Eigen::Vector3d &Pos_end, const Eigen::Vector3d &Vel_end, const Eigen::Vector3d &Acc_end,
double Time) {
Eigen::MatrixXd PolyCoeff(1, 3 * 6);
Eigen::VectorXd Px(6), Py(6), Pz(6);
const static auto Factorial = [](int x) {
int fac = 1;
for (int i = x; i > 0; i--)
fac = fac * i;
return fac;
};
/* Produce Mapping Matrix A to the entire trajectory. */
Eigen::MatrixXd A = Eigen::MatrixXd::Zero(6, 6);
for (int i = 0; i < 3; i++) {
A(2 * i, i) = Factorial(i);
for (int j = i; j < 6; j++)
A(2 * i + 1, j) = Factorial(j) / Factorial(j - i) * pow(Time, j - i);
}
/* Produce the dereivatives in X, Y and Z axis directly. */
Eigen::VectorXd Dx = Eigen::VectorXd::Zero(6);
Eigen::VectorXd Dy = Eigen::VectorXd::Zero(6);
Eigen::VectorXd Dz = Eigen::VectorXd::Zero(6);
Dx(0) = Pos_init(0);
Dy(0) = Pos_init(1);
Dz(0) = Pos_init(2);
Dx(1) = Pos_end(0);
Dy(1) = Pos_end(1);
Dz(1) = Pos_end(2);
Dx(2) = Vel_init(0);
Dy(2) = Vel_init(1);
Dz(2) = Vel_init(2);
Dx(3) = Vel_end(0);
Dy(3) = Vel_end(1);
Dz(3) = Vel_end(2);
Dx(4) = Acc_init(0);
Dy(4) = Acc_init(1);
Dz(4) = Acc_init(2);
Dx(5) = Acc_end(0);
Dy(5) = Acc_end(1);
Dz(5) = Acc_end(2);
Px = A.inverse() * Dx;
Py = A.inverse() * Dy;
Pz = A.inverse() * Dz;
PolyCoeff.block(0, 0, 1, 6) = Px.segment(0, 6).transpose();
PolyCoeff.block(0, 6, 1, 6) = Py.segment(0, 6).transpose();
PolyCoeff.block(0, 12, 1, 6) = Pz.segment(0, 6).transpose();
return PolyCoeff;
}
void getPositionFromCoeff(Eigen::Vector3d &pos, Eigen::MatrixXd coeff, int index, double time) {
int s = index;
double t = time;
float x = coeff(s, 0) + coeff(s, 1) * t + coeff(s, 2) * pow(t, 2) + coeff(s, 3) * pow(t, 3) + coeff(s, 4) * pow(t, 4) + coeff(s, 5) * pow(t, 5);
float y = coeff(s, 6) + coeff(s, 7) * t + coeff(s, 8) * pow(t, 2) + coeff(s, 9) * pow(t, 3) + coeff(s, 10) * pow(t, 4) + coeff(s, 11) * pow(t, 5);
float z =
coeff(s, 12) + coeff(s, 13) * t + coeff(s, 14) * pow(t, 2) + coeff(s, 15) * pow(t, 3) + coeff(s, 16) * pow(t, 4) + coeff(s, 17) * pow(t, 5);
pos(0) = x;
pos(1) = y;
pos(2) = z;
}
void getVelocityFromCoeff(Eigen::Vector3d &vel, Eigen::MatrixXd coeff, int index, double time) {
int s = index;
double t = time;
float vx = coeff(s, 1) + 2 * coeff(s, 2) * pow(t, 1) + 3 * coeff(s, 3) * pow(t, 2) + 4 * coeff(s, 4) * pow(t, 3) + 5 * coeff(s, 5) * pow(t, 4);
float vy = coeff(s, 7) + 2 * coeff(s, 8) * pow(t, 1) + 3 * coeff(s, 9) * pow(t, 2) + 4 * coeff(s, 10) * pow(t, 3) + 5 * coeff(s, 11) * pow(t, 4);
float vz =
coeff(s, 13) + 2 * coeff(s, 14) * pow(t, 1) + 3 * coeff(s, 15) * pow(t, 2) + 4 * coeff(s, 16) * pow(t, 3) + 5 * coeff(s, 17) * pow(t, 4);
vel(0) = vx;
vel(1) = vy;
vel(2) = vz;
}
void getAccelerationFromCoeff(Eigen::Vector3d &acc, Eigen::MatrixXd coeff, int index, double time) {
int s = index;
double t = time;
float ax = 2 * coeff(s, 2) + 6 * coeff(s, 3) * pow(t, 1) + 12 * coeff(s, 4) * pow(t, 2) + 20 * coeff(s, 5) * pow(t, 3);
float ay = 2 * coeff(s, 8) + 6 * coeff(s, 9) * pow(t, 1) + 12 * coeff(s, 10) * pow(t, 2) + 20 * coeff(s, 11) * pow(t, 3);
float az = 2 * coeff(s, 14) + 6 * coeff(s, 15) * pow(t, 1) + 12 * coeff(s, 16) * pow(t, 2) + 20 * coeff(s, 17) * pow(t, 3);
acc(0) = ax;
acc(1) = ay;
acc(2) = az;
}

125
flightlib/src/grad_traj_optimization/qp_generator.cpp Normal file
View File

@@ -0,0 +1,125 @@
/*
Currently, only 5-order single-segment polynomial is used,
but the functionality for piecewise polynomials is retained (i.e. m, num_f, num_p and other variables).
*/
#include "flightlib/grad_traj_optimization/qp_generator.h"
#include <stdio.h>
#include <fstream>
#include <iostream>
#include <string>
using namespace std;
using namespace Eigen;
TrajectoryGenerator::TrajectoryGenerator() {}
TrajectoryGenerator::~TrajectoryGenerator() {}
void TrajectoryGenerator::QPGeneration(const Eigen::VectorXd &Time) {
m = Time.size();
// 阶乘
const static auto Factorial = [](int x) {
int fac = 1;
for (int i = x; i > 0; i--)
fac = fac * i;
return fac;
};
/* Produce Mapping Matrix A to the entire trajectory. */
MatrixXd Ab; // 每一段矩阵的A（论文中M）
MatrixXd A = MatrixXd::Zero(m * 6, m * 6); // m是段数
// Ab 的组成为6行，第1行Tk位置，第二行Tk+1位置，第三行Tk速度，第四行Tk+1速度，五六为加速度
// 采用5次多项式，所以每段轨迹有6个多项式系数（列）
for (int k = 0; k < m; k++) {
Ab = Eigen::MatrixXd::Zero(6, 6);
for (int i = 0; i < 3; i++) {
Ab(2 * i, i) = Factorial(i);
for (int j = i; j < 6; j++)
Ab(2 * i + 1, j) = Factorial(j) / Factorial(j - i) * pow(Time(k), j - i);
}
A.block(k * 6, k * 6, 6, 6) = Ab;
}
_A = A;
/* Produce the Minimum Snap cost function, the Hessian Matrix */
MatrixXd H = MatrixXd::Zero(m * 6, m * 6);
// min snap 的cost function（四阶导数的积分 和 系数的关系 的矩阵），论文中间的矩阵Q
for (int k = 0; k < m; k++) {
for (int i = 3; i < 6; i++) {
for (int j = 3; j < 6; j++) {
H(k * 6 + i, k * 6 + j) = i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) / (i + j - 5) * pow(Time(k), (i + j - 5));
}
}
}
_Q = H; // Only minumum snap term is considered here inf the Hessian matrix
StackOptiDep();
}
void TrajectoryGenerator::StackOptiDep() {
double num_f = 3; // 4 + 4 : only start and target position has fixed derivatives
double num_p = 3 * m; // All other derivatives are free
double num_d = 6 * m;
MatrixXd Ct;
Ct = MatrixXd::Zero(num_d, num_f + num_p);
Ct(0, 0) = 1;
Ct(2, 1) = 1;
Ct(4, 2) = 1; // Stack the start point
Ct(6 * (m - 1) + 1, 3) = 1; // Stack the end point
Ct(6 * (m - 1) + 3, 4) = 1;
Ct(6 * (m - 1) + 5, 5) = 1;
if (m > 1) {
Ct(1, 6) = 1;
Ct(3, 7) = 1;
Ct(5, 8) = 1;
Ct(6 * (m - 1) + 0, 3 * m + 0) = 1;
Ct(6 * (m - 1) + 2, 3 * m + 1) = 1;
Ct(6 * (m - 1) + 4, 3 * m + 2) = 1;
for (int j = 2; j < m; j++) {
Ct(6 * (j - 1) + 0, 6 + 3 * (j - 2) + 0) = 1;
Ct(6 * (j - 1) + 1, 6 + 3 * (j - 1) + 0) = 1;
Ct(6 * (j - 1) + 2, 6 + 3 * (j - 2) + 1) = 1;
Ct(6 * (j - 1) + 3, 6 + 3 * (j - 1) + 1) = 1;
Ct(6 * (j - 1) + 4, 6 + 3 * (j - 2) + 2) = 1;
Ct(6 * (j - 1) + 5, 6 + 3 * (j - 1) + 2) = 1;
}
}
_C = Ct.transpose();
_L = _A.inverse() * Ct;
MatrixXd B = _A.inverse();
_R = _C * B.transpose() * _Q * (_A.inverse()) * Ct;
_Rff.resize(3, 3);
_Rfp.resize(3, 3 * m);
_Rpf.resize(3 * m, 3);
_Rpp.resize(3 * m, 3 * m);
_Rff = _R.block(0, 0, 3, 3);
_Rfp = _R.block(0, 3, 3, 3 * m);
_Rpf = _R.block(3, 0, 3 * m, 3);
_Rpp = _R.block(3, 3, 3 * m, 3 * m);
}
Eigen::MatrixXd TrajectoryGenerator::getA() { return _A; }
Eigen::MatrixXd TrajectoryGenerator::getQ() { return _Q; }
Eigen::MatrixXd TrajectoryGenerator::getC() { return _C; }
Eigen::MatrixXd TrajectoryGenerator::getL() { return _L; }
Eigen::MatrixXd TrajectoryGenerator::getR() { return _R; }
Eigen::MatrixXd TrajectoryGenerator::getRff() { return _Rff; }
Eigen::MatrixXd TrajectoryGenerator::getRpp() { return _Rpp; }
Eigen::MatrixXd TrajectoryGenerator::getRpf() { return _Rpf; }
Eigen::MatrixXd TrajectoryGenerator::getRfp() { return _Rfp; }

235
flightlib/src/grad_traj_optimization/traj_optimization_bridge.cpp Normal file
View File

@@ -0,0 +1,235 @@
#include "flightlib/grad_traj_optimization/traj_optimization_bridge.h"
namespace traj_opt {
std::shared_ptr<sdf_tools::SignedDistanceField> SdfConstruction(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Eigen::Vector3d &map_boundary_min_sdf,
Eigen::Vector3d &map_boundary_max_sdf) {
pcl::PointXYZ min, max;
pcl::getMinMax3D(*cloud, min, max);
// sdf collision map parameter
const double x_size = max.x - min.x;
const double z_size = max.z - min.z;
const double y_size = max.y - min.y;
Eigen::Translation3d origin_translation(min.x, min.y, min.z);
Eigen::Quaterniond origin_rotation(1.0, 0.0, 0.0, 0.0);
const Eigen::Isometry3d origin_transform = origin_translation * origin_rotation;
const std ::string frame = "world";
map_boundary_min_sdf = Eigen::Vector3d(min.x, min.y, min.z);
map_boundary_max_sdf = Eigen::Vector3d(max.x, max.y, max.z);
// create map
sdf_tools ::COLLISION_CELL cell;
cell.occupancy = 0.0;
cell.component = 0;
const sdf_tools::COLLISION_CELL oob_cell = cell;
double resolution_sdf = 0.2;
sdf_tools::CollisionMapGrid collision_map(origin_transform, frame, resolution_sdf, x_size, y_size, z_size, oob_cell);
// add obstacles set in launch file
std::cout << "Generate map..." << std::endl;
sdf_tools::COLLISION_CELL obstacle_cell(1.0);
// add the generated obstacles into collision map （flightmare点云直接建图行列偶尔有全空的情况, 但不影响）
// 点云分辨率改为0.1地图分辨率0.2，就不存在空行的问题; 地图分辨率0.2时，用pt.x + 0.001, pt.y + 0.001, pt.z + 0.001可避免空行且不会造成地图偏移
for (int i = 0; i < cloud->points.size(); i++) {
pcl::PointXYZ pt = cloud->points[i];
collision_map.Set(pt.x, pt.y, pt.z, obstacle_cell);
}
// Build the signed distance field
float oob_value = INFINITY;
std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>> sdf_with_extrema = collision_map.ExtractSignedDistanceField(oob_value);
sdf_tools::SignedDistanceField sdf_for_traj_optimization = sdf_with_extrema.first;
cout << "Signed Distance Field Build!" << endl;
return std::make_shared<sdf_tools::SignedDistanceField>(sdf_for_traj_optimization);
}
} // namespace traj_opt
TrajOptimizationBridge::TrajOptimizationBridge() {
std::string cfg_path = getenv("FLIGHTMARE_PATH") + std::string("/flightlib/configs/traj_opt.yaml");
cfg_ = YAML::LoadFile(cfg_path);
loadParam(cfg_);
resolution = 0.2; // Must be the same as the map in SdfConstruction()
df_.resize(9);
dp_.resize(9);
getLatticeGuiding(lattice_nodes, horizon_num, vertical_num, radio_num, vel_num, horizon_fov, vertical_fov, radio_range, vel_fov, vel_prefile);
}
TrajOptimizationBridge::~TrajOptimizationBridge() {}
// Explanation: In grad_traj_optimization of the current project, dp refers to the end_state, and df refers to the initial_state
void TrajOptimizationBridge::getCostAndGradient(const std::vector<double> &dp, int id, double &cost, std::vector<double> &grad) {
// dp: the predicted X_pva, Y_pva, Z_pva in Body Frame
if (dp_.size() != dp.size()) {
std::cout << "Error: size of dp dose not match !" << std::endl;
return;
}
std::vector<double> dp_b = dp;
// Transform to world frame.
Eigen::Vector3d Pb, Vb, Ab, Pw, Vw, Aw;
for (int i = 0; i < 3; i++) {
Pb(i) = dp_b[3 * i];
Vb(i) = dp_b[3 * i + 1];
Ab(i) = dp_b[3 * i + 2];
}
Eigen::Matrix3d Rwb = q_wb_.toRotationMatrix();
Pw = Rwb * Pb + pos_;
Vw = Rwb * Vb;
Aw = Rwb * Ab;
for (int i = 0; i < 3; i++) {
dp_[3 * i] = Pw(i);
dp_[3 * i + 1] = Vw(i);
dp_[3 * i + 2] = Aw(i);
}
// ----------------------------main optimization procedure--------------------------
GradTrajOptimizer grad_traj_opt(cfg_);
grad_traj_opt.setSignedDistanceField(sdf_, resolution);
grad_traj_opt.setBoundary(map_boundary_min_, map_boundary_max_);
grad_traj_opt.setGoal(goal_);
double cost_;
std::vector<double> grad_w, grad_b;
grad_traj_opt.getCostAndGradient(df_, dp_, cost_, grad_w);
Eigen::Vector3d grad_pb, grad_vb, grad_ab, grad_pw, grad_vw, grad_aw;
for (int i = 0; i < 3; i++) {
grad_pw(i) = grad_w[3 * i];
grad_vw(i) = grad_w[3 * i + 1];
grad_aw(i) = grad_w[3 * i + 2];
}
grad_pb = Rwb.transpose() * grad_pw;
grad_vb = Rwb.transpose() * grad_vw;
grad_ab = Rwb.transpose() * grad_aw;
grad_b.resize(grad_w.size());
for (int i = 0; i < 3; i++) {
grad_b[3 * i] = grad_pb(i);
grad_b[3 * i + 1] = grad_vb(i);
grad_b[3 * i + 2] = grad_ab(i);
}
cost = cost_;
grad = grad_b; // x_pva, y_pva, z_pva
}
void TrajOptimizationBridge::setMap(std::shared_ptr<sdf_tools::SignedDistanceField> sdf_for_traj_optimization, Eigen::Vector3d &map_boundary_min,
Eigen::Vector3d &map_boundary_max) {
map_boundary_min_ = map_boundary_min;
map_boundary_max_ = map_boundary_max;
sdf_ = sdf_for_traj_optimization;
}
void TrajOptimizationBridge::setState(Eigen::Vector3d pos, Eigen::Quaterniond q, Eigen::Vector3d vel, Eigen::Vector3d acc) {
pos_ = pos;
q_wb_ = q;
vel_ = vel;
acc_ = acc;
for (int i = 0; i < 3; i++) {
df_[3 * i] = pos(i);
df_[3 * i + 1] = vel(i);
df_[3 * i + 2] = acc(i);
}
}
// Explanation: In grad_traj_optimization of the current project, dp refers to the end_state, and df refers to the initial_state
void TrajOptimizationBridge::getNextStateAndCost(const std::vector<double> &dp, double &cost, Eigen::Vector3d &pos, Eigen::Vector3d &vel,
Eigen::Vector3d &acc, double sim_t) {
// dp: xyz_pva (in Body Frame)
if (dp_.size() != dp.size()) {
std::cout << "Error: size of dp dose not match !" << std::endl;
return;
}
std::vector<double> dp_b = dp;
Eigen::Vector3d Pb, Vb, Ab, Pw, Vw, Aw;
for (int i = 0; i < 3; i++) {
Pb(i) = dp_b[3 * i];
Vb(i) = dp_b[3 * i + 1];
Ab(i) = dp_b[3 * i + 2];
}
Eigen::Matrix3d Rwb = q_wb_.toRotationMatrix();
Pw = Rwb * Pb + pos_;
Vw = Rwb * Vb;
Aw = Rwb * Ab;
for (int i = 0; i < 3; i++) {
dp_[3 * i] = Pw(i);
dp_[3 * i + 1] = Vw(i);
dp_[3 * i + 2] = Aw(i);
}
GradTrajOptimizer grad_traj_opt(cfg_);
grad_traj_opt.setSignedDistanceField(sdf_, resolution);
grad_traj_opt.setBoundary(map_boundary_min_, map_boundary_max_);
grad_traj_opt.setGoal(goal_);
double cost_;
std::vector<double> grad_w;
grad_traj_opt.getCostAndGradient(df_, dp_, cost_, grad_w); // Df is set here
grad_traj_opt.getCoefficientFromDerivative(pred_coeff_, dp_); // get coefficient by Dp and Df
cost = cost_;
getPositionFromCoeff(pos, pred_coeff_, 0, sim_t);
getVelocityFromCoeff(vel, pred_coeff_, 0, sim_t);
getAccelerationFromCoeff(acc, pred_coeff_, 0, sim_t);
}
/**
set dp and get the coeffs for getNextState() function.
dp is in the world frame because this func only used in real flight and
the prediction must be tramsformed to world frame in python to avoid the attitude inconsistency caused by latency
*/
void TrajOptimizationBridge::solveBVP(const std::vector<double> &dp) {
// dp: xyz_pva given by python（in World Frame, 除了位置没加机身的偏移）
if (dp_.size() != dp.size()) {
std::cout << "Error: size of dp dose not match !" << std::endl;
return;
}
std::vector<double> dp_w = dp;
Eigen::Vector3d Pb, Vb, Ab, Pw, Vw, Aw;
for (int i = 0; i < 3; i++) {
Pw(i) = dp_w[3 * i];
Vw(i) = dp_w[3 * i + 1];
Aw(i) = dp_w[3 * i + 2];
}
Pw = Pw + pos_;
// Eigen::Matrix3d Rwb = q_wb_.toRotationMatrix();
// Pw = Rwb * Pb + pos_;
// Vw = Rwb * Vb;
// Aw = Rwb * Ab;
double traj_time = 2 * cfg_["radio_range"].as<double>() / cfg_["vel_max"].as<double>();
pred_coeff_ = solveCoeffFromBoundaryState(pos_, vel_, acc_, Pw, Vw, Aw, traj_time);
}
// get the next state in world frame
void TrajOptimizationBridge::getNextState(Eigen::Vector3d &pos, Eigen::Vector3d &vel, Eigen::Vector3d &acc, double sim_t) {
getPositionFromCoeff(pos, pred_coeff_, 0, sim_t);
getVelocityFromCoeff(vel, pred_coeff_, 0, sim_t);
getAccelerationFromCoeff(acc, pred_coeff_, 0, sim_t);
}
void TrajOptimizationBridge::setGoal(Eigen::Vector3d goal) {
Eigen::Vector3d goal_dir = (goal - pos_) / (goal - pos_).norm();
Eigen::Vector3d temp_goal = goal;
temp_goal = pos_ + goal_length * goal_dir;
goal_ = temp_goal;
}
void TrajOptimizationBridge::loadParam(YAML::Node &cfg) {
horizon_num = cfg["horizon_num"].as<int>();
vertical_num = cfg["vertical_num"].as<int>();
vel_num = cfg["vel_num"].as<int>();
radio_num = cfg["radio_num"].as<int>();
horizon_fov = cfg["horizon_camera_fov"].as<double>() * (horizon_num - 1) / horizon_num;
vertical_fov = cfg["vertical_camera_fov"].as<double>() * (vertical_num - 1) / vertical_num;
vel_fov = cfg["vel_fov"].as<double>();
radio_range = cfg["radio_range"].as<double>();
vel_prefile = cfg["vel_prefile"].as<double>();
goal_length = cfg["goal_length"].as<double>();
}

9
flightlib/src/objects/object_base.cpp Normal file
View File

@@ -0,0 +1,9 @@
#include "flightlib/objects/object_base.hpp"
namespace flightlib {
ObjectBase::ObjectBase() {}
ObjectBase::~ObjectBase() {}
} // namespace flightlib

308
flightlib/src/objects/quadrotor.cpp Normal file
View File

@@ -0,0 +1,308 @@
#include "flightlib/objects/quadrotor.hpp"
namespace flightlib {
Quadrotor::Quadrotor(const std::string &cfg_path)
: world_box_((Matrix<3, 2>() << -100, 100, -100, 100, -100, 100).finished()), size_(1.0, 1.0, 1.0), collision_(false) {
//
YAML::Node cfg = YAML::LoadFile(cfg_path);
// create quadrotor dynamics and update the parameters
dynamics_.updateParams(cfg);
init();
}
Quadrotor::Quadrotor(const QuadrotorDynamics &dynamics)
: world_box_((Matrix<3, 2>() << -100, 100, -100, 100, -100, 100).finished()), dynamics_(dynamics), size_(1.0, 1.0, 1.0), collision_(false) {
init();
}
Quadrotor::~Quadrotor() {}
bool Quadrotor::setState(const Ref<Vector<>> p_, const Ref<Vector<>> v_, const Quaternion q_, const Ref<Vector<>> a_, const Scalar ctl_dt) {
QuadState old_state = state_;
state_.p = p_;
state_.v = v_;
state_.q(q_);
state_.a = a_;
state_.t += ctl_dt;
constrainInWorldBox(old_state);
return true;
}
bool Quadrotor::run(const Command &cmd, const Scalar ctl_dt) {
if (!setCommand(cmd))
return false; // 限幅
return run(ctl_dt);
}
bool Quadrotor::run(const Scalar ctl_dt) {
if (!state_.valid())
return false;
if (!cmd_.valid())
return false;
QuadState old_state = state_;
QuadState next_state = state_;
// time
const Scalar max_dt = integrator_ptr_->dtMax();
Scalar remain_ctl_dt = ctl_dt;
// simulation loop
while (remain_ctl_dt > 0.0) {
const Scalar sim_dt = std::min(remain_ctl_dt, max_dt);
const Vector<4> motor_thrusts_des = cmd_.isSingleRotorThrusts() ? cmd_.thrusts : runFlightCtl(sim_dt, state_.w, cmd_);
runMotors(sim_dt, motor_thrusts_des);
// motor_thrusts_ = cmd_.thrusts;
const Vector<4> force_torques = B_allocation_ * motor_thrusts_;
// Compute linear acceleration and body torque
const Vector<3> force(0.0, 0.0, force_torques[0]);
state_.a = state_.q() * force * 1.0 / dynamics_.getMass() + gz_;
// compute body torque
state_.tau = force_torques.segment<3>(1);
// dynamics integration
integrator_ptr_->step(state_.x, sim_dt, next_state.x);
// update state and sim time
state_.qx /= state_.qx.norm();
//
state_.x = next_state.x;
remain_ctl_dt -= sim_dt;
}
state_.t += ctl_dt;
//
constrainInWorldBox(old_state);
return true;
}
void Quadrotor::init(void) {
// reset
updateDynamics(dynamics_);
reset();
}
bool Quadrotor::reset(void) {
state_.setZero();
motor_omega_.setZero();
motor_thrusts_.setZero();
return true;
}
bool Quadrotor::reset(const QuadState &state) {
if (!state.valid())
return false;
state_ = state;
motor_omega_.setZero();
motor_thrusts_.setZero();
return true;
}
/*
There is no controller (or using an ideal controller). The attitude is simply obtained from the desired acceleration.
This is because our algorithm is only concerned with the quality of the trajectory, while control is performed by external controller.
*/
void Quadrotor::runSimpleFlight(const Eigen::Vector3f &ref_acc, float ref_yaw, Eigen::Quaternionf &quat_des) {
float mass_ = 1.0;
float ONE_G = 9.8;
// float M_PI = 3.1415925;
Eigen::Vector3f force_ = mass_ * ONE_G * Eigen::Vector3f(0, 0, 1);
force_.noalias() += mass_ * ref_acc;
// Limit control angle to theta degree
float theta = M_PI / 4;
float c = cos(theta);
Eigen::Vector3f f;
f.noalias() = force_ - mass_ * ONE_G * Eigen::Vector3f(0, 0, 1);
if (Eigen::Vector3f(0, 0, 1).dot(force_ / force_.norm()) < c) {
float nf = f.norm();
float A = c * c * nf * nf - f(2) * f(2);
float B = 2 * (c * c - 1) * f(2) * mass_ * ONE_G;
float C = (c * c - 1) * mass_ * mass_ * ONE_G * ONE_G;
float s = (-B + sqrt(B * B - 4 * A * C)) / (2 * A);
force_.noalias() = s * f + mass_ * ONE_G * Eigen::Vector3f(0, 0, 1);
}
Eigen::Vector3f b1c, b2c, b3c;
Eigen::Vector3f b1d(cos(ref_yaw), sin(ref_yaw), 0);
if (force_.norm() > 1e-6)
b3c.noalias() = force_.normalized();
else
b3c.noalias() = Eigen::Vector3f(0, 0, 1);
b2c.noalias() = b3c.cross(b1d).normalized();
b1c.noalias() = b2c.cross(b3c).normalized();
Eigen::Matrix3f R;
R << b1c, b2c, b3c;
quat_des = Eigen::Quaternionf(R);
}
Vector<4> Quadrotor::runFlightCtl(const Scalar sim_dt, const Vector<3> &omega, const Command &command) {
const Scalar force = dynamics_.getMass() * command.collective_thrust;
const Vector<3> omega_err = command.omega - omega;
const Vector<3> body_torque_des = dynamics_.getJ() * Kinv_ang_vel_tau_ * omega_err + state_.w.cross(dynamics_.getJ() * state_.w);
const Vector<4> thrust_and_torque(force, body_torque_des.x(), body_torque_des.y(), body_torque_des.z());
const Vector<4> motor_thrusts_des = B_allocation_inv_ * thrust_and_torque;
return dynamics_.clampThrust(motor_thrusts_des);
}
void Quadrotor::runMotors(const Scalar sim_dt, const Vector<4> &motor_thruts_des) {
const Vector<4> motor_omega_des = dynamics_.motorThrustToOmega(motor_thruts_des);
const Vector<4> motor_omega_clamped = dynamics_.clampMotorOmega(motor_omega_des);
// simulate motors as a first-order system
const Scalar c = std::exp(-sim_dt * dynamics_.getMotorTauInv());
motor_omega_ = c * motor_omega_ + (1.0 - c) * motor_omega_clamped;
motor_thrusts_ = dynamics_.motorOmegaToThrust(motor_omega_);
motor_thrusts_ = dynamics_.clampThrust(motor_thrusts_);
}
bool Quadrotor::setCommand(const Command &cmd) {
if (!cmd.valid())
return false;
cmd_ = cmd;
// 推力角速率
if (std::isfinite(cmd_.collective_thrust))
cmd_.collective_thrust = dynamics_.clampCollectiveThrust(cmd_.collective_thrust);
if (cmd_.omega.allFinite())
cmd_.omega = dynamics_.clampBodyrates(cmd_.omega);
// 转子推力
if (cmd_.thrusts.allFinite())
cmd_.thrusts = dynamics_.clampThrust(cmd_.thrusts);
return true;
}
bool Quadrotor::setState(const QuadState &state) {
if (!state.valid())
return false;
state_ = state;
return true;
}
bool Quadrotor::setWorldBox(const Ref<Matrix<3, 2>> box) {
if (box(0, 0) >= box(0, 1) || box(1, 0) >= box(1, 1) || box(2, 0) >= box(2, 1)) {
return false;
}
world_box_ = box;
return true;
}
bool Quadrotor::constrainInWorldBox(const QuadState &old_state) {
if (!old_state.valid())
return false;
// violate world box constraint in the x-axis
if (state_.x(QS::POSX) < world_box_(0, 0) || state_.x(QS::POSX) > world_box_(0, 1)) {
state_.x(QS::POSX) = old_state.x(QS::POSX);
state_.x(QS::VELX) = 0.0;
}
// violate world box constraint in the y-axis
if (state_.x(QS::POSY) < world_box_(1, 0) || state_.x(QS::POSY) > world_box_(1, 1)) {
state_.x(QS::POSY) = old_state.x(QS::POSY);
state_.x(QS::VELY) = 0.0;
}
// violate world box constraint in the x-axis
if (state_.x(QS::POSZ) <= world_box_(2, 0) || state_.x(QS::POSZ) > world_box_(2, 1)) {
//
state_.x(QS::POSZ) = world_box_(2, 0);
// reset velocity to zero
state_.x(QS::VELX) = 0.0;
state_.x(QS::VELY) = 0.0;
// reset acceleration to zero
state_.a << 0.0, 0.0, 0.0;
// reset angular velocity to zero
state_.w << 0.0, 0.0, 0.0;
}
return true;
}
bool Quadrotor::getState(QuadState *const state) const {
if (!state_.valid())
return false;
*state = state_;
return true;
}
bool Quadrotor::getMotorThrusts(Ref<Vector<4>> motor_thrusts) const {
motor_thrusts = motor_thrusts_;
return true;
}
bool Quadrotor::getMotorOmega(Ref<Vector<4>> motor_omega) const {
motor_omega = motor_omega_;
return true;
}
bool Quadrotor::getDynamics(QuadrotorDynamics *const dynamics) const {
if (!dynamics_.valid())
return false;
*dynamics = dynamics_;
return true;
}
const QuadrotorDynamics &Quadrotor::getDynamics() { return dynamics_; }
bool Quadrotor::updateDynamics(const QuadrotorDynamics &dynamics) {
if (!dynamics.valid()) {
std::cout << "[Quadrotor] dynamics is not valid!" << std::endl;
return false;
}
dynamics_ = dynamics;
integrator_ptr_ = std::make_unique<IntegratorRK4>(dynamics_.getDynamicsFunction(), 2.5e-3);
B_allocation_ = dynamics_.getAllocationMatrix();
B_allocation_inv_ = B_allocation_.inverse();
return true;
}
bool Quadrotor::addRGBCamera(std::shared_ptr<RGBCamera> camera) {
rgb_cameras_.push_back(camera);
return true;
}
Vector<3> Quadrotor::getSize(void) const { return size_; }
Vector<3> Quadrotor::getPosition(void) const { return state_.p; }
std::vector<std::shared_ptr<RGBCamera>> Quadrotor::getCameras(void) const { return rgb_cameras_; };
bool Quadrotor::getCamera(const size_t cam_id, std::shared_ptr<RGBCamera> camera) const {
if (cam_id <= rgb_cameras_.size()) {
return false;
}
camera = rgb_cameras_[cam_id];
return true;
}
bool Quadrotor::getCollision() const { return collision_; }
int Quadrotor::getNumCamera() const { return rgb_cameras_.size(); }
} // namespace flightlib

9
flightlib/src/objects/unity_camera.cpp Normal file
View File

@@ -0,0 +1,9 @@
#include "flightlib/objects/unity_camera.hpp"
namespace flightlib {
UnityCamera::UnityCamera() {}
UnityCamera::~UnityCamera() {}
} // namespace flightlib

293
flightlib/src/ros_nodes/flight_pilot.cpp Normal file
View File

@@ -0,0 +1,293 @@
#include "flightlib/ros_nodes/flight_pilot.hpp"
namespace flightros {
FlightPilot::FlightPilot(const ros::NodeHandle& nh, const ros::NodeHandle& pnh)
: nh_(nh), pnh_(pnh), scene_id_(4), unity_ready_(false), unity_render_(false), receive_id_(0), frameID(0), main_loop_freq_(50.0) {
// quad initialization
quad_ptr_ = std::make_shared<Quadrotor>();
// load parameters
std::string cfg_path = getenv("FLIGHTMARE_PATH") + std::string("/flightlib/configs/quadrotor_ros.yaml");
YAML::Node cfg_ = YAML::LoadFile(cfg_path);
loadParams(cfg_);
configCamera(cfg_);
quad_ptr_->setSize(quad_size_);
// initialization
quad_state_.setZero();
if (scene_id_ == UnityScene::NATUREFOREST) {
quad_state_.x(QS::POSX) = 51; // 41-61
quad_state_.x(QS::POSY) = 108; // 98-118
quad_state_.x(QS::POSZ) = 34;
}
quad_ptr_->reset(quad_state_);
sgm_.reset(new sgm_gpu::SgmGpu(width_, height_));
// initialize subscriber and publisher
left_img_pub = nh_.advertise<sensor_msgs::Image>("RGB_image", 1);
stereo_pub = nh_.advertise<sensor_msgs::Image>("stereo_image", 1);
depth_pub = nh_.advertise<sensor_msgs::Image>("depth_image", 1);
cam_info_pub = nh_.advertise<sensor_msgs::CameraInfo>("camera_info", 1);
state_est_sub_ = nh_.subscribe(odom_topic_, 1, &FlightPilot::poseCallback, this);
spawn_tree_sub_ = nh_.subscribe("/spawn_tree", 1, &FlightPilot::spawnTreeCallback, this);
clear_tree_sub_ = nh_.subscribe("/clear_tree", 1, &FlightPilot::clearTreeCallback, this);
save_pc_sub_ = nh_.subscribe("/save_pc", 1, &FlightPilot::savePointcloudCallback, this);
timestamp = ros::Time::now();
// connect unity and setup unity
setUnity(unity_render_);
connectUnity();
if (!unity_ready_)
ROS_ERROR("Start the gazebo and unity first!");
spawnTreesAndSavePointCloud();
timer_main_loop_ = nh_.createTimer(ros::Rate(main_loop_freq_), &FlightPilot::mainLoopCallback, this);
}
FlightPilot::~FlightPilot() { disconnectUnity(); }
void FlightPilot::spawnTreeCallback(const std_msgs::Empty::ConstPtr& msg) {
if (!unity_ready_ || unity_bridge_ptr_ == nullptr)
return;
unity_bridge_ptr_->spawnTrees(bounding_box_, bounding_box_origin_, avg_tree_spacing_);
}
void FlightPilot::clearTreeCallback(const std_msgs::Empty::ConstPtr& msg) { unity_bridge_ptr_->rmTrees(); }
// If the point cloud is not saved when init, it also can be saved by this callback.
void FlightPilot::savePointcloudCallback(const std_msgs::Empty::ConstPtr& msg) {
Vector<3> min_corner = bounding_box_origin_ - 0.5 * bounding_box_;
Vector<3> max_corner = bounding_box_origin_ + 0.5 * bounding_box_;
unity_bridge_ptr_->generatePointcloud(min_corner, max_corner, ply_id_, ply_path_, scene_id_, pointcloud_resolution_);
}
void FlightPilot::poseCallback(const nav_msgs::Odometry::ConstPtr& msg) {
quad_state_.x[QS::POSX] = (Scalar)msg->pose.pose.position.x;
quad_state_.x[QS::POSY] = (Scalar)msg->pose.pose.position.y;
quad_state_.x[QS::POSZ] = (Scalar)msg->pose.pose.position.z;
quad_state_.x[QS::ATTW] = (Scalar)msg->pose.pose.orientation.w;
quad_state_.x[QS::ATTX] = (Scalar)msg->pose.pose.orientation.x;
quad_state_.x[QS::ATTY] = (Scalar)msg->pose.pose.orientation.y;
quad_state_.x[QS::ATTZ] = (Scalar)msg->pose.pose.orientation.z;
quad_ptr_->setState(quad_state_);
timestamp = msg->header.stamp;
}
void FlightPilot::mainLoopCallback(const ros::TimerEvent& event) {
if (!unity_render_ || !unity_ready_)
return;
frameID++;
ros::Time timestamp_ = timestamp;
unity_bridge_ptr_->getRender(frameID); // 1ms
FrameID frameID_rt;
unity_bridge_ptr_->handleOutput(frameID_rt); // 30ms
while (frameID != frameID_rt)
unity_bridge_ptr_->handleOutput(frameID_rt);
cv::Mat left_img, right_img, depth_img;
// publish RGB image
rgb_camera_left->getRGBImage(left_img);
sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", left_img).toImageMsg();
img_msg->header.stamp = timestamp_;
left_img_pub.publish(img_msg);
// publish camera info
int width = rgb_camera_left->getWidth();
int hight = rgb_camera_left->getHeight();
float fov = rgb_camera_left->getFOV();
float cx = width / 2.0;
float cy = hight / 2.0;
float fx = cy / tan(0.5 * fov * M_PI / 180.0); // 他这个特殊，fov是竖直方向的
float fy = fx;
boost::array<double, 9> K = {fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0};
sensor_msgs::CameraInfo cam_msg;
cam_msg.height = hight;
cam_msg.width = width;
cam_msg.K = K;
cam_info_pub.publish(cam_msg);
// publish depth image
if (use_depth) {
rgb_camera_left->getDepthMap(depth_img);
depth_img = depth_img * 1000.0;
depth_img = cv::min(depth_img, 20.0);
sensor_msgs::ImagePtr depth_msg = cv_bridge::CvImage(std_msgs::Header(), "32FC1", depth_img).toImageMsg();
depth_msg->header.stamp = timestamp_;
depth_pub.publish(depth_msg);
}
// publish stereo image
if (use_stereo) {
rgb_camera_right->getRGBImage(right_img);
cv::Mat stereo_(height_, width_, CV_32FC1);
computeDepthImage(left_img, right_img, &stereo_);
if (use_depth) {
// Complete the NaN values in the depth map, as the RealSense performs better than SGM.
cv::Mat mask, mask1, mask2;
cv::compare(stereo_, 0, mask1, cv::CMP_EQ); // 将 A 中为 0 的位置置为 255，其余位置置为 0
cv::compare(stereo_, 20, mask2, cv::CMP_GT); // 将 A 中大于 20 的位置置为 255，其余位置置为 0
mask = mask1 | mask2; // 将两个掩码进行逻辑或操作
depth_img.copyTo(stereo_, mask); // 将 B 中 mask 为 255 的位置的值复制到 A 中
}
sensor_msgs::ImagePtr stereo_msg = cv_bridge::CvImage(std_msgs::Header(), "32FC1", stereo_).toImageMsg();
stereo_msg->header.stamp = timestamp_;
stereo_pub.publish(stereo_msg);
}
}
bool FlightPilot::setUnity(const bool render) {
unity_render_ = render;
if (unity_render_ && unity_bridge_ptr_ == nullptr) {
// create unity bridge
unity_bridge_ptr_ = UnityBridge::getInstance();
unity_bridge_ptr_->addQuadrotor(quad_ptr_);
ROS_INFO("[%s] Unity Bridge is created.", pnh_.getNamespace().c_str());
}
return true;
}
bool FlightPilot::spawnTreesAndSavePointCloud() {
if (!unity_ready_ || unity_bridge_ptr_ == nullptr)
return false;
unity_bridge_ptr_->spawnTrees(bounding_box_, bounding_box_origin_, avg_tree_spacing_);
// Saving point cloud during the testing is much time-consuming, but can be used for evaluation.
// The following can be commented if evaluation is not needed.
Vector<3> min_corner = bounding_box_origin_ - 0.5 * bounding_box_;
Vector<3> max_corner = bounding_box_origin_ + 0.5 * bounding_box_;
unity_bridge_ptr_->generatePointcloud(min_corner, max_corner, ply_id_, ply_path_, scene_id_, pointcloud_resolution_);
return true;
}
bool FlightPilot::connectUnity() {
if (!unity_render_ || unity_bridge_ptr_ == nullptr)
return false;
unity_ready_ = unity_bridge_ptr_->connectUnity(scene_id_);
return unity_ready_;
}
bool FlightPilot::disconnectUnity() {
if (unity_render_ && unity_bridge_ptr_ != nullptr)
;
unity_bridge_ptr_->disconnectUnity();
unity_ready_ = false;
}
bool FlightPilot::loadParams(const YAML::Node& cfg) {
// ros
main_loop_freq_ = cfg["main_loop_freq"].as<int>();
odom_topic_ = cfg["odom_topic"].as<std::string>();
// camera
width_ = cfg["rgb_camera_left"]["width"].as<int>();
height_ = cfg["rgb_camera_left"]["height"].as<int>();
fov_ = cfg["rgb_camera_left"]["fov"].as<Scalar>();
use_depth = cfg["rgb_camera_left"]["enable_depth"].as<bool>();
use_stereo = cfg["rgb_camera_right"]["on"].as<bool>();
// scence
scene_id_ = cfg["scene_id"].as<int>();
unity_render_ = cfg["unity_render"].as<bool>();
Scalar quad_size_i = cfg["quad_size"].as<Scalar>();
quad_size_ = Vector<3>(quad_size_i, quad_size_i, quad_size_i);
avg_tree_spacing_ = cfg["unity"]["avg_tree_spacing"].as<Scalar>();
for (int i = 0; i < 3; ++i) {
bounding_box_(i) = cfg["unity"]["bounding_box"][i].as<Scalar>();
bounding_box_origin_(i) = cfg["unity"]["bounding_box_origin"][i].as<Scalar>();
}
pointcloud_resolution_ = cfg["unity"]["pointcloud_resolution"].as<Scalar>();
ply_path_ = getenv("FLIGHTMARE_PATH") + cfg["ply_path"].as<std::string>();
if (!std::filesystem::exists(ply_path_)) {
std::filesystem::create_directories(ply_path_);
std::cout << "Directory created: " << ply_path_ << std::endl;
}
return true;
}
void FlightPilot::computeDepthImage(const cv::Mat& left_frame, const cv::Mat& right_frame, cv::Mat* const depth) {
cv::Mat disparity(height_, width_, CV_8UC1);
sgm_->computeDisparity(left_frame, right_frame, disparity);
disparity.convertTo(disparity, CV_32FC1);
// compute depth from disparity
float f = (width_ / 2.0) / std::tan((M_PI * (fov_ / 180.0)) / 2.0);
// depth = stereo_baseline_ * f / disparity
for (int r = 0; r < height_; ++r) {
for (int c = 0; c < width_; ++c) {
if (disparity.at<float>(r, c) == 0.0f) {
depth->at<float>(r, c) = 0.0f;
} else if (disparity.at<float>(r, c) == 255.0f) {
depth->at<float>(r, c) = 0.0f;
} else {
depth->at<float>(r, c) = static_cast<float>(stereo_baseline_) * f / disparity.at<float>(r, c);
}
}
}
}
bool FlightPilot::configCamera(const YAML::Node& cfg) {
if (!cfg["rgb_camera_left"] || !cfg["rgb_camera_right"]) {
ROS_ERROR("Cannot config stereo Camera");
return false;
}
// create left camera --------------------------------------------
rgb_camera_left = std::make_shared<RGBCamera>();
// load camera settings
std::vector<Scalar> t_BC_vec = cfg["rgb_camera_left"]["t_BC"].as<std::vector<Scalar>>();
std::vector<Scalar> r_BC_vec = cfg["rgb_camera_left"]["r_BC"].as<std::vector<Scalar>>();
Vector<3> t_BC(t_BC_vec.data());
Matrix<3, 3> r_BC = (AngleAxis(r_BC_vec[0] * M_PI / 180.0, Vector<3>::UnitX()) * AngleAxis(r_BC_vec[1] * M_PI / 180.0, Vector<3>::UnitY()) *
AngleAxis(r_BC_vec[2] * M_PI / 180.0, Vector<3>::UnitZ()))
.toRotationMatrix(); // the rotation order has been verified
// Convert the horizontal FOV (usually used) to vertical FOV (flightmare).
Scalar rgb_fov_deg_ = cfg["rgb_camera_left"]["fov"].as<Scalar>();
double hor_fov_radians = (M_PI * (rgb_fov_deg_ / 180.0));
Scalar img_rows_ = cfg["rgb_camera_left"]["height"].as<Scalar>();
Scalar img_cols_ = cfg["rgb_camera_left"]["width"].as<Scalar>();
double flightmare_fov = 2. * std::atan(std::tan(hor_fov_radians / 2) * img_rows_ / img_cols_);
flightmare_fov = (flightmare_fov / M_PI) * 180.0;
rgb_camera_left->setFOV(flightmare_fov);
rgb_camera_left->setWidth(cfg["rgb_camera_left"]["width"].as<int>());
rgb_camera_left->setHeight(cfg["rgb_camera_left"]["height"].as<int>());
rgb_camera_left->setRelPose(t_BC, r_BC);
rgb_camera_left->enableOpticalFlow(cfg["rgb_camera_left"]["enable_opticalflow"].as<bool>());
rgb_camera_left->enableSegmentation(cfg["rgb_camera_left"]["enable_segmentation"].as<bool>());
rgb_camera_left->enableDepth(cfg["rgb_camera_left"]["enable_depth"].as<bool>());
// add camera to the quadrotor
quad_ptr_->addRGBCamera(rgb_camera_left);
// create right camera --------------------------------------------
if (use_stereo) {
rgb_camera_right = std::make_shared<RGBCamera>();
// load camera settings
std::vector<Scalar> t_BC_vec_r = cfg["rgb_camera_right"]["t_BC"].as<std::vector<Scalar>>();
std::vector<Scalar> r_BC_vec_r = cfg["rgb_camera_right"]["r_BC"].as<std::vector<Scalar>>();
Vector<3> t_BC_r(t_BC_vec_r.data());
Matrix<3, 3> r_BC_r = (AngleAxis(r_BC_vec[0] * M_PI / 180.0, Vector<3>::UnitX()) * AngleAxis(r_BC_vec[1] * M_PI / 180.0, Vector<3>::UnitY()) *
AngleAxis(r_BC_vec[2] * M_PI / 180.0, Vector<3>::UnitZ()))
.toRotationMatrix();
rgb_camera_right->setFOV(flightmare_fov);
rgb_camera_right->setWidth(cfg["rgb_camera_left"]["width"].as<int>());
rgb_camera_right->setHeight(cfg["rgb_camera_left"]["height"].as<int>());
rgb_camera_right->setRelPose(t_BC_r, r_BC_r);
rgb_camera_right->enableOpticalFlow(false);
rgb_camera_right->enableSegmentation(false);
rgb_camera_right->enableDepth(false);
// add camera to the quadrotor
quad_ptr_->addRGBCamera(rgb_camera_right);
stereo_baseline_ = fabs(t_BC(1) - t_BC_r(1));
}
return true;
}
} // namespace flightros

13
flightlib/src/ros_nodes/flight_pilot_node.cpp Normal file
View File

@@ -0,0 +1,13 @@
#include <ros/ros.h>
#include "flightlib/ros_nodes/flight_pilot.hpp"
int main(int argc, char** argv) {
ros::init(argc, argv, "flight_pilot");
flightros::FlightPilot pilot(ros::NodeHandle(), ros::NodeHandle("~"));
// spin the ros
ros::spin();
return 0;
}

101
flightlib/src/ros_nodes/map_visual_node.cpp Normal file
View File

@@ -0,0 +1,101 @@
// Publish the surrounding point cloud map based on the drone's position for visualization.
#include <nav_msgs/Odometry.h>
#include <pcl/common/common.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <time.h>
#include <yaml-cpp/yaml.h>
#include <Eigen/Core>
#include "visualization_msgs/Marker.h"
namespace map_visual {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
void pcl_input() {
std::string cfg_path = getenv("FLIGHTMARE_PATH") + std::string("/flightlib/configs/quadrotor_ros.yaml");
YAML::Node cfg_ = YAML::LoadFile(cfg_path);
std::string ply_path_ = getenv("FLIGHTMARE_PATH") + cfg_["ply_path"].as<std::string>() + "pointcloud-0.ply";
pcl::io::loadPLYFile<pcl::PointXYZ>(ply_path_, *cloud);
std::cout << "size of pointcloud: " << cloud->points.size() << std::endl;
}
void odom_cb(const nav_msgs::Odometry::ConstPtr odom_msg, ros::Publisher* local_map_pub, ros::Publisher* mesh_pub,
tf::TransformBroadcaster* uav_tf_br) {
// 1. publish tf
tf::Transform transform;
transform.setOrigin(tf::Vector3(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z));
tf::Quaternion q(0, 0, 0, 1);
// tf::Quaternion q(odom_msg->pose.pose.orientation.x, odom_msg->pose.pose.orientation.y,
// odom_msg->pose.pose.orientation.z, odom_msg->pose.pose.orientation.w);
transform.setRotation(q);
uav_tf_br->sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "uav"));
// 2. publish map
Eigen::Vector3f pose_cur(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z);
Eigen::Vector3f local_map_half_size(8, 8, 1);
pcl::PointCloud<pcl::PointXYZ>::Ptr local_map(new pcl::PointCloud<pcl::PointXYZ>);
for (auto& x : cloud->points) {
if (x.z > pose_cur(2) - 6 && x.z < pose_cur(2) + local_map_half_size(2) && x.x > pose_cur(0) - local_map_half_size(0) &&
x.x < pose_cur(0) + local_map_half_size(0) && x.y > pose_cur(1) - local_map_half_size(1) && x.y < pose_cur(1) + local_map_half_size(1)) {
local_map->points.push_back(x);
}
}
pcl_conversions::toPCL(ros::Time::now(), local_map->header.stamp);
local_map->header.frame_id = "world";
local_map_pub->publish(local_map);
// 3. publish UAV model
std::string mesh_resource = std::string("file://") + getenv("FLIGHTMARE_PATH") + std::string("/flightlib/src/ros_nodes/model/uav.dae");
visualization_msgs::Marker meshROS;
meshROS.header.frame_id = "world";
meshROS.header.stamp = ros::Time::now();
meshROS.ns = "mesh";
meshROS.id = 0;
meshROS.type = visualization_msgs::Marker::MESH_RESOURCE;
meshROS.action = visualization_msgs::Marker::ADD;
meshROS.pose.position.x = odom_msg->pose.pose.position.x - 0.2;
meshROS.pose.position.y = odom_msg->pose.pose.position.y;
meshROS.pose.position.z = odom_msg->pose.pose.position.z;
meshROS.pose.orientation.w = odom_msg->pose.pose.orientation.w;
meshROS.pose.orientation.x = odom_msg->pose.pose.orientation.x;
meshROS.pose.orientation.y = odom_msg->pose.pose.orientation.y;
meshROS.pose.orientation.z = odom_msg->pose.pose.orientation.z;
float scale = 2;
meshROS.scale.x = scale;
meshROS.scale.y = scale;
meshROS.scale.z = scale;
meshROS.color.a = 1;
meshROS.color.r = 1;
meshROS.color.g = 1;
meshROS.color.b = 1;
meshROS.mesh_resource = mesh_resource;
meshROS.mesh_use_embedded_materials = true;
mesh_pub->publish(meshROS);
}
} // namespace map_visual
using namespace map_visual;
int main(int argc, char** argv) {
pcl_input();
ros::init(argc, argv, "map_visual");
ros::NodeHandle nh;
tf::TransformBroadcaster uav_tf_br;
ros::Publisher local_map_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("/local_map", 1);
ros::Publisher mesh_pub = nh.advertise<visualization_msgs::Marker>("/uav_mesh", 1);
ros::Subscriber odom_sub =
nh.subscribe<nav_msgs::Odometry>("/juliett/ground_truth/odom", 1, boost::bind(&odom_cb, _1, &local_map_pub, &mesh_pub, &uav_tf_br));
std::cout << "Map visual node OK!" << std::endl;
ros::spin();
}

10597
flightlib/src/ros_nodes/model/uav.dae Executable file
View File

File diff suppressed because one or more lines are too long

145
flightlib/src/ros_nodes/traj_eval_node.cpp Normal file
View File

@@ -0,0 +1,145 @@
// A very rough evaluation: Input the map point cloud, receive odometry, and calculate: execution time,
// trajectory length, distance to the nearest obstacle, and dynamic cost (logging method to be optimized).
#include <nav_msgs/Odometry.h>
#include <pcl/common/common.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>
#include <ros/ros.h>
#include <time.h>
#include <Eigen/Core>
#include "flightlib/controller/ctrl_ref.h"
namespace traj_eval {
float start_record = -39;
float finish_record = 18;
// eval
std::ofstream log_, log_x, ctrl_log;
Eigen::Vector3f pose_last;
Eigen::Vector3f acc_last;
ros::Time start, end;
bool odom_init = false;
bool odom_finish = false;
bool first_cmd = true;
float length_ = 0;
float dist_ = 0;
float ctrl_cost_ = 0;
int num_ = 0;
// map
pcl::search::KdTree<pcl::PointXYZ> kdtree;
float resolution = 0.2;
Eigen::Vector3i m_size;
Eigen::Vector3i m_origin;
void map_input() {
std::string ply_path_ = getenv("FLIGHTMARE_PATH") + std::string("/flightrender/RPG_Flightmare/pointcloud_data/pointcloud-0.ply");
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPLYFile<pcl::PointXYZ>(ply_path_, *cloud);
pcl::PointXYZ min, max;
pcl::getMinMax3D(*cloud, min, max);
m_size(0) = ceil((max.x - min.x) / resolution);
m_size(1) = ceil((max.y - min.y) / resolution);
m_size(2) = ceil((max.z - min.z + 0.01) / resolution);
m_origin = Eigen::Vector3i(min.x, min.y, min.z);
kdtree.setInputCloud(cloud);
}
int to_id(int x, int y, int z) { return x * m_size(1) * m_size(2) + y * m_size(2) + z; }
float distance_at(Eigen::Vector3f pose) {
pcl::PointXYZ drone_;
drone_.x = pose(0);
drone_.y = pose(1);
drone_.z = pose(2);
int K = 1;
std::vector<int> indices(K);
std::vector<float> distances(K); // 存储近邻点对应距离的平方
kdtree.nearestKSearch(drone_, K, indices, distances);
return std::sqrt(distances[0]);
}
void odom_cb(const nav_msgs::Odometry::Ptr odom_msg) {
if (!odom_init && odom_msg->pose.pose.position.x > start_record) {
odom_init = true;
pose_last = Eigen::Vector3f(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z);
start = ros::Time::now();
std::cout << "start!" << std::endl;
return;
}
if (!odom_init || odom_finish)
return;
Eigen::Vector3f pose_cur(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z);
length_ += (pose_cur - pose_last).norm();
dist_ += distance_at(pose_cur);
log_ << distance_at(pose_cur) << std::endl;
log_x << pose_cur(0) << std::endl;
pose_last = pose_cur;
num_++;
if (odom_msg->pose.pose.position.x > finish_record) {
odom_finish = true;
end = ros::Time::now();
std::cout << "finish! \n time:" << (end - start).toSec() << " s,\nlength:" << length_ << " m,\ndist:" << dist_ / num_
<< " m,\nctrl cost:" << ctrl_cost_ << " m2/s5" << std::endl;
log_.close();
log_x.close();
ctrl_log.close();
}
}
void ctrl_cb(const quad_pos_ctrl::ctrl_ref& ctrl_msg) {
if (!odom_init || odom_finish)
return;
if (first_cmd) {
first_cmd = false;
acc_last = Eigen::Vector3f(ctrl_msg.acc_ref[0], -ctrl_msg.acc_ref[1], -ctrl_msg.acc_ref[2]);
return;
}
Eigen::Vector3f cur_acc(ctrl_msg.acc_ref[0], -ctrl_msg.acc_ref[1], -ctrl_msg.acc_ref[2]);
Eigen::Vector3f d_acc = (acc_last - cur_acc) / 0.02;
float acc_norm2 = d_acc.dot(d_acc);
ctrl_cost_ += 0.02 * acc_norm2;
acc_last = cur_acc;
ctrl_log << ctrl_msg.pos_ref[0] << ',';
ctrl_log << ctrl_msg.pos_ref[1] << ',';
ctrl_log << ctrl_msg.pos_ref[2] << ',';
ctrl_log << ctrl_msg.vel_ref[0] << ',';
ctrl_log << ctrl_msg.vel_ref[1] << ',';
ctrl_log << ctrl_msg.vel_ref[2] << ',';
ctrl_log << ctrl_msg.acc_ref[0] << ',';
ctrl_log << ctrl_msg.acc_ref[1] << ',';
ctrl_log << ctrl_msg.acc_ref[2] << std::endl;
}
} // namespace traj_eval
using namespace traj_eval;
int main(int argc, char** argv) {
map_input();
// Move to the same log file.
std::string log_file = getenv("FLIGHTMARE_PATH") + std::string("/run/utils/dist.csv");
std::cout << "log path:" << log_file << std::endl;
log_.open(log_file.c_str(), std::ios::out);
std::string log_file2 = getenv("FLIGHTMARE_PATH") + std::string("/run/utils/dist_x.csv");
log_x.open(log_file2.c_str(), std::ios::out);
std::string log_file3 = getenv("FLIGHTMARE_PATH") + std::string("/run/utils/ctrl_log.csv");
ctrl_log.open(log_file3.c_str(), std::ios::out);
ros::init(argc, argv, "traj_eval");
ros::NodeHandle nh;
ros::Subscriber odom_sub = nh.subscribe("/juliett/ground_truth/odom", 1, odom_cb);
ros::Subscriber ctrl_sub = nh.subscribe("/juliett_pos_ctrl_node/controller/ctrl_ref", 1, ctrl_cb);
ros::spin();
}

347
flightlib/src/ros_nodes/yopo_planner_node.cpp Normal file
View File

@@ -0,0 +1,347 @@
#include <nav_msgs/Odometry.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <ros/ros.h>
#include <std_msgs/Float32MultiArray.h>
#include <yaml-cpp/yaml.h>
#include <Eigen/Core>
#include "flightlib/controller/PositionCommand.h"
#include "flightlib/controller/ctrl_ref.h"
#include "flightlib/grad_traj_optimization/opt_utile.h"
#include "flightlib/grad_traj_optimization/traj_optimization_bridge.h"
namespace yopo_net {
nav_msgs::Odometry odom_msg;
quad_pos_ctrl::ctrl_ref ctrl_ref_last;
quadrotor_msgs::PositionCommand pos_cmd_last;
bool odom_init = false;
bool odom_ref_init = false;
bool yopo_init = false;
bool done = false;
float traj_time = 2.0;
float sample_t = 0.0;
float last_yaw_ = 0; // NWU
TrajOptimizationBridge* traj_opt_bridge;
TrajOptimizationBridge* traj_opt_bridge_for_vis;
std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> lattice_nodes;
Eigen::Vector3d goal_(100, 0, 2);
Eigen::Quaterniond quat_(1, 0, 0, 0);
Eigen::Vector3d last_pos_(0, 0, 0), last_vel_(0, 0, 0), last_acc_(0, 0, 0);
ros::Publisher trajs_visual_pub, best_traj_visual_pub, state_ref_pub, ctrl_pub, mpc_ctrl_pub, so3_ctrl_pub, lattice_trajs_visual_pub;
ros::Subscriber odom_sub, odom_ref_sub, yopo_best_sub, yopo_all_sub, goal_sub;
void odom_cb(const nav_msgs::Odometry::Ptr msg) {
odom_msg = *msg;
odom_init = true;
quat_.w() = odom_msg.pose.pose.orientation.w;
quat_.x() = odom_msg.pose.pose.orientation.x;
quat_.y() = odom_msg.pose.pose.orientation.y;
quat_.z() = odom_msg.pose.pose.orientation.z;
if (!odom_ref_init) {
last_pos_ << odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, odom_msg.pose.pose.position.z;
last_vel_ << 0, 0, 0;
last_acc_ << 0, 0, 0;
}
// check if reach the goal
Eigen::Vector3d dist(odom_msg.pose.pose.position.x - goal_(0), odom_msg.pose.pose.position.y - goal_(1),
odom_msg.pose.pose.position.z - goal_(2));
if (dist.norm() < 4) {
if (!done)
printf("Done!\n");
done = true;
}
}
void goal_cb(const std_msgs::Float32MultiArray::Ptr msg) {
Eigen::Vector3d last_goal = goal_;
goal_(0) = msg->data[0];
goal_(1) = msg->data[1];
goal_(2) = msg->data[2];
if (last_goal != goal_)
done = false;
}
void traj_to_pcl(TrajOptimizationBridge* traj_opt_bridge_input, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, double cost = 0.0) {
for (float dt = 0.0; dt < traj_time; dt = dt + 0.05) {
Eigen::Vector3d next_pos, next_vel, next_acc;
traj_opt_bridge_input->getNextState(next_pos, next_vel, next_acc, dt);
pcl::PointXYZI clrP;
clrP.x = next_pos(0);
clrP.y = next_pos(1);
clrP.z = next_pos(2);
clrP.intensity = cost;
cloud->points.push_back(clrP);
}
}
void yopo_cb(const std_msgs::Float32MultiArray::ConstPtr msg) {
if (!odom_init)
return;
std::vector<double> endstate;
for (int i = 0; i < msg->data.size(); i++) {
endstate.push_back(static_cast<double>(msg->data[i]));
}
traj_opt_bridge->setState(last_pos_.cast<double>(), quat_.cast<double>(), last_vel_.cast<double>(), last_acc_.cast<double>());
traj_opt_bridge->solveBVP(endstate);
// int milliseconds_yopo_pub = msg->layout.data_offset; // 预测时采用的状态和图像的时间戳(ms)
// int milliseconds_now = uint64_t(ros::Time::now().toSec() * 1000) % 1000000;
// double delta_t = double(milliseconds_now - milliseconds_yopo_pub) / 1000;
// std::cout<<"yopo开始预测到当前时间: "<<delta_t<<std::endl;
sample_t = 0.0; // = delta_t
pcl::PointCloud<pcl::PointXYZI>::Ptr best_traj_cld(new pcl::PointCloud<pcl::PointXYZI>);
traj_to_pcl(traj_opt_bridge, best_traj_cld);
pcl_conversions::toPCL(ros::Time::now(), best_traj_cld->header.stamp); // for test
best_traj_cld->header.frame_id = "world";
best_traj_visual_pub.publish(best_traj_cld);
yopo_init = true;
}
void trajs_vis_cb(const std_msgs::Float32MultiArray::ConstPtr msg) {
if (!odom_init)
return;
// ---------------- visualization of all trajs --------------------
std::vector<std::vector<double>> endstates_b;
endstates_b.resize(msg->layout.dim[0].size);
std::vector<double> scores;
for (int i = 0; i < msg->layout.dim[0].size; i++) {
for (int j = 0; j < msg->layout.dim[1].size - 1; j++) {
endstates_b[i].push_back(static_cast<double>(msg->data[i * msg->layout.dim[1].size + j]));
}
scores.push_back(static_cast<double>(msg->data[(i + 1) * msg->layout.dim[1].size - 1]));
}
traj_opt_bridge_for_vis->setState(last_pos_.cast<double>(), quat_.cast<double>(), last_vel_.cast<double>(), last_acc_.cast<double>());
pcl::PointCloud<pcl::PointXYZI>::Ptr trajs_cld(new pcl::PointCloud<pcl::PointXYZI>);
for (size_t i = 0; i < endstates_b.size(); i++) {
traj_opt_bridge_for_vis->solveBVP(endstates_b[i]);
traj_to_pcl(traj_opt_bridge_for_vis, trajs_cld, scores[i]);
}
pcl_conversions::toPCL(ros::Time::now(), trajs_cld->header.stamp);
trajs_cld->header.frame_id = "world";
trajs_visual_pub.publish(trajs_cld);
// ---------------- visualization of lattice ------------------------
pcl::PointCloud<pcl::PointXYZI>::Ptr lattice_trajs_cld(new pcl::PointCloud<pcl::PointXYZI>);
Eigen::Vector3d pos_1(0.0, 0.0, 0.0), vel_1(0.0, 0.0, 0.0), acc_1(0.0, 0.0, 0.0);
for (size_t i = 0; i < lattice_nodes.size(); i++) {
pos_1 = lattice_nodes[i].first;
vel_1 = lattice_nodes[i].second;
std::vector<double> endstate_lattice = {pos_1(0), vel_1(0), acc_1(0), pos_1(1), vel_1(1), acc_1(1), pos_1(2), vel_1(2), acc_1(2)};
traj_opt_bridge_for_vis->solveBVP(endstate_lattice);
traj_to_pcl(traj_opt_bridge_for_vis, lattice_trajs_cld);
}
pcl_conversions::toPCL(ros::Time::now(), lattice_trajs_cld->header.stamp);
lattice_trajs_cld->header.frame_id = "world";
lattice_trajs_visual_pub.publish(lattice_trajs_cld);
}
std::pair<float, float> calculate_yaw(float sample_t, float dt) {
constexpr float PI = 3.1415926;
constexpr float YAW_DOT_MAX_PER_SEC = 0.3 * PI;
std::pair<float, float> yaw_yawdot(0, 0);
float yaw = 0;
float yawdot = 0;
// dir of vel
Eigen::Vector3d nxt_p, nxt_v, nxt_a;
traj_opt_bridge->getNextState(nxt_p, nxt_v, nxt_a, sample_t);
Eigen::Vector3d dir_vel = nxt_v / nxt_v.norm();
// dir of goal
Eigen::Vector3d dir_goal(goal_(0) - nxt_p(0), goal_(1) - nxt_p(1), goal_(2) - nxt_p(2));
float goal_dist = dir_goal.norm();
dir_goal = dir_goal / goal_dist;
// or just dir_des = dir_vel
Eigen::Vector3d dir_des = dir_vel + dir_goal;
float yaw_temp = goal_dist > 0.2 ? atan2(dir_des(1), dir_des(0)) : last_yaw_;
float max_yaw_change = YAW_DOT_MAX_PER_SEC * dt;
if (yaw_temp - last_yaw_ > PI) {
if (yaw_temp - last_yaw_ - 2 * PI < -max_yaw_change) {
yaw = last_yaw_ - max_yaw_change;
if (yaw < -PI)
yaw += 2 * PI;
yawdot = -YAW_DOT_MAX_PER_SEC;
} else {
yaw = yaw_temp;
if (yaw - last_yaw_ > PI)
yawdot = -YAW_DOT_MAX_PER_SEC;
else
yawdot = (yaw_temp - last_yaw_) / dt;
}
} else if (yaw_temp - last_yaw_ < -PI) {
if (yaw_temp - last_yaw_ + 2 * PI > max_yaw_change) {
yaw = last_yaw_ + max_yaw_change;
if (yaw > PI)
yaw -= 2 * PI;
yawdot = YAW_DOT_MAX_PER_SEC;
} else {
yaw = yaw_temp;
if (yaw - last_yaw_ < -PI)
yawdot = YAW_DOT_MAX_PER_SEC;
else
yawdot = (yaw_temp - last_yaw_) / dt;
}
} else {
if (yaw_temp - last_yaw_ < -max_yaw_change) {
yaw = last_yaw_ - max_yaw_change;
if (yaw < -PI)
yaw += 2 * PI;
yawdot = -YAW_DOT_MAX_PER_SEC;
} else if (yaw_temp - last_yaw_ > max_yaw_change) {
yaw = last_yaw_ + max_yaw_change;
if (yaw > PI)
yaw -= 2 * PI;
yawdot = YAW_DOT_MAX_PER_SEC;
} else {
yaw = yaw_temp;
if (yaw - last_yaw_ > PI)
yawdot = -YAW_DOT_MAX_PER_SEC;
else if (yaw - last_yaw_ < -PI)
yawdot = YAW_DOT_MAX_PER_SEC;
else
yawdot = (yaw_temp - last_yaw_) / dt;
}
}
last_yaw_ = yaw;
yaw_yawdot.first = yaw;
yaw_yawdot.second = yawdot;
return yaw_yawdot;
}
void ref_pub_cb(const ros::TimerEvent&) {
if (!yopo_init)
return;
if (done) {
// single state control for smoother performance
ctrl_ref_last.header.stamp = ros::Time::now();
ctrl_ref_last.vel_ref = {0, 0, 0};
ctrl_ref_last.acc_ref = {0, 0, 0};
ctrl_ref_last.ref_mask = 1;
ctrl_pub.publish(ctrl_ref_last);
// un-smooth, just for simpler demonstration
pos_cmd_last.header.stamp = ros::Time::now();
pos_cmd_last.velocity.x = 0.95 * pos_cmd_last.velocity.x;
pos_cmd_last.velocity.y = 0.95 * pos_cmd_last.velocity.y;
pos_cmd_last.velocity.z = 0.95 * pos_cmd_last.velocity.z;
pos_cmd_last.acceleration.x = 0.95 * pos_cmd_last.acceleration.x;
pos_cmd_last.acceleration.y = 0.95 * pos_cmd_last.acceleration.y;
pos_cmd_last.acceleration.z = 0.95 * pos_cmd_last.acceleration.z;
pos_cmd_last.yaw_dot = 0.95 * pos_cmd_last.yaw_dot;
so3_ctrl_pub.publish(pos_cmd_last);
return;
}
sample_t += 0.02;
Eigen::Vector3d desired_p, desired_v, desired_a;
traj_opt_bridge->getNextState(desired_p, desired_v, desired_a, sample_t);
std::pair<float, float> yaw_yawdot(0, 0);
yaw_yawdot = calculate_yaw(sample_t, 0.02);
// Realworld & our PID Controller
quad_pos_ctrl::ctrl_ref ctrl_msg;
ctrl_msg.header.stamp = ros::Time::now();
ctrl_msg.pos_ref = {desired_p(0), -desired_p(1), -desired_p(2)};
ctrl_msg.vel_ref = {desired_v(0), -desired_v(1), -desired_v(2)};
ctrl_msg.acc_ref = {desired_a(0), -desired_a(1), -desired_a(2)};
ctrl_msg.yaw_ref = -yaw_yawdot.first;
ctrl_msg.ref_mask = 7;
ctrl_ref_last = ctrl_msg;
ctrl_pub.publish(ctrl_msg);
// SO3 Simulator & SO3 Controller
quadrotor_msgs::PositionCommand cmd;
cmd.header.frame_id = "world";
cmd.header.stamp = ros::Time::now();
cmd.trajectory_flag = quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_READY;
cmd.position.x = desired_p(0);
cmd.position.y = desired_p(1);
cmd.position.z = desired_p(2);
cmd.velocity.x = desired_v(0);
cmd.velocity.y = desired_v(1);
cmd.velocity.z = desired_v(2);
cmd.acceleration.x = desired_a(0);
cmd.acceleration.y = desired_a(1);
cmd.acceleration.z = desired_a(2);
cmd.yaw = yaw_yawdot.first;
cmd.yaw_dot = yaw_yawdot.second;
pos_cmd_last = cmd;
so3_ctrl_pub.publish(cmd);
// update the desire state for next planning
last_pos_ = desired_p;
last_vel_ = desired_v;
last_acc_ = desired_a;
// for reference of yopo network
nav_msgs::Odometry odom_;
odom_.header.stamp = ros::Time::now();
odom_.pose.pose.position.x = desired_p(0);
odom_.pose.pose.position.y = desired_p(1);
odom_.pose.pose.position.z = desired_p(2);
odom_.twist.twist.linear.x = desired_v(0);
odom_.twist.twist.linear.y = desired_v(1);
odom_.twist.twist.linear.z = desired_v(2);
odom_.twist.twist.angular.x = desired_a(0);
odom_.twist.twist.angular.y = desired_a(1);
odom_.twist.twist.angular.z = desired_a(2);
state_ref_pub.publish(odom_);
odom_ref_init = true;
}
} // namespace yopo_net
using namespace yopo_net;
int main(int argc, char** argv) {
ros::init(argc, argv, "yopo_test");
ros::NodeHandle nh;
string cfg_path = getenv("FLIGHTMARE_PATH") + std::string("/flightlib/configs/traj_opt.yaml");
YAML::Node cfg_ = YAML::LoadFile(cfg_path);
traj_time = 2 * cfg_["radio_range"].as<double>() / cfg_["vel_max"].as<double>();
int horizon_num = cfg_["horizon_num"].as<int>();
int vertical_num = cfg_["vertical_num"].as<int>();
int vel_num = cfg_["vel_num"].as<int>();
int radio_num = cfg_["radio_num"].as<int>();
double horizon_fov = cfg_["horizon_camera_fov"].as<double>() * (horizon_num - 1) / horizon_num;
double vertical_fov = cfg_["vertical_camera_fov"].as<double>() * (vertical_num - 1) / vertical_num;
double vel_fov = cfg_["vel_fov"].as<double>();
double radio_range = cfg_["radio_range"].as<double>();
double vel_prefile = cfg_["vel_prefile"].as<double>();
getLatticeGuiding(lattice_nodes, horizon_num, vertical_num, radio_num, vel_num, horizon_fov, vertical_fov, radio_range, vel_fov, vel_prefile);
traj_opt_bridge = new TrajOptimizationBridge();
traj_opt_bridge_for_vis = new TrajOptimizationBridge();
lattice_trajs_visual_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZI>>("/yopo_net/lattice_trajs_visual", 1);
trajs_visual_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZI>>("/yopo_net/trajs_visual", 1);
best_traj_visual_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZI>>("/yopo_net/best_traj_visual", 1);
state_ref_pub = nh.advertise<nav_msgs::Odometry>("/juliett/state_ref/odom", 10);
// our PID Controller (realworld) & SO3 Controller (simulation)
ctrl_pub = nh.advertise<quad_pos_ctrl::ctrl_ref>("/juliett_pos_ctrl_node/controller/ctrl_ref", 10);
so3_ctrl_pub = nh.advertise<quadrotor_msgs::PositionCommand>("/so3_control/pos_cmd", 10);
odom_sub = nh.subscribe("/juliett/ground_truth/odom", 1, yopo_net::odom_cb, ros::TransportHints().tcpNoDelay());
yopo_best_sub = nh.subscribe("/yopo_net/pred_endstate", 1, yopo_net::yopo_cb, ros::TransportHints().tcpNoDelay());
yopo_all_sub = nh.subscribe("/yopo_net/pred_endstates", 1, trajs_vis_cb);
goal_sub = nh.subscribe("/yopo_net/goal", 1, goal_cb);
ros::Timer ref_timer = nh.createTimer(ros::Duration(0.02), ref_pub_cb);
std::cout << "YOPO Planner Node OK!" << std::endl;
ros::spin();
}

10
flightlib/src/sensors/imu.cpp Normal file
View File

@@ -0,0 +1,10 @@
#include "flightlib/sensors/imu.hpp"
namespace flightlib {
IMU::IMU() {}
IMU::~IMU() {}
} // namespace flightlib

200
flightlib/src/sensors/rgb_camera.cpp Normal file
View File

@@ -0,0 +1,200 @@
#include "flightlib/sensors/rgb_camera.hpp"
namespace flightlib {
RGBCamera::RGBCamera() : channels_(3), width_(720), height_(480), fov_{70.0}, depth_scale_{0.2}, enabled_layers_({true, false, false, false}) {
post_processing_ = {{RGBCameraTypes::Depth, false},
{RGBCameraTypes::OpticalFlow, false},
{RGBCameraTypes::ObjectSegment, false},
{RGBCameraTypes::CategorySegment, false}};
}
RGBCamera::~RGBCamera() {}
bool RGBCamera::feedImageQueue(const int image_layer, const cv::Mat& image_mat) {
queue_mutex_.lock();
// rgb_queue_.resize(queue_size_) queue_size_由1改为0
switch (image_layer) {
case 0: // rgb image
if (rgb_queue_.size() > queue_size_)
rgb_queue_.resize(queue_size_);
rgb_queue_.push_back(image_mat);
break;
case CameraLayer::DepthMap:
if (depth_queue_.size() > queue_size_)
depth_queue_.resize(queue_size_);
depth_queue_.push_back(image_mat);
break;
case CameraLayer::Segmentation:
if (segmentation_queue_.size() > queue_size_)
segmentation_queue_.resize(queue_size_);
segmentation_queue_.push_back(image_mat);
break;
case CameraLayer::OpticalFlow:
if (opticalflow_queue_.size() > queue_size_)
opticalflow_queue_.resize(queue_size_);
opticalflow_queue_.push_back(image_mat);
break;
}
queue_mutex_.unlock();
return true;
}
bool RGBCamera::setRelPose(const Ref<Vector<3>> B_r_BC, const Ref<Matrix<3, 3>> R_BC) {
if (!B_r_BC.allFinite() || !R_BC.allFinite()) {
logger_.error(
"The setting value for Camera Relative Pose Matrix is not valid, discard "
"the setting.");
return false;
}
B_r_BC_ = B_r_BC;
T_BC_.block<3, 3>(0, 0) = R_BC;
T_BC_.block<3, 1>(0, 3) = B_r_BC;
T_BC_.row(3) << 0.0, 0.0, 0.0, 1.0;
return true;
}
bool RGBCamera::setWidth(const int width) {
if (width <= 0.0) {
logger_.warn("The setting value for Image Width is not valid, discard the setting.");
return false;
}
width_ = width;
return true;
}
bool RGBCamera::setHeight(const int height) {
if (height <= 0.0) {
logger_.warn(
"The setting value for Image Height is not valid, discard the "
"setting.");
return false;
}
height_ = height;
return true;
}
bool RGBCamera::setFOV(const Scalar fov) {
if (fov <= 0.0) {
logger_.warn(
"The setting value for Camera Field-of-View is not valid, discard the "
"setting.");
return false;
}
fov_ = fov;
return true;
}
bool RGBCamera::setDepthScale(const Scalar depth_scale) {
if (depth_scale_ < 0.0 || depth_scale_ > 1.0) {
logger_.warn(
"The setting value for Camera Depth Scale is not valid, discard the "
"setting.");
return false;
}
depth_scale_ = depth_scale;
return true;
}
bool RGBCamera::setPostProcesscing(const std::vector<bool>& enabled_layers) {
if (enabled_layers_.size() != enabled_layers.size()) {
logger_.warn("Vector size does not match. The vector size should be equal to %d.", enabled_layers_.size());
return false;
}
enabled_layers_ = enabled_layers;
return true;
}
std::vector<bool> RGBCamera::getEnabledLayers(void) const { return enabled_layers_; }
std::vector<std::string> RGBCamera::GetPostProcessing(void) {
std::vector<std::string> post_processing;
for (const auto& pp : post_processing_) {
if (pp.second) {
post_processing.push_back(pp.first);
}
}
return post_processing;
}
Matrix<4, 4> RGBCamera::getRelPose(void) const { return T_BC_; }
int RGBCamera::getChannels(void) const { return channels_; }
int RGBCamera::getWidth(void) const { return width_; }
int RGBCamera::getHeight(void) const { return height_; }
Scalar RGBCamera::getFOV(void) const { return fov_; }
Scalar RGBCamera::getDepthScale(void) const { return depth_scale_; }
void RGBCamera::enableDepth(const bool on) {
// if (enabled_layers_[CameraLayer::DepthMap] == on) {
// logger_.warn("Depth layer was already %s.", on ? "on" : "off");
// }
enabled_layers_[CameraLayer::DepthMap] = on;
post_processing_[RGBCameraTypes::Depth] = on;
}
void RGBCamera::enableSegmentation(const bool on) {
// if (enabled_layers_[CameraLayer::Segmentation] == on) {
// logger_.warn("Segmentation layer was already %s.", on ? "on" : "off");
// }
enabled_layers_[CameraLayer::Segmentation] = on;
post_processing_[RGBCameraTypes::ObjectSegment] = on;
post_processing_[RGBCameraTypes::CategorySegment] = on;
}
void RGBCamera::enableOpticalFlow(const bool on) {
// if (enabled_layers_[CameraLayer::OpticalFlow] == on) {
// logger_.warn("Optical Flow layer was already %s.", on ? "on" : "off");
// }
enabled_layers_[CameraLayer::OpticalFlow] = on;
post_processing_[RGBCameraTypes::OpticalFlow] = on;
}
bool RGBCamera::getRGBImage(cv::Mat& rgb_img) {
if (!rgb_queue_.empty()) {
rgb_img = rgb_queue_.front();
rgb_queue_.pop_front();
return true;
}
return false;
}
bool RGBCamera::getDepthMap(cv::Mat& depth_map) {
if (!depth_queue_.empty()) {
depth_map = depth_queue_.front();
depth_queue_.pop_front();
return true;
}
return false;
}
bool RGBCamera::getSegmentation(cv::Mat& segmentation) {
if (!segmentation_queue_.empty()) {
segmentation = segmentation_queue_.front();
segmentation_queue_.pop_front();
return true;
}
return false;
}
bool RGBCamera::getOpticalFlow(cv::Mat& opticalflow) {
if (!opticalflow_queue_.empty()) {
opticalflow = opticalflow_queue_.front();
opticalflow_queue_.pop_front();
return true;
}
return false;
}
void RGBCamera::clearImageQueue() {
rgb_queue_.clear();
depth_queue_.clear();
segmentation_queue_.clear();
opticalflow_queue_.clear();
}
} // namespace flightlib

9
flightlib/src/sensors/sensor_base.cpp Normal file
View File

@@ -0,0 +1,9 @@
#include "flightlib/sensors/sensor_base.hpp"
namespace flightlib {
SensorBase::SensorBase() {}
SensorBase::~SensorBase() {}
} // namespace flightlib

115
flightlib/src/sensors/sgm_gpu/costs.cu Normal file
View File

@@ -0,0 +1,115 @@
/***********************************************************************
Copyright (C) 2019 Hironori Fujimoto
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
***********************************************************************/
#include "flightlib/sensors/sgm_gpu/costs.h"
#include <stdio.h>
namespace sgm_gpu
{
__global__ void
__launch_bounds__(1024, 2)
CenterSymmetricCensusKernelSM2(const uint8_t *im, const uint8_t *im2, cost_t *transform, cost_t *transform2, const uint32_t rows, const uint32_t cols) {
const int idx = blockIdx.x*blockDim.x+threadIdx.x;
const int idy = blockIdx.y*blockDim.y+threadIdx.y;
const int win_cols = (32+LEFT*2); // 32+4*2 = 40
const int win_rows = (32+TOP*2); // 32+3*2 = 38
__shared__ uint8_t window[win_cols*win_rows];
__shared__ uint8_t window2[win_cols*win_rows];
const int id = threadIdx.y*blockDim.x+threadIdx.x;
const int sm_row = id / win_cols;
const int sm_col = id % win_cols;
const int im_row = blockIdx.y*blockDim.y+sm_row-TOP;
const int im_col = blockIdx.x*blockDim.x+sm_col-LEFT;
const bool boundaries = (im_row >= 0 && im_col >= 0 && im_row < rows && im_col < cols);
window[sm_row*win_cols+sm_col] = boundaries ? im[im_row*cols+im_col] : 0;
window2[sm_row*win_cols+sm_col] = boundaries ? im2[im_row*cols+im_col] : 0;
// Not enough threads to fill window and window2
const int block_size = blockDim.x*blockDim.y;
if(id < (win_cols*win_rows-block_size)) {
const int id = threadIdx.y*blockDim.x+threadIdx.x+block_size;
const int sm_row = id / win_cols;
const int sm_col = id % win_cols;
const int im_row = blockIdx.y*blockDim.y+sm_row-TOP;
const int im_col = blockIdx.x*blockDim.x+sm_col-LEFT;
const bool boundaries = (im_row >= 0 && im_col >= 0 && im_row < rows && im_col < cols);
window[sm_row*win_cols+sm_col] = boundaries ? im[im_row*cols+im_col] : 0;
window2[sm_row*win_cols+sm_col] = boundaries ? im2[im_row*cols+im_col] : 0;
}
__syncthreads();
cost_t census = 0;
cost_t census2 = 0;
if(idy < rows && idx < cols) {
for(int k = 0; k < CENSUS_HEIGHT/2; k++) {
for(int m = 0; m < CENSUS_WIDTH; m++) {
const uint8_t e1 = window[(threadIdx.y+k)*win_cols+threadIdx.x+m];
const uint8_t e2 = window[(threadIdx.y+2*TOP-k)*win_cols+threadIdx.x+2*LEFT-m];
const uint8_t i1 = window2[(threadIdx.y+k)*win_cols+threadIdx.x+m];
const uint8_t i2 = window2[(threadIdx.y+2*TOP-k)*win_cols+threadIdx.x+2*LEFT-m];
const int shft = k*CENSUS_WIDTH+m;
// Compare to the center
cost_t tmp = (e1 >= e2);
// Shift to the desired position
tmp <<= shft;
// Add it to its place
census |= tmp;
// Compare to the center
cost_t tmp2 = (i1 >= i2);
// Shift to the desired position
tmp2 <<= shft;
// Add it to its place
census2 |= tmp2;
}
}
if(CENSUS_HEIGHT % 2 != 0) {
const int k = CENSUS_HEIGHT/2;
for(int m = 0; m < CENSUS_WIDTH/2; m++) {
const uint8_t e1 = window[(threadIdx.y+k)*win_cols+threadIdx.x+m];
const uint8_t e2 = window[(threadIdx.y+2*TOP-k)*win_cols+threadIdx.x+2*LEFT-m];
const uint8_t i1 = window2[(threadIdx.y+k)*win_cols+threadIdx.x+m];
const uint8_t i2 = window2[(threadIdx.y+2*TOP-k)*win_cols+threadIdx.x+2*LEFT-m];
const int shft = k*CENSUS_WIDTH+m;
// Compare to the center
cost_t tmp = (e1 >= e2);
// Shift to the desired position
tmp <<= shft;
// Add it to its place
census |= tmp;
// Compare to the center
cost_t tmp2 = (i1 >= i2);
// Shift to the desired position
tmp2 <<= shft;
// Add it to its place
census2 |= tmp2;
}
}
transform[idy*cols+idx] = census;
transform2[idy*cols+idx] = census2;
}
}
} // namespace sgm_gpu

71
flightlib/src/sensors/sgm_gpu/hamming_cost.cu Normal file
View File

@@ -0,0 +1,71 @@
/***********************************************************************
Copyright (C) 2019 Hironori Fujimoto
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
***********************************************************************/
#include "flightlib/sensors/sgm_gpu/hamming_cost.h"
namespace sgm_gpu
{
//d_transform0, d_transform1, d_cost, rows, cols
__global__ void
HammingDistanceCostKernel ( const cost_t *d_transform0, const cost_t *d_transform1,
uint8_t *d_cost, const int rows, const int cols ) {
//const int Dmax= blockDim.x; // Dmax is CTA size
const int y= blockIdx.x; // y is CTA Identifier
const int THRid = threadIdx.x; // THRid is Thread Identifier
__shared__ cost_t SharedMatch[2*MAX_DISPARITY];
__shared__ cost_t SharedBase [MAX_DISPARITY];
SharedMatch [MAX_DISPARITY+THRid] = d_transform1[y*cols+0]; // init position
int n_iter = cols/MAX_DISPARITY;
for (int ix=0; ix<n_iter; ix++) {
const int x = ix*MAX_DISPARITY;
SharedMatch [THRid] = SharedMatch [THRid + MAX_DISPARITY];
SharedMatch [THRid+MAX_DISPARITY] = d_transform1 [y*cols+x+THRid];
SharedBase [THRid] = d_transform0 [y*cols+x+THRid];
__syncthreads();
for (int i=0; i<MAX_DISPARITY; i++) {
const cost_t base = SharedBase [i];
const cost_t match = SharedMatch[(MAX_DISPARITY-1-THRid)+1+i];
d_cost[(y*cols+x+i)*MAX_DISPARITY+THRid] = popcount( base ^ match );
}
__syncthreads();
}
// For images with cols not multiples of MAX_DISPARITY
const int x = MAX_DISPARITY*(cols/MAX_DISPARITY);
const int left = cols-x;
if(left > 0) {
SharedMatch [THRid] = SharedMatch [THRid + MAX_DISPARITY];
if(THRid < left) {
SharedMatch [THRid+MAX_DISPARITY] = d_transform1 [y*cols+x+THRid];
SharedBase [THRid] = d_transform0 [y*cols+x+THRid];
}
__syncthreads();
for (int i=0; i<left; i++) {
const cost_t base = SharedBase [i];
const cost_t match = SharedMatch[(MAX_DISPARITY-1-THRid)+1+i];
d_cost[(y*cols+x+i)*MAX_DISPARITY+THRid] = popcount( base ^ match );
}
__syncthreads();
}
}
} // namespace sgm_gpu

69
flightlib/src/sensors/sgm_gpu/left_right_consistency.cu Normal file
View File

@@ -0,0 +1,69 @@
/***********************************************************************
Copyright (C) 2019 Hironori Fujimoto
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
***********************************************************************/
#include "flightlib/sensors/sgm_gpu/left_right_consistency.h"
#include "flightlib/sensors/sgm_gpu/configuration.h"
namespace sgm_gpu
{
__global__ void ChooseRightDisparity(uint8_t *right_disparity, const uint16_t *smoothed_cost, const uint32_t rows, const uint32_t cols) {
const int x = blockIdx.x*blockDim.x+threadIdx.x;
const int y = blockIdx.y*blockDim.y+threadIdx.y;
if (x >= cols || y >= rows)
return;
int min_cost_disparity = 0;
uint16_t min_cost = smoothed_cost[(y*cols + x)*MAX_DISPARITY + min_cost_disparity];
for (int d = 1; d < MAX_DISPARITY; d++) {
if (x + d >= cols)
break;
uint16_t tmp_cost = smoothed_cost[(y*cols + (x+d))*MAX_DISPARITY + d];
if (tmp_cost < min_cost) {
min_cost = tmp_cost;
min_cost_disparity = d;
}
}
right_disparity[y*cols+x] = min_cost_disparity;
}
__global__ void LeftRightConsistencyCheck(uint8_t* disparity, const uint8_t* disparity_right, uint32_t rows, uint32_t cols)
{
const int x = blockIdx.x*blockDim.x+threadIdx.x;
const int y = blockIdx.y*blockDim.y+threadIdx.y;
if (x >= cols || y >= rows)
return;
const int x_right = x - disparity[y*cols + x];
if (x_right < 0) {
disparity[y*cols + x] = 255;
return;
}
int diff = disparity[y*cols + x] - disparity_right[y*cols + x_right];
diff = diff < 0 ? diff * -1 : diff;
if (diff > 1) {
disparity[y*cols + x] = 255;
}
}
} // namespace sgm_gpu

26
flightlib/src/sensors/sgm_gpu/median_filter.cu Normal file
View File

@@ -0,0 +1,26 @@
/***********************************************************************
Copyright (C) 2019 Hironori Fujimoto
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
***********************************************************************/
#include "flightlib/sensors/sgm_gpu/median_filter.h"
namespace sgm_gpu
{
__global__ void MedianFilter3x3(const uint8_t* __restrict__ d_input, uint8_t* __restrict__ d_out, const uint32_t rows, const uint32_t cols) {
MedianFilter<3>(d_input, d_out, rows, cols);
}
}

270
flightlib/src/sensors/sgm_gpu/sgm_gpu.cu Normal file
View File

@@ -0,0 +1,270 @@
/***********************************************************************
Copyright (C) 2020 Hironori Fujimoto
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
***********************************************************************/
#include "flightlib/sensors/sgm_gpu/sgm_gpu.h"
#include "flightlib/sensors/sgm_gpu/cost_aggregation.h"
#include "flightlib/sensors/sgm_gpu/costs.h"
#include "flightlib/sensors/sgm_gpu/hamming_cost.h"
#include "flightlib/sensors/sgm_gpu/left_right_consistency.h"
#include "flightlib/sensors/sgm_gpu/median_filter.h"
namespace sgm_gpu {
// Variables which have CUDA-related type are put here
// in order to include sgm_gpu.h from non-CUDA package
cudaStream_t stream1_;
cudaStream_t stream2_;
cudaStream_t stream3_;
dim3 BLOCK_SIZE_;
dim3 grid_size_;
SgmGpu::SgmGpu(const int cols, const int rows)
: memory_allocated_(false), cols_(cols), rows_(rows) {
// Get parameters used in SGM algorithm
p1_ = 6; // static_cast<uint8_t>(private_node_handle_->param("p1", 6));
p2_ = 96; // static_cast<uint8_t>(private_node_handle_->param("p2", 96));
check_consistency_ = true; // private_node_handle_->param("check_consistency", true);
// Create streams
cudaStreamCreate(&stream1_);
cudaStreamCreate(&stream2_);
cudaStreamCreate(&stream3_);
}
SgmGpu::~SgmGpu() {
freeMemory();
cudaStreamDestroy(stream1_);
cudaStreamDestroy(stream2_);
cudaStreamDestroy(stream3_);
}
void SgmGpu::allocateMemory(uint32_t cols, uint32_t rows) {
freeMemory();
cols_ = cols;
rows_ = rows;
int total_pixel = cols_ * rows_;
cudaMalloc((void **)&d_im0_, sizeof(uint8_t) * total_pixel);
cudaMalloc((void **)&d_im1_, sizeof(uint8_t) * total_pixel);
cudaMalloc((void **)&d_transform0_, sizeof(cost_t) * total_pixel);
cudaMalloc((void **)&d_transform1_, sizeof(cost_t) * total_pixel);
int cost_volume_size = total_pixel * MAX_DISPARITY;
cudaMalloc((void **)&d_cost_, sizeof(uint8_t) * cost_volume_size);
cudaMalloc((void **)&d_L0_, sizeof(uint8_t) * cost_volume_size);
cudaMalloc((void **)&d_L1_, sizeof(uint8_t) * cost_volume_size);
cudaMalloc((void **)&d_L2_, sizeof(uint8_t) * cost_volume_size);
cudaMalloc((void **)&d_L3_, sizeof(uint8_t) * cost_volume_size);
cudaMalloc((void **)&d_L4_, sizeof(uint8_t) * cost_volume_size);
cudaMalloc((void **)&d_L5_, sizeof(uint8_t) * cost_volume_size);
cudaMalloc((void **)&d_L6_, sizeof(uint8_t) * cost_volume_size);
cudaMalloc((void **)&d_L7_, sizeof(uint8_t) * cost_volume_size);
cudaMalloc((void **)&d_s_, sizeof(uint16_t) * cost_volume_size);
cudaMalloc((void **)&d_disparity_, sizeof(uint8_t) * total_pixel);
cudaMalloc((void **)&d_disparity_filtered_uchar_,
sizeof(uint8_t) * total_pixel);
cudaMalloc((void **)&d_disparity_right_, sizeof(uint8_t) * total_pixel);
cudaMalloc((void **)&d_disparity_right_filtered_uchar_,
sizeof(uint8_t) * total_pixel);
memory_allocated_ = true;
}
void SgmGpu::freeMemory() {
if (!memory_allocated_) return;
cudaFree(d_im0_);
cudaFree(d_im1_);
cudaFree(d_transform0_);
cudaFree(d_transform1_);
cudaFree(d_L0_);
cudaFree(d_L1_);
cudaFree(d_L2_);
cudaFree(d_L3_);
cudaFree(d_L4_);
cudaFree(d_L5_);
cudaFree(d_L6_);
cudaFree(d_L7_);
cudaFree(d_disparity_);
cudaFree(d_disparity_filtered_uchar_);
cudaFree(d_disparity_right_);
cudaFree(d_disparity_right_filtered_uchar_);
cudaFree(d_cost_);
cudaFree(d_s_);
memory_allocated_ = false;
}
bool SgmGpu::computeDisparity(const cv::Mat &left_image,
const cv::Mat &right_image,
cv::Mat &disparity_out) {
// Convert images to grayscale
cv::Mat left_mono8, right_mono8;
if (left_image.channels() > 1) {
cv::cvtColor(left_image, left_mono8, CV_RGB2GRAY);
}
if (right_image.channels() > 1) {
cv::cvtColor(right_image, right_mono8, CV_RGB2GRAY);
}
// Resize images to their width and height divisible by 4 for limit of CUDA
// code
resizeToDivisibleBy4(left_mono8, right_mono8);
// Reallocate memory if needed
bool size_changed = (cols_ != left_mono8.cols || rows_ != left_mono8.rows);
if (!memory_allocated_ || size_changed)
allocateMemory(left_mono8.cols, left_mono8.rows);
// Copy image to GPU device
size_t mono8_image_size = left_mono8.total() * sizeof(uint8_t);
cudaMemcpyAsync(d_im0_, left_mono8.ptr<uint8_t>(), mono8_image_size,
cudaMemcpyHostToDevice, stream1_);
cudaMemcpyAsync(d_im1_, right_mono8.ptr<uint8_t>(), mono8_image_size,
cudaMemcpyHostToDevice, stream1_);
BLOCK_SIZE_.x = 32;
BLOCK_SIZE_.y = 32;
grid_size_.x = (cols_ + BLOCK_SIZE_.x - 1) / BLOCK_SIZE_.x;
grid_size_.y = (rows_ + BLOCK_SIZE_.y - 1) / BLOCK_SIZE_.y;
CenterSymmetricCensusKernelSM2<<<grid_size_, BLOCK_SIZE_, 0, stream1_>>>(
d_im0_, d_im1_, d_transform0_, d_transform1_, rows_, cols_);
cudaStreamSynchronize(stream1_);
HammingDistanceCostKernel<<<rows_, MAX_DISPARITY, 0, stream1_>>>(
d_transform0_, d_transform1_, d_cost_, rows_, cols_);
const int PIXELS_PER_BLOCK = COSTAGG_BLOCKSIZE / WARP_SIZE;
const int PIXELS_PER_BLOCK_HORIZ = COSTAGG_BLOCKSIZE_HORIZ / WARP_SIZE;
// Cost Aggregation
CostAggregationKernelLeftToRight<<<(rows_ + PIXELS_PER_BLOCK_HORIZ - 1) /
PIXELS_PER_BLOCK_HORIZ,
COSTAGG_BLOCKSIZE_HORIZ, 0, stream2_>>>(
d_cost_, d_L0_, d_s_, p1_, p2_, rows_, cols_, d_transform0_,
d_transform1_, d_disparity_, d_L0_, d_L1_, d_L2_, d_L3_, d_L4_, d_L5_,
d_L6_);
CostAggregationKernelRightToLeft<<<(rows_ + PIXELS_PER_BLOCK_HORIZ - 1) /
PIXELS_PER_BLOCK_HORIZ,
COSTAGG_BLOCKSIZE_HORIZ, 0, stream3_>>>(
d_cost_, d_L1_, d_s_, p1_, p2_, rows_, cols_, d_transform0_,
d_transform1_, d_disparity_, d_L0_, d_L1_, d_L2_, d_L3_, d_L4_, d_L5_,
d_L6_);
CostAggregationKernelUpToDown<<<(cols_ + PIXELS_PER_BLOCK - 1) /
PIXELS_PER_BLOCK,
COSTAGG_BLOCKSIZE, 0, stream1_>>>(
d_cost_, d_L2_, d_s_, p1_, p2_, rows_, cols_, d_transform0_,
d_transform1_, d_disparity_, d_L0_, d_L1_, d_L2_, d_L3_, d_L4_, d_L5_,
d_L6_);
CostAggregationKernelDownToUp<<<(cols_ + PIXELS_PER_BLOCK - 1) /
PIXELS_PER_BLOCK,
COSTAGG_BLOCKSIZE, 0, stream1_>>>(
d_cost_, d_L3_, d_s_, p1_, p2_, rows_, cols_, d_transform0_,
d_transform1_, d_disparity_, d_L0_, d_L1_, d_L2_, d_L3_, d_L4_, d_L5_,
d_L6_);
CostAggregationKernelDiagonalDownUpLeftRight<<<
(cols_ + PIXELS_PER_BLOCK - 1) / PIXELS_PER_BLOCK, COSTAGG_BLOCKSIZE, 0,
stream1_>>>(d_cost_, d_L4_, d_s_, p1_, p2_, rows_, cols_, d_transform0_,
d_transform1_, d_disparity_, d_L0_, d_L1_, d_L2_, d_L3_,
d_L4_, d_L5_, d_L6_);
CostAggregationKernelDiagonalUpDownLeftRight<<<
(cols_ + PIXELS_PER_BLOCK - 1) / PIXELS_PER_BLOCK, COSTAGG_BLOCKSIZE, 0,
stream1_>>>(d_cost_, d_L5_, d_s_, p1_, p2_, rows_, cols_, d_transform0_,
d_transform1_, d_disparity_, d_L0_, d_L1_, d_L2_, d_L3_,
d_L4_, d_L5_, d_L6_);
CostAggregationKernelDiagonalDownUpRightLeft<<<
(cols_ + PIXELS_PER_BLOCK - 1) / PIXELS_PER_BLOCK, COSTAGG_BLOCKSIZE, 0,
stream1_>>>(d_cost_, d_L6_, d_s_, p1_, p2_, rows_, cols_, d_transform0_,
d_transform1_, d_disparity_, d_L0_, d_L1_, d_L2_, d_L3_,
d_L4_, d_L5_, d_L6_);
CostAggregationKernelDiagonalUpDownRightLeft<<<
(cols_ + PIXELS_PER_BLOCK - 1) / PIXELS_PER_BLOCK, COSTAGG_BLOCKSIZE, 0,
stream1_>>>(d_cost_, d_L7_, d_s_, p1_, p2_, rows_, cols_, d_transform0_,
d_transform1_, d_disparity_, d_L0_, d_L1_, d_L2_, d_L3_,
d_L4_, d_L5_, d_L6_);
int total_pixel = rows_ * cols_;
MedianFilter3x3<<<(total_pixel + MAX_DISPARITY - 1) / MAX_DISPARITY,
MAX_DISPARITY, 0, stream1_>>>(
d_disparity_, d_disparity_filtered_uchar_, rows_, cols_);
if (check_consistency_) {
ChooseRightDisparity<<<grid_size_, BLOCK_SIZE_, 0, stream1_>>>(
d_disparity_right_, d_s_, rows_, cols_);
MedianFilter3x3<<<(total_pixel + MAX_DISPARITY - 1) / MAX_DISPARITY,
MAX_DISPARITY, 0, stream1_>>>(
d_disparity_right_, d_disparity_right_filtered_uchar_, rows_, cols_);
LeftRightConsistencyCheck<<<grid_size_, BLOCK_SIZE_, 0, stream1_>>>(
d_disparity_filtered_uchar_, d_disparity_right_filtered_uchar_, rows_,
cols_);
}
cudaError_t err = cudaGetLastError();
if (err != cudaSuccess) {
printf("libsgm_gpu ERROR: %s %d\n", cudaGetErrorString(err), err);
return false;
}
cudaDeviceSynchronize();
cv::Mat disparity(rows_, cols_, CV_8UC1);
cudaMemcpy(disparity.data, d_disparity_filtered_uchar_,
sizeof(uint8_t) * total_pixel, cudaMemcpyDeviceToHost);
// Restore image size if resized to be divisible by 4
if (cols_ != left_image.cols || rows_ != left_image.rows) {
cv::Size input_size(left_image.cols, left_image.rows);
cv::resize(disparity, disparity, input_size, 0, 0, cv::INTER_AREA);
}
disparity_out = disparity;
// convertToMsg(disparity, left_camera_info, right_camera_info,
// disparity_msg);
return true;
}
void SgmGpu::resizeToDivisibleBy4(cv::Mat &left_image, cv::Mat &right_image) {
bool need_resize = false;
cv::Size original_size, resized_size;
original_size = cv::Size(left_image.cols, left_image.rows);
resized_size = original_size;
if (original_size.width % 4 != 0) {
need_resize = true;
resized_size.width = (original_size.width / 4 + 1) * 4;
}
if (original_size.height % 4 != 0) {
need_resize = true;
resized_size.height = (original_size.height / 4 + 1) * 4;
}
if (need_resize) {
cv::resize(left_image, left_image, resized_size, 0, 0, cv::INTER_LINEAR);
cv::resize(right_image, right_image, resized_size, 0, 0, cv::INTER_LINEAR);
}
}
} // namespace sgm_gpu

49
flightlib/src/wrapper/pybind_wrapper.cpp Normal file
View File

@@ -0,0 +1,49 @@
// pybind11
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
// flightlib
#include "flightlib/envs/env_base.hpp"
#include "flightlib/envs/quadrotor_env.hpp"
#include "flightlib/envs/vec_env.hpp"
namespace py = pybind11;
using namespace flightlib;
// vec_env -> quadrotor_env
PYBIND11_MODULE(flightgym, m) {
py::class_<VecEnv<QuadrotorEnv>>(m, "QuadrotorEnv_v1")
.def(py::init<>())
.def(py::init<const std::string&>())
.def(py::init<const std::string&, const bool>())
// unity
.def("close", &VecEnv<QuadrotorEnv>::close)
.def("connectUnity", &VecEnv<QuadrotorEnv>::connectUnity)
.def("disconnectUnity", &VecEnv<QuadrotorEnv>::disconnectUnity)
.def("render", &VecEnv<QuadrotorEnv>::render)
.def("spawnTrees", &VecEnv<QuadrotorEnv>::spawnTrees)
.def("savePointcloud", &VecEnv<QuadrotorEnv>::savePointcloud)
.def("spawnTreesAndSavePointcloud", &VecEnv<QuadrotorEnv>::spawnTreesAndSavePointcloud)
// set
.def("step", &VecEnv<QuadrotorEnv>::step)
.def("reset", &VecEnv<QuadrotorEnv>::reset)
.def("setState", &VecEnv<QuadrotorEnv>::setState)
.def("setGoal", &VecEnv<QuadrotorEnv>::setGoal)
.def("setSeed", &VecEnv<QuadrotorEnv>::setSeed)
.def("setMapID", &VecEnv<QuadrotorEnv>::setMapID)
// get
.def("getNumOfEnvs", &VecEnv<QuadrotorEnv>::getNumOfEnvs)
.def("getWorldBox", &VecEnv<QuadrotorEnv>::getWorldBox)
.def("getObsDim", &VecEnv<QuadrotorEnv>::getObsDim)
.def("getActDim", &VecEnv<QuadrotorEnv>::getActDim)
.def("getRewDim", &VecEnv<QuadrotorEnv>::getRewDim)
.def("getRGBImage", &VecEnv<QuadrotorEnv>::getRGBImage)
.def("getStereoImage", &VecEnv<QuadrotorEnv>::getStereoImage)
.def("getDepthImage", &VecEnv<QuadrotorEnv>::getDepthImage)
.def("getImgHeight", &VecEnv<QuadrotorEnv>::getImgHeight)
.def("getImgWidth", &VecEnv<QuadrotorEnv>::getImgWidth)
.def("getRewardNames", &VecEnv<QuadrotorEnv>::getRewardNames)
.def("getCostAndGradient", &VecEnv<QuadrotorEnv>::getCostAndGradient)
.def("__repr__", [](const VecEnv<QuadrotorEnv>& a) { return "Flightmare Environment"; });
}

9
flightlib/third_party/arc_utilities/.gitignore vendored Normal file
View File

@@ -0,0 +1,9 @@
# editor temporary files
*.swp
*~
# Vim Ctags file
tags
# Compiled python files
*.pyc

143
flightlib/third_party/arc_utilities/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,143 @@
cmake_minimum_required(VERSION 2.8.3)
project(arc_utilities)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs geometry_msgs)
find_package(cmake_modules REQUIRED)
find_package(Eigen3 REQUIRED)
set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS filesystem)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/groovy/api/catkin/html/user_guide/setup_dot_py.html
catkin_python_setup()
#######################################
## Declare ROS messages and services ##
#######################################
## Generate messages in the 'msg' folder
# add_message_files(DIRECTORY msg FILES )
## Generate services in the 'srv' folder
# add_service_files(DIRECTORY srv FILES )
## Generate added messages and services with any dependencies listed here
# generate_messages(DEPENDENCIES geometry_msgs std_msgs)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS roscpp rospy std_msgs sensor_msgs geometry_msgs DEPENDS Eigen3)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(include SYSTEM ${catkin_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS})
set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS} -O3 -Wall -Wextra -Wconversion -Werror")
# Utility library
add_library(${PROJECT_NAME}
include/${PROJECT_NAME}/log.hpp
include/${PROJECT_NAME}/maybe.hpp
include/${PROJECT_NAME}/ros_helpers.hpp
include/${PROJECT_NAME}/abb_irb1600_145_fk_fast.hpp
include/${PROJECT_NAME}/iiwa_7_fk_fast.hpp
include/${PROJECT_NAME}/iiwa_14_fk_fast.hpp
include/${PROJECT_NAME}/arc_exceptions.hpp
include/${PROJECT_NAME}/arc_helpers.hpp
include/${PROJECT_NAME}/pretty_print.hpp
include/${PROJECT_NAME}/zlib_helpers.hpp
include/${PROJECT_NAME}/base64_helpers.hpp
include/${PROJECT_NAME}/voxel_grid.hpp
include/${PROJECT_NAME}/dynamic_spatial_hashed_voxel_grid.hpp
include/${PROJECT_NAME}/aligned_eigen_types.hpp
include/${PROJECT_NAME}/eigen_helpers.hpp
include/${PROJECT_NAME}/eigen_helpers_conversions.hpp
include/${PROJECT_NAME}/simple_rrt_planner.hpp
include/${PROJECT_NAME}/simple_astar_planner.hpp
include/${PROJECT_NAME}/simple_prm_planner.hpp
include/${PROJECT_NAME}/simple_kmeans_clustering.hpp
include/${PROJECT_NAME}/simple_hierarchical_clustering.hpp
include/${PROJECT_NAME}/simple_hausdorff_distance.hpp
include/${PROJECT_NAME}/simple_dtw.hpp
include/${PROJECT_NAME}/timing.hpp
include/${PROJECT_NAME}/dijkstras.hpp
include/${PROJECT_NAME}/shortcut_smoothing.hpp
include/${PROJECT_NAME}/first_order_deformation.h
include/${PROJECT_NAME}/get_neighbours.hpp
include/${PROJECT_NAME}/serialization.hpp
include/${PROJECT_NAME}/serialization_eigen.hpp
include/${PROJECT_NAME}/serialization_ros.hpp
include/${PROJECT_NAME}/filesystem.hpp
src/${PROJECT_NAME}/zlib_helpers.cpp
src/${PROJECT_NAME}/base64_helpers.cpp
src/timing.cpp
src/${PROJECT_NAME}/first_order_deformation.cpp)
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # ${PROJECT_NAME}_gencpp)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} z)
# Simple test node for simple_hierarchical_clustering
add_executable(test_hierarchical_clustering src/test_hierarchical_clustering.cpp)
add_dependencies(test_hierarchical_clustering ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # ${PROJECT_NAME}_gencpp)
target_link_libraries(test_hierarchical_clustering ${PROJECT_NAME} ${catkin_LIBRARIES})
# Simple test node for utility functions
add_executable(test_arc_utilities src/test_arc_utilities.cpp)
add_dependencies(test_arc_utilities ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # ${PROJECT_NAME}_gencpp)
target_link_libraries(test_arc_utilities ${PROJECT_NAME} ${catkin_LIBRARIES})
# # Simple test node for Eigen3 math
# add_executable(test_eigen_math src/test_eigen_math.cpp)
# add_dependencies(test_eigen_math ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # ${PROJECT_NAME}_gencpp)
# target_link_libraries(test_eigen_math ${PROJECT_NAME} ${catkin_LIBRARIES})
# # Simple test node for averaging math
# add_executable(test_averaging src/test_averaging.cpp)
# add_dependencies(test_averaging ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # ${PROJECT_NAME}_gencpp)
# target_link_libraries(test_averaging ${PROJECT_NAME} ${catkin_LIBRARIES})
# # Simple test node for Dijkstras
# add_executable(test_dijkstras src/test_dijkstras.cpp)
# add_dependencies(test_dijkstras ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # ${PROJECT_NAME}_gencpp)
# target_link_libraries(test_dijkstras ${PROJECT_NAME} ${catkin_LIBRARIES})
# # Simple test node for shortcut smoothing
# add_executable(test_shortcut_smoothing src/test_shortcut_smoothing.cpp)
# add_dependencies(test_shortcut_smoothing ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # ${PROJECT_NAME}_gencpp)
# target_link_libraries(test_shortcut_smoothing ${PROJECT_NAME} ${catkin_LIBRARIES})
# # Simple test node for DistanceToLine function
# add_executable(test_closest_point src/test_closest_point.cpp)
# add_dependencies(test_closest_point ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # ${PROJECT_NAME}_gencpp)
# target_link_libraries(test_closest_point ${PROJECT_NAME} ${catkin_LIBRARIES})
#############
## Install ##
#############
## Mark library for installation
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
PATTERN ".svn" EXCLUDE
)

24
flightlib/third_party/arc_utilities/LICENSE vendored Normal file
View File

@@ -0,0 +1,24 @@
Copyright (c) 2015, Autonomous Robotic Collaboration Laboratory
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

Some files were not shown because too many files have changed in this diff Show More