From 335c1f101586b907a66a900b0e16ab238e6c6020 Mon Sep 17 00:00:00 2001 From: ZbyLGsc <405540572@qq.com> Date: Sat, 11 Jan 2020 18:03:30 +0800 Subject: [PATCH] release mapping system for local planning --- .vscode/settings.json | 26 + README.md | 93 +- dyn_planner/.vscode/settings.json | 58 - dyn_planner/bspline_opt/CMakeLists.txt | 40 - .../include/bspline_opt/bspline_optimizer.h | 104 - .../include/bspline_opt/non_uniform_bspline.h | 68 - dyn_planner/bspline_opt/package.xml | 70 - .../bspline_opt/src/bspline_optimizer.cpp | 364 -- .../bspline_opt/src/non_uniform_bspline.cpp | 714 ---- dyn_planner/log.md | 31 - dyn_planner/path_searching/CMakeLists.txt | 39 - .../include/path_searching/astar.h | 173 - .../path_searching/kinodynamic_astar.h | 186 - dyn_planner/path_searching/package.xml | 70 - dyn_planner/path_searching/src/astar.cpp | 339 -- .../path_searching/src/kinodynamic_astar.cpp | 747 ---- dyn_planner/plan_env/CMakeLists.txt | 40 - .../include/plan_env/edt_environment.h | 49 - .../plan_env/include/plan_env/sdf_map.h | 105 - dyn_planner/plan_env/package.xml | 68 - dyn_planner/plan_env/src/edt_environment.cpp | 63 - dyn_planner/plan_env/src/sdf_map.cpp | 709 --- dyn_planner/plan_manage/CMakeLists.txt | 64 - dyn_planner/plan_manage/README.md | 2 - dyn_planner/plan_manage/config/traj.rviz | 551 --- .../include/plan_manage/backward.hpp | 3804 ----------------- .../include/plan_manage/dyn_planner_manager.h | 87 - .../include/plan_manage/planning_fsm.h | 102 - dyn_planner/plan_manage/launch/rviz.launch | 3 - .../plan_manage/launch/simulation.launch | 103 - dyn_planner/plan_manage/launch/simulator.xml | 79 - .../plan_manage/launch/test_collision.launch | 51 - dyn_planner/plan_manage/msg/Bspline.msg | 6 - dyn_planner/plan_manage/package.xml | 77 - .../plan_manage/script/polyfit_predict.py | 80 - dyn_planner/plan_manage/script/traj_opt.py | 78 - .../plan_manage/src/dyn_planner_manager.cpp | 220 - .../plan_manage/src/dyn_planner_node.cpp | 29 - dyn_planner/plan_manage/src/planning_fsm.cpp | 384 -- dyn_planner/plan_manage/src/traj_server.cpp | 243 -- dyn_planner/traj_utils/CMakeLists.txt | 38 - .../traj_utils/planning_visualization.h | 53 - dyn_planner/traj_utils/package.xml | 71 - .../traj_utils/src/planning_visualization.cpp | 79 - fast_planner | 1 + files/bib.txt | 10 + {img => files}/exp1.gif | Bin {img => files}/exp2.gif | Bin {img => files}/exp3.gif | Bin {img => files}/title.png | Bin uav_simulator/README.md | 2 - .../odom_visualization/CMakeLists.txt | 0 .../{ => Utils}/odom_visualization/Makefile | 0 .../odom_visualization/bin/odom_visualization | Bin .../bin/odom_visualization_vicon45 | Bin .../odom_visualization/build/CATKIN_IGNORE | 0 .../odom_visualization/build/CMakeCache.txt | 0 .../CMakeFiles/2.8.12.2/CMakeCCompiler.cmake | 0 .../2.8.12.2/CMakeCXXCompiler.cmake | 0 .../2.8.12.2/CMakeDetermineCompilerABI_C.bin | Bin .../CMakeDetermineCompilerABI_CXX.bin | Bin .../CMakeFiles/2.8.12.2/CMakeSystem.cmake | 0 .../2.8.12.2/CompilerIdC/CMakeCCompilerId.c | 0 .../CMakeFiles/2.8.12.2/CompilerIdC/a.out | Bin .../CompilerIdCXX/CMakeCXXCompilerId.cpp | 0 .../CMakeFiles/2.8.12.2/CompilerIdCXX/a.out | Bin .../CMakeDirectoryInformation.cmake | 0 .../build/CMakeFiles/CMakeError.log | 0 .../build/CMakeFiles/CMakeOutput.log | 0 .../build/CMakeFiles/CMakeRuleHashes.txt | 0 .../build/CMakeFiles/Makefile.cmake | 0 .../build/CMakeFiles/Makefile2 | 0 .../ROSBUILD_genmsg_cpp.dir/DependInfo.cmake | 0 .../ROSBUILD_genmsg_cpp.dir/build.make | 0 .../ROSBUILD_genmsg_cpp.dir/cmake_clean.cmake | 0 .../ROSBUILD_genmsg_cpp.dir/progress.make | 0 .../ROSBUILD_genmsg_lisp.dir/DependInfo.cmake | 0 .../ROSBUILD_genmsg_lisp.dir/build.make | 0 .../cmake_clean.cmake | 0 .../ROSBUILD_genmsg_lisp.dir/progress.make | 0 .../ROSBUILD_gensrv_cpp.dir/DependInfo.cmake | 0 .../ROSBUILD_gensrv_cpp.dir/build.make | 0 .../ROSBUILD_gensrv_cpp.dir/cmake_clean.cmake | 0 .../ROSBUILD_gensrv_cpp.dir/progress.make | 0 .../ROSBUILD_gensrv_lisp.dir/DependInfo.cmake | 0 .../ROSBUILD_gensrv_lisp.dir/build.make | 0 .../cmake_clean.cmake | 0 .../ROSBUILD_gensrv_lisp.dir/progress.make | 0 .../build/CMakeFiles/TargetDirectories.txt | 0 .../DependInfo.cmake | 0 .../build.make | 0 .../cmake_clean.cmake | 0 .../progress.make | 0 .../clean_test_results.dir/DependInfo.cmake | 0 .../clean_test_results.dir/build.make | 0 .../clean_test_results.dir/cmake_clean.cmake | 0 .../clean_test_results.dir/progress.make | 0 .../build/CMakeFiles/cmake.check_cache | 0 .../CMakeFiles/doxygen.dir/DependInfo.cmake | 0 .../build/CMakeFiles/doxygen.dir/build.make | 0 .../CMakeFiles/doxygen.dir/cmake_clean.cmake | 0 .../CMakeFiles/doxygen.dir/progress.make | 0 .../odom_visualization.dir/CXX.includecache | 0 .../odom_visualization.dir/DependInfo.cmake | 0 .../odom_visualization.dir/build.make | 0 .../odom_visualization.dir/cmake_clean.cmake | 0 .../odom_visualization.dir/depend.internal | 0 .../odom_visualization.dir/depend.make | 0 .../odom_visualization.dir/flags.make | 0 .../odom_visualization.dir/link.txt | 0 .../odom_visualization.dir/progress.make | 0 .../src/odom_visualization.cpp.o | Bin .../build/CMakeFiles/progress.marks | 0 .../DependInfo.cmake | 0 .../build.make | 0 .../cmake_clean.cmake | 0 .../progress.make | 0 .../rosbuild_precompile.dir/DependInfo.cmake | 0 .../rosbuild_precompile.dir/build.make | 0 .../rosbuild_precompile.dir/cmake_clean.cmake | 0 .../rosbuild_precompile.dir/depend.internal | 0 .../rosbuild_precompile.dir/depend.make | 0 .../rosbuild_precompile.dir/progress.make | 0 .../DependInfo.cmake | 0 .../rosbuild_premsgsrvgen.dir/build.make | 0 .../cmake_clean.cmake | 0 .../rosbuild_premsgsrvgen.dir/progress.make | 0 .../rospack_genmsg.dir/DependInfo.cmake | 0 .../CMakeFiles/rospack_genmsg.dir/build.make | 0 .../rospack_genmsg.dir/cmake_clean.cmake | 0 .../rospack_genmsg.dir/progress.make | 0 .../DependInfo.cmake | 0 .../rospack_genmsg_libexe.dir/build.make | 0 .../cmake_clean.cmake | 0 .../rospack_genmsg_libexe.dir/depend.internal | 0 .../rospack_genmsg_libexe.dir/depend.make | 0 .../rospack_genmsg_libexe.dir/progress.make | 0 .../rospack_gensrv.dir/DependInfo.cmake | 0 .../CMakeFiles/rospack_gensrv.dir/build.make | 0 .../rospack_gensrv.dir/cmake_clean.cmake | 0 .../rospack_gensrv.dir/progress.make | 0 .../CMakeFiles/run_tests.dir/DependInfo.cmake | 0 .../build/CMakeFiles/run_tests.dir/build.make | 0 .../run_tests.dir/cmake_clean.cmake | 0 .../CMakeFiles/run_tests.dir/progress.make | 0 .../test-future.dir/DependInfo.cmake | 0 .../CMakeFiles/test-future.dir/build.make | 0 .../test-future.dir/cmake_clean.cmake | 0 .../CMakeFiles/test-future.dir/progress.make | 0 .../test-results-run.dir/DependInfo.cmake | 0 .../test-results-run.dir/build.make | 0 .../test-results-run.dir/cmake_clean.cmake | 0 .../test-results-run.dir/progress.make | 0 .../test-results.dir/DependInfo.cmake | 0 .../CMakeFiles/test-results.dir/build.make | 0 .../test-results.dir/cmake_clean.cmake | 0 .../CMakeFiles/test-results.dir/progress.make | 0 .../CMakeFiles/test.dir/DependInfo.cmake | 0 .../build/CMakeFiles/test.dir/build.make | 0 .../CMakeFiles/test.dir/cmake_clean.cmake | 0 .../build/CMakeFiles/test.dir/progress.make | 0 .../CMakeFiles/tests.dir/DependInfo.cmake | 0 .../build/CMakeFiles/tests.dir/build.make | 0 .../CMakeFiles/tests.dir/cmake_clean.cmake | 0 .../build/CMakeFiles/tests.dir/progress.make | 0 .../odom_visualization/build/Makefile | 0 .../catkin_generated/version/package.cmake | 0 .../build/catkin_generated/env_cached.sh | 0 .../catkin_generated/generate_cached_setup.py | 0 .../catkin_generated/installspace/.rosinstall | 0 .../installspace/_setup_util.py | 0 .../catkin_generated/installspace/env.sh | 0 .../catkin_generated/installspace/setup.bash | 0 .../catkin_generated/installspace/setup.sh | 0 .../catkin_generated/installspace/setup.zsh | 0 .../catkin_generated/ordered_paths.cmake | 0 .../build/catkin_generated/setup_cached.sh | 0 .../interrogate_setup_dot_py.py.stamp | 0 .../odom_visualization/package.xml.stamp | 0 .../build/cmake_install.cmake | 0 .../odom_visualization/build/devel/.catkin | 0 .../build/devel/.rosinstall | 0 .../build/devel/_setup_util.py | 0 .../odom_visualization/build/devel/env.sh | 0 .../profile.d/05.catkin-test-results.sh | 0 .../etc/catkin/profile.d/05.catkin_make.bash | 0 .../profile.d/05.catkin_make_isolated.bash | 0 .../odom_visualization/build/devel/setup.bash | 0 .../odom_visualization/build/devel/setup.sh | 0 .../odom_visualization/build/devel/setup.zsh | 0 .../CMakeDirectoryInformation.cmake | 0 .../CMakeFiles/gtest.dir/DependInfo.cmake | 0 .../gtest/CMakeFiles/gtest.dir/build.make | 0 .../CMakeFiles/gtest.dir/cmake_clean.cmake | 0 .../gtest/CMakeFiles/gtest.dir/depend.make | 0 .../gtest/CMakeFiles/gtest.dir/flags.make | 0 .../build/gtest/CMakeFiles/gtest.dir/link.txt | 0 .../gtest/CMakeFiles/gtest.dir/progress.make | 0 .../gtest_main.dir/DependInfo.cmake | 0 .../CMakeFiles/gtest_main.dir/build.make | 0 .../gtest_main.dir/cmake_clean.cmake | 0 .../CMakeFiles/gtest_main.dir/depend.make | 0 .../CMakeFiles/gtest_main.dir/flags.make | 0 .../gtest/CMakeFiles/gtest_main.dir/link.txt | 0 .../CMakeFiles/gtest_main.dir/progress.make | 0 .../build/gtest/CMakeFiles/progress.marks | 0 .../odom_visualization/build/gtest/Makefile | 0 .../build/gtest/cmake_install.cmake | 0 .../odom_visualization/mainpage.dox | 0 .../meshes/hummingbird.mesh | Bin .../odom_visualization/package.xml | 0 .../src/odom_visualization.cpp | 0 .../src/odom_visualization.cpp~ | 0 .../{ => Utils}/quadrotor_msgs/CMakeLists.txt | 0 .../quadrotor_msgs/cmake/FindEigen3.cmake | 0 .../include/quadrotor_msgs/comm_types.h | 0 .../include/quadrotor_msgs/decode_msgs.h | 0 .../include/quadrotor_msgs/encode_msgs.h | 0 .../quadrotor_msgs/msg/AuxCommand.msg | 0 .../quadrotor_msgs/msg/Corrections.msg | 0 .../{ => Utils}/quadrotor_msgs/msg/Gains.msg | 0 .../quadrotor_msgs/msg/LQRTrajectory.msg | 0 .../quadrotor_msgs/msg/Odometry.msg | 0 .../quadrotor_msgs/msg/OutputData.msg | 0 .../quadrotor_msgs/msg/PPROutputData.msg | 0 .../msg/PolynomialTrajectory.msg | 0 .../msg/PolynomialTrajectory.msg~ | 0 .../quadrotor_msgs/msg/PositionCommand.msg | 0 .../quadrotor_msgs/msg/PositionCommand.msg~ | 0 .../quadrotor_msgs/msg/SO3Command.msg | 0 .../{ => Utils}/quadrotor_msgs/msg/Serial.msg | 0 .../quadrotor_msgs/msg/StatusData.msg | 0 .../quadrotor_msgs/msg/TRPYCommand.msg | 0 .../{ => Utils}/quadrotor_msgs/package.xml | 0 .../quadrotor_msgs/src/decode_msgs.cpp | 0 .../quadrotor_msgs/src/encode_msgs.cpp | 0 .../src/quadrotor_msgs/__init__.py | 0 .../src/quadrotor_msgs/__init__.pyc | Bin .../src/quadrotor_msgs/msg/_AuxCommand.py | 0 .../src/quadrotor_msgs/msg/_Corrections.py | 0 .../src/quadrotor_msgs/msg/_Gains.py | 0 .../src/quadrotor_msgs/msg/_OutputData.py | 0 .../src/quadrotor_msgs/msg/_PPROutputData.py | 0 .../quadrotor_msgs/msg/_PositionCommand.py | 0 .../src/quadrotor_msgs/msg/_SO3Command.py | 0 .../src/quadrotor_msgs/msg/_Serial.py | 0 .../src/quadrotor_msgs/msg/_StatusData.py | 0 .../src/quadrotor_msgs/msg/_TRPYCommand.py | 0 .../src/quadrotor_msgs/msg/__init__.py | 0 .../Utils/rviz_plugins/CMakeLists.txt | 1 - .../Utils/rviz_plugins/src/gamelikeinput.cpp | 4 +- .../src/waypoint_generator.cpp | 2 +- .../local_sensing/.vscode/settings.json | 5 + uav_simulator/local_sensing/CMakeLists.txt | 89 + .../local_sensing/CMakeModules/FindCUDA.cmake | 1311 ++++++ .../CMakeModules/FindCUDA/make2cmake.cmake | 79 + .../CMakeModules/FindCUDA/parse_cubin.cmake | 112 + .../CMakeModules/FindCUDA/run_nvcc.cmake | 280 ++ .../CMakeModules/FindEigen.cmake | 81 + .../CMakeModules/FindEigen.cmake~ | 81 + .../local_sensing/cfg/local_sensing_node.cfg | 15 + uav_simulator/local_sensing/package.xml | 38 + .../local_sensing/params/camera.yaml | 12 + uav_simulator/local_sensing/src/AlignError.h | 86 + .../local_sensing/src/ceres_extensions.h | 161 + .../local_sensing/src/csv_convert.py | 15 + .../local_sensing/src/cuda_exception.cuh | 45 + .../local_sensing/src/depth_render.cu | 197 + .../local_sensing/src/depth_render.cuh | 52 + .../local_sensing/src/device_image.cuh | 179 + uav_simulator/local_sensing/src/empty.cpp | 1 + uav_simulator/local_sensing/src/empty.h | 0 uav_simulator/local_sensing/src/euroc.cpp | 378 ++ uav_simulator/local_sensing/src/helper_math.h | 1453 +++++++ .../local_sensing/src/pcl_render_node.cpp | 390 ++ .../src/pointcloud_render_node.cpp | 199 + .../.vscode/c_cpp_properties.json | 10 +- .../src/random_forest_sensing.cpp | 198 +- .../so3_disturbance_generator/CMakeLists.txt | 46 +- 279 files changed, 5559 insertions(+), 10334 deletions(-) create mode 100644 .vscode/settings.json delete mode 100644 dyn_planner/.vscode/settings.json delete mode 100644 dyn_planner/bspline_opt/CMakeLists.txt delete mode 100644 dyn_planner/bspline_opt/include/bspline_opt/bspline_optimizer.h delete mode 100644 dyn_planner/bspline_opt/include/bspline_opt/non_uniform_bspline.h delete mode 100644 dyn_planner/bspline_opt/package.xml delete mode 100644 dyn_planner/bspline_opt/src/bspline_optimizer.cpp delete mode 100644 dyn_planner/bspline_opt/src/non_uniform_bspline.cpp delete mode 100644 dyn_planner/log.md delete mode 100644 dyn_planner/path_searching/CMakeLists.txt delete mode 100644 dyn_planner/path_searching/include/path_searching/astar.h delete mode 100644 dyn_planner/path_searching/include/path_searching/kinodynamic_astar.h delete mode 100644 dyn_planner/path_searching/package.xml delete mode 100644 dyn_planner/path_searching/src/astar.cpp delete mode 100644 dyn_planner/path_searching/src/kinodynamic_astar.cpp delete mode 100644 dyn_planner/plan_env/CMakeLists.txt delete mode 100644 dyn_planner/plan_env/include/plan_env/edt_environment.h delete mode 100644 dyn_planner/plan_env/include/plan_env/sdf_map.h delete mode 100644 dyn_planner/plan_env/package.xml delete mode 100644 dyn_planner/plan_env/src/edt_environment.cpp delete mode 100644 dyn_planner/plan_env/src/sdf_map.cpp delete mode 100644 dyn_planner/plan_manage/CMakeLists.txt delete mode 100644 dyn_planner/plan_manage/README.md delete mode 100644 dyn_planner/plan_manage/config/traj.rviz delete mode 100644 dyn_planner/plan_manage/include/plan_manage/backward.hpp delete mode 100644 dyn_planner/plan_manage/include/plan_manage/dyn_planner_manager.h delete mode 100644 dyn_planner/plan_manage/include/plan_manage/planning_fsm.h delete mode 100644 dyn_planner/plan_manage/launch/rviz.launch delete mode 100644 dyn_planner/plan_manage/launch/simulation.launch delete mode 100644 dyn_planner/plan_manage/launch/simulator.xml delete mode 100644 dyn_planner/plan_manage/launch/test_collision.launch delete mode 100644 dyn_planner/plan_manage/msg/Bspline.msg delete mode 100644 dyn_planner/plan_manage/package.xml delete mode 100644 dyn_planner/plan_manage/script/polyfit_predict.py delete mode 100644 dyn_planner/plan_manage/script/traj_opt.py delete mode 100644 dyn_planner/plan_manage/src/dyn_planner_manager.cpp delete mode 100644 dyn_planner/plan_manage/src/dyn_planner_node.cpp delete mode 100644 dyn_planner/plan_manage/src/planning_fsm.cpp delete mode 100644 dyn_planner/plan_manage/src/traj_server.cpp delete mode 100644 dyn_planner/traj_utils/CMakeLists.txt delete mode 100644 dyn_planner/traj_utils/include/traj_utils/planning_visualization.h delete mode 100644 dyn_planner/traj_utils/package.xml delete mode 100644 dyn_planner/traj_utils/src/planning_visualization.cpp create mode 160000 fast_planner create mode 100644 files/bib.txt rename {img => files}/exp1.gif (100%) rename {img => files}/exp2.gif (100%) rename {img => files}/exp3.gif (100%) rename {img => files}/title.png (100%) delete mode 100644 uav_simulator/README.md rename uav_simulator/{ => Utils}/odom_visualization/CMakeLists.txt (100%) rename uav_simulator/{ => Utils}/odom_visualization/Makefile (100%) rename uav_simulator/{ => Utils}/odom_visualization/bin/odom_visualization (100%) rename uav_simulator/{ => Utils}/odom_visualization/bin/odom_visualization_vicon45 (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CATKIN_IGNORE (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeCache.txt (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/CMakeDirectoryInformation.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/CMakeError.log (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/CMakeOutput.log (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/CMakeRuleHashes.txt (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/Makefile.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/Makefile2 (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/TargetDirectories.txt (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/clean_test_results.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/clean_test_results.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/cmake.check_cache (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/doxygen.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/doxygen.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/doxygen.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/doxygen.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/odom_visualization.dir/CXX.includecache (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/odom_visualization.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/odom_visualization.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/odom_visualization.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/odom_visualization.dir/depend.internal (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/odom_visualization.dir/depend.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/odom_visualization.dir/flags.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/odom_visualization.dir/link.txt (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/odom_visualization.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/odom_visualization.dir/src/odom_visualization.cpp.o (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/progress.marks (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/depend.internal (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/depend.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/depend.internal (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/depend.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/run_tests.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/run_tests.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/run_tests.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/run_tests.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/test-future.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/test-future.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/test-future.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/test-future.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/test-results-run.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/test-results-run.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/test-results-run.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/test-results-run.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/test-results.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/test-results.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/test-results.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/test-results.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/test.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/test.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/test.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/test.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/tests.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/tests.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/tests.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/CMakeFiles/tests.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/Makefile (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/catkin/catkin_generated/version/package.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/catkin_generated/env_cached.sh (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/catkin_generated/generate_cached_setup.py (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/catkin_generated/installspace/.rosinstall (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/catkin_generated/installspace/_setup_util.py (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/catkin_generated/installspace/env.sh (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/catkin_generated/installspace/setup.bash (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/catkin_generated/installspace/setup.sh (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/catkin_generated/installspace/setup.zsh (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/catkin_generated/ordered_paths.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/catkin_generated/setup_cached.sh (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/catkin_generated/stamps/odom_visualization/interrogate_setup_dot_py.py.stamp (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/catkin_generated/stamps/odom_visualization/package.xml.stamp (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/cmake_install.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/devel/.catkin (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/devel/.rosinstall (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/devel/_setup_util.py (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/devel/env.sh (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/devel/etc/catkin/profile.d/05.catkin-test-results.sh (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/devel/etc/catkin/profile.d/05.catkin_make.bash (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/devel/etc/catkin/profile.d/05.catkin_make_isolated.bash (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/devel/setup.bash (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/devel/setup.sh (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/devel/setup.zsh (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/CMakeFiles/gtest.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/CMakeFiles/gtest.dir/depend.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/CMakeFiles/gtest.dir/flags.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/CMakeFiles/gtest.dir/link.txt (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/CMakeFiles/gtest.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/build.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/depend.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/flags.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/link.txt (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/progress.make (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/CMakeFiles/progress.marks (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/Makefile (100%) rename uav_simulator/{ => Utils}/odom_visualization/build/gtest/cmake_install.cmake (100%) rename uav_simulator/{ => Utils}/odom_visualization/mainpage.dox (100%) rename uav_simulator/{ => Utils}/odom_visualization/meshes/hummingbird.mesh (100%) rename uav_simulator/{ => Utils}/odom_visualization/package.xml (100%) rename uav_simulator/{ => Utils}/odom_visualization/src/odom_visualization.cpp (100%) rename uav_simulator/{ => Utils}/odom_visualization/src/odom_visualization.cpp~ (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/CMakeLists.txt (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/cmake/FindEigen3.cmake (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/include/quadrotor_msgs/comm_types.h (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/include/quadrotor_msgs/decode_msgs.h (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/include/quadrotor_msgs/encode_msgs.h (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/msg/AuxCommand.msg (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/msg/Corrections.msg (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/msg/Gains.msg (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/msg/LQRTrajectory.msg (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/msg/Odometry.msg (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/msg/OutputData.msg (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/msg/PPROutputData.msg (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/msg/PolynomialTrajectory.msg (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/msg/PolynomialTrajectory.msg~ (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/msg/PositionCommand.msg (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/msg/PositionCommand.msg~ (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/msg/SO3Command.msg (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/msg/Serial.msg (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/msg/StatusData.msg (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/msg/TRPYCommand.msg (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/package.xml (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/src/decode_msgs.cpp (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/src/encode_msgs.cpp (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/src/quadrotor_msgs/__init__.py (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/src/quadrotor_msgs/__init__.pyc (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/src/quadrotor_msgs/msg/_AuxCommand.py (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/src/quadrotor_msgs/msg/_Corrections.py (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/src/quadrotor_msgs/msg/_Gains.py (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/src/quadrotor_msgs/msg/_OutputData.py (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/src/quadrotor_msgs/msg/_PPROutputData.py (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/src/quadrotor_msgs/msg/_PositionCommand.py (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/src/quadrotor_msgs/msg/_SO3Command.py (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/src/quadrotor_msgs/msg/_Serial.py (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/src/quadrotor_msgs/msg/_StatusData.py (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/src/quadrotor_msgs/msg/_TRPYCommand.py (100%) rename uav_simulator/{ => Utils}/quadrotor_msgs/src/quadrotor_msgs/msg/__init__.py (100%) create mode 100644 uav_simulator/local_sensing/.vscode/settings.json create mode 100644 uav_simulator/local_sensing/CMakeLists.txt create mode 100644 uav_simulator/local_sensing/CMakeModules/FindCUDA.cmake create mode 100644 uav_simulator/local_sensing/CMakeModules/FindCUDA/make2cmake.cmake create mode 100644 uav_simulator/local_sensing/CMakeModules/FindCUDA/parse_cubin.cmake create mode 100644 uav_simulator/local_sensing/CMakeModules/FindCUDA/run_nvcc.cmake create mode 100644 uav_simulator/local_sensing/CMakeModules/FindEigen.cmake create mode 100644 uav_simulator/local_sensing/CMakeModules/FindEigen.cmake~ create mode 100755 uav_simulator/local_sensing/cfg/local_sensing_node.cfg create mode 100644 uav_simulator/local_sensing/package.xml create mode 100644 uav_simulator/local_sensing/params/camera.yaml create mode 100644 uav_simulator/local_sensing/src/AlignError.h create mode 100644 uav_simulator/local_sensing/src/ceres_extensions.h create mode 100644 uav_simulator/local_sensing/src/csv_convert.py create mode 100644 uav_simulator/local_sensing/src/cuda_exception.cuh create mode 100644 uav_simulator/local_sensing/src/depth_render.cu create mode 100644 uav_simulator/local_sensing/src/depth_render.cuh create mode 100644 uav_simulator/local_sensing/src/device_image.cuh create mode 100644 uav_simulator/local_sensing/src/empty.cpp create mode 100644 uav_simulator/local_sensing/src/empty.h create mode 100644 uav_simulator/local_sensing/src/euroc.cpp create mode 100644 uav_simulator/local_sensing/src/helper_math.h create mode 100644 uav_simulator/local_sensing/src/pcl_render_node.cpp create mode 100644 uav_simulator/local_sensing/src/pointcloud_render_node.cpp rename {dyn_planner => uav_simulator/map_generator}/.vscode/c_cpp_properties.json (52%) diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 000000000..7f6db7481 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,26 @@ +{ + "workbench.colorCustomizations": { + "activityBarBadge.background": "#2979FF", + "list.activeSelectionForeground": "#2979FF", + "list.inactiveSelectionForeground": "#2979FF", + "list.highlightForeground": "#2979FF", + "scrollbarSlider.activeBackground": "#2979FF50", + "editorSuggestWidget.highlightForeground": "#2979FF", + "textLink.foreground": "#2979FF", + "progressBar.background": "#2979FF", + "pickerGroup.foreground": "#2979FF", + "tab.activeBorder": "#2979FF", + "notificationLink.foreground": "#2979FF", + "editorWidget.resizeBorder": "#2979FF", + "editorWidget.border": "#2979FF", + "settings.modifiedItemIndicator": "#2979FF", + "settings.headerForeground": "#2979FF", + "panelTitle.activeBorder": "#2979FF", + "breadcrumb.activeSelectionForeground": "#2979FF", + "menu.selectionForeground": "#2979FF", + "menubar.selectionForeground": "#2979FF", + "activityBar.background": "#092E56", + "titleBar.activeBackground": "#0C4078", + "titleBar.activeForeground": "#F6FAFE" + } +} \ No newline at end of file diff --git a/README.md b/README.md index 729954a81..6980bf1ef 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,20 @@ # Fast-Planner -__Fast-Planner__ is a quadrotor trajectory generator for fast autonomous flight. It consists of -the front-end _kinodynamic path searching_, the back-end _gradient-based Bspline trajectory optimization_ and the postprocessing named as _iterative time adjustment_. The key features of the planner are that it generates -high-quality trajectories within a few milliseconds and that it can generate aggressive motion under the premise of dynamic feasibility. This work was reported on the [IEEE Spectrum](https://spectrum.ieee.org/automaton/robotics/robotics-hardware/video-friday-nasa-lemur-robot). +## News + +This package is under active maintenance. New features will be listed here. + +- The online mapping algorithm is now available. It can take in depth image and camera pose pairs as input, do raycasting to update a probabilistic volumetric map, and build a Euclidean signed distance filed (ESDF) for the planning system. + +## Overview + +__Fast-Planner__ is a robust and efficient planning system that enables agile and fast autonomous flight for quadrotors. +It takes in information from odometry, sensor streams (such as depth images and point cloud), and outputs high-quality trajectories within a few milliseconds. +It can support aggressive and fully autonomous flight even in unknown and cluttered environments. +Demonstrations about the planner have been reported on the [IEEE Spectrum](https://spectrum.ieee.org/automaton/robotics/robotics-hardware/video-friday-nasa-lemur-robot). + + -This package is under active maintenance. More features (e.g., avoiding dynamic obstacles, discovering topological distinctive trajectories) will be added in the future. __Authors__: [Boyu Zhou](http://boyuzhou.net), [Fei Gao](https://ustfei.com/) and [Shaojie Shen](http://uav.ust.hk/group/) from the [HUKST Aerial Robotics Group](http://uav.ust.hk/). @@ -12,47 +22,43 @@ __Video__:

- + - +

- video + video

-This package contains the implementation of __Fast-Planner__ (in folder __dyn_planner__) and a lightweight -quadrotor simulator (in __uav_simulator__). +This package contains the implementation of __Fast-Planner__ (in folder __fast_planner__) and a lightweight +quadrotor simulator (in __uav_simulator__). Key components are: + +- __plan_env__: The online mapping algorithms. It takes in depth image (or point cloud) and camera pose (odometry) pairs as input, do raycasting to update a probabilistic volumetric map, and build an Euclidean signed distance filed (ESDF) for the planning system. +- __path_searching__: Front-end path searching algorithms. Currently it includes a kinodynamic version of A* algorithm that respects the dynamics of quadrotors. The standard A* is also available. +- __bspline_opt__: The gradient-based trajectory optimization based on B-spline trajectory representation. +- __plan_manage__: High-level modules that schedule and call the mapping and planning algorithms. Interfaces for launching the whole system, as well as the configuration files are contained here. -If you use __Fast-Planner__ for your application or research, please cite our related papers: +If you use __Fast-Planner__ for your application or research, please cite our [related papers](files/bib.txt): - [__Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight__](https://ieeexplore.ieee.org/document/8758904), Boyu Zhou, Fei Gao, Luqi Wang, Chuhao Liu and Shaojie Shen, IEEE Robotics and Automation Letters (RA-L), 2019. -``` -@article{zhou2019robust, - title={Robust and efficient quadrotor trajectory generation for fast autonomous flight}, - author={Zhou, Boyu and Gao, Fei and Wang, Luqi and Liu, Chuhao and Shen, Shaojie}, - journal={IEEE Robotics and Automation Letters}, - volume={4}, - number={4}, - pages={3529--3536}, - year={2019}, - publisher={IEEE} -} -``` + ## 1. Prerequisites -- Our software is developed in Ubuntu 16.04, [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu). +- Our software is developed and tested in Ubuntu 16.04, [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu). Other version may require minor modification. - We use [**NLopt**](https://nlopt.readthedocs.io/en/latest/NLopt_Installation) to solve the non-linear optimization problem. -- The drone simulator depends on the C++ linear algebra library __Armadillo__, which can be installed by ``` sudo apt-get install libarmadillo-dev ```. +- The __uav_simulator__ depends on the C++ linear algebra library __Armadillo__, which can be installed by ``` sudo apt-get install libarmadillo-dev ```. + +- _Optional_: If you want to run the more realistic depth camera in __uav_simulator__, installation of [CUDA Toolkit](https://developer.nvidia.com/cuda-toolkit) is needed. Otherwise, a less realistic depth sensor model will be used (See section _Use GPU Depth Rendering_ below). ## 2. Build on ROS -Clone this repository to your catkin workspace and catkin_make. A new workspace is recommended: +After the prerequisites are satisfied, you can clone this repository to your catkin workspace and catkin_make. A new workspace is recommended: ``` cd ${YOUR_WORKSPACE_PATH}/src git clone https://github.com/HKUST-Aerial-Robotics/Fast-Planner.git @@ -60,6 +66,24 @@ Clone this repository to your catkin workspace and catkin_make. A new workspace catkin_make ``` +### Use GPU Depth Rendering (Optional) + + The **local_sensing** package in __uav_simulator__ has the option of using GPU or CPU to render the depth sensor measurement. By default, it is set to CPU version in CMakeLists: + + ``` + set(ENABLE_CUDA false) + # set(ENABLE_CUDA true) + ``` +The GPU version is recommended, because it generates depth images more like a real depth camera. +If you want to use the GPU depth rendering, set ENABLE_CUDA to true, and also remember to change the 'arch' and 'code' flags according to your graphics card devices. You can check the right code [here](https://github.com/tpruvot/ccminer/wiki/Compatibility). + +``` + set(CUDA_NVCC_FLAGS + -gencode arch=compute_61,code=sm_61; + ) +``` +For installation of CUDA, please go to [CUDA ToolKit](https://developer.nvidia.com/cuda-toolkit) + ## 3. Run the Simulation Run [Rviz](http://wiki.ros.org/rviz) with our configuration firstly: @@ -78,21 +102,26 @@ Then run the quadrotor simulator and __Fast-Planner__: roslaunch plan_manage simulation.launch ``` -Normally, you will find the randomly generated map and the drone model in ```Rviz```. At this time, you can select a goal for the drone using the ```3D Nav Goal``` tool. - -In the tools panel of Rviz, click '+' and select the plugin 'Goal3DTool'. If you have successfully compiled all packages from __uav_simulator__, now you can see '3D Nav Goal' in the tools panel. To set a goal, click the '3D Nav Goal' (shortcut keyboard 'g', may conflict with 2D Nav Goal). Then __click__ and __hold__ both the left and right mouse buttons to select _(x,y)_, and __move__ the mouse to change _z_. When a goal is set successfully, a new trajectory will be generated immediately and executed by the drone, as displayed below: +Normally, you will find the randomly generated map and the drone model in ```Rviz```. At this time, you can select a goal for the drone using the ```2D Nav Goal``` tool. When a goal is set successfully, a new trajectory will be generated immediately and executed by the drone. A sample is displayed below:

- +

-## 4. Acknowledgements +## 4. Use in Your Application + +If you have successfully run the simulation and want to use __Fast-Planner__ in your project, +please explore the simulation.launch file. +Important parameters that may be changed in your usage are contained and documented. +Finally, please kindly give a STAR to this repo if it helps your research or work, thanks! :) + +## 5. Acknowledgements We use **NLopt** for non-linear optimization. -## 5. Licence +## 6. Licence The source code is released under [GPLv3](http://www.gnu.org/licenses/) license. -## 6. Disclaimer +## 7. Disclaimer This is research code, it 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. diff --git a/dyn_planner/.vscode/settings.json b/dyn_planner/.vscode/settings.json deleted file mode 100644 index b3c8d2af8..000000000 --- a/dyn_planner/.vscode/settings.json +++ /dev/null @@ -1,58 +0,0 @@ -{ - "files.associations": { - "cctype": "cpp", - "cmath": "cpp", - "csignal": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "array": "cpp", - "atomic": "cpp", - "strstream": "cpp", - "*.tcc": "cpp", - "bitset": "cpp", - "chrono": "cpp", - "clocale": "cpp", - "complex": "cpp", - "cstdint": "cpp", - "deque": "cpp", - "list": "cpp", - "unordered_map": "cpp", - "vector": "cpp", - "exception": "cpp", - "optional": "cpp", - "fstream": "cpp", - "functional": "cpp", - "initializer_list": "cpp", - "iomanip": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "istream": "cpp", - "limits": "cpp", - "memory": "cpp", - "new": "cpp", - "ostream": "cpp", - "numeric": "cpp", - "ratio": "cpp", - "sstream": "cpp", - "stdexcept": "cpp", - "streambuf": "cpp", - "system_error": "cpp", - "thread": "cpp", - "cinttypes": "cpp", - "type_traits": "cpp", - "tuple": "cpp", - "typeindex": "cpp", - "typeinfo": "cpp", - "utility": "cpp", - "algorithm": "cpp", - "core": "cpp", - "eigen": "cpp" -}, -"python.formatting.provider": "yapf" -} \ No newline at end of file diff --git a/dyn_planner/bspline_opt/CMakeLists.txt b/dyn_planner/bspline_opt/CMakeLists.txt deleted file mode 100644 index dc2763329..000000000 --- a/dyn_planner/bspline_opt/CMakeLists.txt +++ /dev/null @@ -1,40 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(bspline_opt) - -find_package(Eigen3 REQUIRED) -find_package(PCL 1.7 REQUIRED) - -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - visualization_msgs - plan_env -) - - -catkin_package( - INCLUDE_DIRS include - LIBRARIES bspline_opt - CATKIN_DEPENDS plan_env -# DEPENDS system_lib -) - -include_directories( - SYSTEM - include - ${catkin_INCLUDE_DIRS} - ${Eigen3_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS} -O3 -Wall") - -add_library( bspline_opt - src/bspline_optimizer.cpp - src/non_uniform_bspline.cpp - ) -target_link_libraries( bspline_opt - ${catkin_LIBRARIES} - /usr/local/lib/libnlopt.so - ) diff --git a/dyn_planner/bspline_opt/include/bspline_opt/bspline_optimizer.h b/dyn_planner/bspline_opt/include/bspline_opt/bspline_optimizer.h deleted file mode 100644 index 9f91d82ea..000000000 --- a/dyn_planner/bspline_opt/include/bspline_opt/bspline_optimizer.h +++ /dev/null @@ -1,104 +0,0 @@ -#ifndef _BSPLINE_OPTIMIZER_H_ -#define _BSPLINE_OPTIMIZER_H_ - -#include -#include -#include "plan_env/edt_environment.h" - -// Gradient and elasitc band optimization - -// Input: a signed distance field and a sequence of points -// Output: the optimized sequence of points -// The format of points: N x 3 matrix, each row is a point -namespace dyn_planner -{ -class BsplineOptimizer -{ -private: - EDTEnvironment::Ptr edt_env_; - Eigen::MatrixXd control_points_; // nx3 - Eigen::Vector3d end_pt_; - vector> ranges_; - bool use_guide_; - - /* optimization parameters */ - double lamda1_; // curvature weight - double lamda2_; // distance weight - double lamda3_; // feasibility weight - double lamda4_; // end point weight - double lamda5_; // guide cost weight - double dist0_; // safe distance - double dist1_; // unsafe distance - double max_vel_, max_acc_; // constrains parameters - int variable_num_; - int algorithm_; - int max_iteration_num_, iter_num_; - std::vector best_variable_; - double min_cost_; - int start_id_, end_id_; - - /* bspline */ - double bspline_interval_; // ts - int order_; // bspline order - - int end_constrain_; - bool dynamic_; - double time_traj_start_; - - int collision_type_; - -public: - BsplineOptimizer() - { - } - ~BsplineOptimizer() - { - } - - enum END_CONSTRAINT - { - HARD_CONSTRAINT = 1, - SOFT_CONSTRAINT = 2 - }; - - /* main API */ - void setControlPoints(Eigen::MatrixXd points); - void setBSplineInterval(double ts); - void setEnvironment(const EDTEnvironment::Ptr& env); - - void setParam(ros::NodeHandle& nh); - void setOptimizationRange(int start, int end); - - void optimize(int end_cons, bool dynamic, double time_start = -1.0); - Eigen::MatrixXd getControlPoints(); - -private: - /* NLopt cost */ - static double costFunction(const std::vector& x, std::vector& grad, void* func_data); - /* helper function */ - void getDistanceAndGradient(Eigen::Vector3d& pos, double& dist, Eigen::Vector3d& grad); - - /* calculate each part of cost function with control points q */ - void combineCost(const std::vector& x, vector& grad, double& cost); - - void calcSmoothnessCost(const vector& q, double& cost, vector& gradient); - void calcDistanceCost(const vector& q, double& cost, vector& gradient); - void calcFeasibilityCost(const vector& q, double& cost, vector& gradient); - void calcEndpointCost(const vector& q, double& cost, vector& gradient); - -public: - /* for evaluation */ - vector vec_cost_; - vector vec_time_; - ros::Time time_start_; - - void getCostCurve(vector& cost, vector& time) - { - cost = vec_cost_; - time = vec_time_; - } - - typedef shared_ptr Ptr; -}; -} // namespace dyn_planner -#endif \ No newline at end of file diff --git a/dyn_planner/bspline_opt/include/bspline_opt/non_uniform_bspline.h b/dyn_planner/bspline_opt/include/bspline_opt/non_uniform_bspline.h deleted file mode 100644 index aa35219c5..000000000 --- a/dyn_planner/bspline_opt/include/bspline_opt/non_uniform_bspline.h +++ /dev/null @@ -1,68 +0,0 @@ -#ifndef _NON_UNIFORM_BSPLINE_H_ -#define _NON_UNIFORM_BSPLINE_H_ - -#include -#include -#include - -using namespace std; - -namespace dyn_planner -{ -class NonUniformBspline -{ -private: - /* non-uniform bspline */ - int p_, n_, m_; - Eigen::MatrixXd control_points_; - Eigen::VectorXd u_; // knots vector - double interval_; // init interval - - Eigen::Vector3d x0_, v0_, a0_; - - Eigen::MatrixXd getDerivativeControlPoints(); - -public: - static double limit_vel_, limit_acc_, limit_ratio_; - - NonUniformBspline() - { - } - NonUniformBspline(Eigen::MatrixXd points, int order, double interval, bool zero = true); - ~NonUniformBspline(); - void setKnot(Eigen::VectorXd knot); - Eigen::VectorXd getKnot(); - - Eigen::MatrixXd getControlPoint() - { - return control_points_; - } - - void getTimeSpan(double& um, double& um_p); - - Eigen::Vector3d evaluateDeBoor(double t); - - NonUniformBspline getDerivative(); - - static void getControlPointEqu3(Eigen::MatrixXd samples, double ts, Eigen::MatrixXd& control_pts); - static void BsplineParameterize(const double& ts, const vector& point_set, - const vector& start_end_derivative, Eigen::MatrixXd& ctrl_pts); - - /* check feasibility, reallocate time and recompute first 3 ctrl pts */ - bool checkFeasibility(bool show = false); - bool reallocateTime(bool show = false); - bool adjustTime(bool show = false); - void recomputeInit(); - - /* for evaluation */ - double getTimeSum(); - double getLength(); - double getJerk(); - - void getMeanAndMaxVel(double& mean_v, double& max_v); - void getMeanAndMaxAcc(double& mean_a, double& max_a); - - // typedef std::shared_ptr Ptr; -}; -} // namespace dyn_planner -#endif \ No newline at end of file diff --git a/dyn_planner/bspline_opt/package.xml b/dyn_planner/bspline_opt/package.xml deleted file mode 100644 index f31ee0eb2..000000000 --- a/dyn_planner/bspline_opt/package.xml +++ /dev/null @@ -1,70 +0,0 @@ - - - bspline_opt - 0.0.0 - The bspline_opt package - - - - - bzhouai - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - plan_env - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - plan_env - - - - - - - - diff --git a/dyn_planner/bspline_opt/src/bspline_optimizer.cpp b/dyn_planner/bspline_opt/src/bspline_optimizer.cpp deleted file mode 100644 index 44251d35f..000000000 --- a/dyn_planner/bspline_opt/src/bspline_optimizer.cpp +++ /dev/null @@ -1,364 +0,0 @@ -#include "bspline_opt/bspline_optimizer.h" -#include -using namespace std; - -namespace dyn_planner -{ -void BsplineOptimizer::setControlPoints(Eigen::MatrixXd points) -{ - this->control_points_ = points; - this->start_id_ = order_; - this->end_id_ = this->control_points_.rows() - order_; - use_guide_ = false; -} - -void BsplineOptimizer::setOptimizationRange(int start, int end) -{ - this->start_id_ = min(max(start, order_), int(control_points_.rows() - order_)); - this->end_id_ = min(max(end, order_), int(control_points_.rows() - order_)); - cout << "opt range:" << this->start_id_ << ", " << this->end_id_ << endl; -} - -void BsplineOptimizer::setParam(ros::NodeHandle& nh) -{ - nh.param("optimization/lamda1", lamda1_, -1.0); - nh.param("optimization/lamda2", lamda2_, -1.0); - nh.param("optimization/lamda3", lamda3_, -1.0); - nh.param("optimization/lamda4", lamda4_, -1.0); - nh.param("optimization/lamda5", lamda5_, -1.0); - nh.param("optimization/dist0", dist0_, -1.0); - nh.param("optimization/dist1", dist1_, -1.0); - nh.param("optimization/max_vel", max_vel_, -1.0); - nh.param("optimization/max_acc", max_acc_, -1.0); - nh.param("optimization/max_iteration_num", max_iteration_num_, -1); - nh.param("optimization/algorithm", algorithm_, -1); - nh.param("optimization/order", order_, -1); - - std::cout << "lamda1: " << lamda1_ << std::endl; - std::cout << "lamda2: " << lamda2_ << std::endl; - std::cout << "lamda3: " << lamda3_ << std::endl; - std::cout << "lamda4: " << lamda4_ << std::endl; -} - -void BsplineOptimizer::setBSplineInterval(double ts) -{ - this->bspline_interval_ = ts; -} - -void BsplineOptimizer::setEnvironment(const EDTEnvironment::Ptr& env) -{ - this->edt_env_ = env; -} - -Eigen::MatrixXd BsplineOptimizer::getControlPoints() -{ - return this->control_points_; -} - -/* best algorithm_ is 40: SLSQP(constrained), 11 LBFGS(unconstrained barrier -method */ -void BsplineOptimizer::optimize(int end_cons, bool dynamic, double time_start) -{ - /* ---------- initialize solver ---------- */ - end_constrain_ = end_cons; - dynamic_ = dynamic; - time_traj_start_ = time_start; - iter_num_ = 0; - - if (end_constrain_ == HARD_CONSTRAINT) - { - variable_num_ = 3 * (end_id_ - start_id_); - // std::cout << "hard: " << end_constrain_ << std::endl; - } - else if (end_constrain_ == SOFT_CONSTRAINT) - { - variable_num_ = 3 * (control_points_.rows() - start_id_); - // std::cout << "soft: " << end_constrain_ << std::endl; - } - - min_cost_ = std::numeric_limits::max(); - - nlopt::opt opt(nlopt::algorithm(algorithm_), variable_num_); - - opt.set_min_objective(BsplineOptimizer::costFunction, this); - opt.set_maxeval(max_iteration_num_); - // opt.set_xtol_rel(1e-4); - // opt.set_maxtime(1e-2); - - /* ---------- init variables ---------- */ - vector q(variable_num_); - double final_cost; - for (int i = 0; i < int(control_points_.rows()); ++i) - { - if (i < start_id_) - continue; - - if (end_constrain_ == HARD_CONSTRAINT && i >= end_id_) - { - continue; - // std::cout << "jump" << std::endl; - } - - for (int j = 0; j < 3; j++) - q[3 * (i - start_id_) + j] = control_points_(i, j); - } - - if (end_constrain_ == SOFT_CONSTRAINT) - { - end_pt_ = (1 / 6.0) * - (control_points_.row(control_points_.rows() - 3) + 4 * control_points_.row(control_points_.rows() - 2) + - control_points_.row(control_points_.rows() - 1)); - // std::cout << "end pt" << std::endl; - } - - try - { - /* ---------- optimization ---------- */ - cout << "[Optimization]: begin-------------" << endl; - cout << fixed << setprecision(7); - vec_time_.clear(); - vec_cost_.clear(); - time_start_ = ros::Time::now(); - - nlopt::result result = opt.optimize(q, final_cost); - - /* ---------- get results ---------- */ - std::cout << "[Optimization]: iter num: " << iter_num_ << std::endl; - // cout << "Min cost:" << min_cost_ << endl; - - for (int i = 0; i < control_points_.rows(); ++i) - { - if (i < start_id_) - continue; - - if (end_constrain_ == HARD_CONSTRAINT && i >= end_id_) - continue; - - for (int j = 0; j < 3; j++) - control_points_(i, j) = best_variable_[3 * (i - start_id_) + j]; - } - - cout << "[Optimization]: end-------------" << endl; - } - catch (std::exception& e) - { - cout << "[Optimization]: nlopt exception: " << e.what() << endl; - } -} - -void BsplineOptimizer::calcSmoothnessCost(const vector& q, double& cost, - vector& gradient) -{ - cost = 0.0; - std::fill(gradient.begin(), gradient.end(), Eigen::Vector3d(0, 0, 0)); - - Eigen::Vector3d jerk; - - for (int i = 0; i < q.size() - order_; i++) - { - /* evaluate jerk */ - jerk = q[i + 3] - 3 * q[i + 2] + 3 * q[i + 1] - q[i]; - cost += jerk.squaredNorm(); - - /* jerk gradient */ - gradient[i + 0] += 2.0 * jerk * (-1.0); - gradient[i + 1] += 2.0 * jerk * (3.0); - gradient[i + 2] += 2.0 * jerk * (-3.0); - gradient[i + 3] += 2.0 * jerk * (1.0); - } -} - -void BsplineOptimizer::calcDistanceCost(const vector& q, double& cost, - vector& gradient) -{ - cost = 0.0; - std::fill(gradient.begin(), gradient.end(), Eigen::Vector3d(0, 0, 0)); - - double dist; - Eigen::Vector3d dist_grad, g_zero(0, 0, 0); - - int end_idx = end_constrain_ == SOFT_CONSTRAINT ? q.size() : q.size() - order_; - - for (int i = order_; i < end_idx; i++) - { - if (!dynamic_) - { - edt_env_->evaluateEDTWithGrad(q[i], -1.0, dist, dist_grad); - } - else - { - double time = double(i + 2 - order_) * bspline_interval_ + time_traj_start_; - edt_env_->evaluateEDTWithGrad(q[i], time, dist, dist_grad); - } - - cost += dist < dist0_ ? pow(dist - dist0_, 2) : 0.0; - gradient[i] += dist < dist0_ ? 2.0 * (dist - dist0_) * dist_grad : g_zero; - } -} - -void BsplineOptimizer::calcFeasibilityCost(const vector& q, double& cost, - vector& gradient) -{ - cost = 0.0; - std::fill(gradient.begin(), gradient.end(), Eigen::Vector3d(0, 0, 0)); - - /* ---------- abbreviation ---------- */ - double ts, vm2, am2, ts_inv2, ts_inv4; - vm2 = max_vel_ * max_vel_; - am2 = max_acc_ * max_acc_; - - ts = bspline_interval_; - ts_inv2 = 1 / ts / ts; - ts_inv4 = ts_inv2 * ts_inv2; - - /* ---------- velocity feasibility ---------- */ - for (int i = 0; i < q.size() - 1; i++) - { - Eigen::Vector3d vi = q[i + 1] - q[i]; - for (int j = 0; j < 3; j++) - { - double vd = vi(j) * vi(j) * ts_inv2 - vm2; - cost += vd > 0.0 ? pow(vd, 2) : 0.0; - - gradient[i + 0](j) += vd > 0.0 ? 2.0 * vd * ts_inv2 * (-2.0) * vi(j) : 0.0; - gradient[i + 1](j) += vd > 0.0 ? 2.0 * vd * ts_inv2 * (2.0) * vi(j) : 0.0; - } - } - - /* ---------- acceleration feasibility ---------- */ - for (int i = 0; i < q.size() - 2; i++) - { - Eigen::Vector3d ai = q[i + 2] - 2 * q[i + 1] + q[i]; - for (int j = 0; j < 3; j++) - { - double ad = ai(j) * ai(j) * ts_inv4 - am2; - cost += ad > 0.0 ? pow(ad, 2) : 0.0; - - gradient[i + 0](j) += ad > 0.0 ? 2.0 * ad * ts_inv4 * (2.0) * ai(j) : 0.0; - gradient[i + 1](j) += ad > 0.0 ? 2.0 * ad * ts_inv4 * (-4.0) * ai(j) : 0.0; - gradient[i + 2](j) += ad > 0.0 ? 2.0 * ad * ts_inv4 * (2.0) * ai(j) : 0.0; - } - } -} - -void BsplineOptimizer::calcEndpointCost(const vector& q, double& cost, - vector& gradient) -{ - cost = 0.0; - std::fill(gradient.begin(), gradient.end(), Eigen::Vector3d(0, 0, 0)); - - // zero cost and gradient in hard constraints - if (end_constrain_ == SOFT_CONSTRAINT) - { - Eigen::Vector3d q_3, q_2, q_1, qd; - q_3 = q[q.size() - 3]; - q_2 = q[q.size() - 2]; - q_1 = q[q.size() - 1]; - - qd = 1 / 6.0 * (q_3 + 4 * q_2 + q_1) - end_pt_; - cost += qd.squaredNorm(); - - gradient[q.size() - 3] += 2 * qd * (1 / 6.0); - gradient[q.size() - 2] += 2 * qd * (4 / 6.0); - gradient[q.size() - 1] += 2 * qd * (1 / 6.0); - } -} - -void BsplineOptimizer::combineCost(const std::vector& x, std::vector& grad, double& f_combine) -{ - /* ---------- convert to control point vector ---------- */ - vector q; - // q.resize(control_points_.rows()); - - /* first p points */ - for (int i = 0; i < order_; i++) - q.push_back(control_points_.row(i)); - - /* optimized control points */ - for (int i = 0; i < variable_num_ / 3; i++) - { - Eigen::Vector3d qi(x[3 * i], x[3 * i + 1], x[3 * i + 2]); - q.push_back(qi); - } - - /* last p points */ - if (end_constrain_ == END_CONSTRAINT::HARD_CONSTRAINT) - { - for (int i = 0; i < order_; i++) - q.push_back(control_points_.row(control_points_.rows() - order_ + i)); - } - - /* ---------- evaluate cost and gradient ---------- */ - double f_smoothness, f_distance, f_feasibility, f_endpoint; - - vector g_smoothness, g_distance, g_feasibility, g_endpoint; - g_smoothness.resize(control_points_.rows()); - g_distance.resize(control_points_.rows()); - g_feasibility.resize(control_points_.rows()); - g_endpoint.resize(control_points_.rows()); - - calcSmoothnessCost(q, f_smoothness, g_smoothness); - calcDistanceCost(q, f_distance, g_distance); - calcFeasibilityCost(q, f_feasibility, g_feasibility); - calcEndpointCost(q, f_endpoint, g_endpoint); - - /* ---------- convert to NLopt format...---------- */ - grad.resize(variable_num_); - - f_combine = lamda1_ * f_smoothness + lamda2_ * f_distance + lamda3_ * f_feasibility + lamda4_ * f_endpoint; - - for (int i = 0; i < variable_num_ / 3; i++) - for (int j = 0; j < 3; j++) - { - /* the first p points is static here */ - grad[3 * i + j] = lamda1_ * g_smoothness[i + order_](j) + lamda2_ * g_distance[i + order_](j) + - lamda3_ * g_feasibility[i + order_](j) + lamda4_ * g_endpoint[i + order_](j); - } - - /* ---------- print cost ---------- */ - iter_num_ += 1; - - if (iter_num_ % 100 == 0) - { - // cout << iter_num_ << " smooth: " << lamda1_ * f_smoothness << " , dist: " << lamda2_ * f_distance - // << ", fea: " << lamda3_ * f_feasibility << ", end: " << lamda4_ * f_endpoint << ", total: " << f_combine - // << endl; - } -} - -double BsplineOptimizer::costFunction(const std::vector& x, std::vector& grad, void* func_data) -{ - BsplineOptimizer* opt = reinterpret_cast(func_data); - - double cost; - opt->combineCost(x, grad, cost); - - /* save the min cost result */ - if (cost < opt->min_cost_) - { - opt->min_cost_ = cost; - opt->best_variable_ = x; - } - - return cost; - - // /* ---------- evaluation ---------- */ - - // ros::Time te1 = ros::Time::now(); - // double time_now = (te1 - opt->time_start_).toSec(); - // opt->vec_time_.push_back(time_now); - // if (opt->vec_cost_.size() == 0) - // { - // opt->vec_cost_.push_back(f_combine); - // } - // else if (opt->vec_cost_.back() > f_combine) - // { - // opt->vec_cost_.push_back(f_combine); - // } - // else - // { - // opt->vec_cost_.push_back(opt->vec_cost_.back()); - // } -} - -} // namespace dyn_planner \ No newline at end of file diff --git a/dyn_planner/bspline_opt/src/non_uniform_bspline.cpp b/dyn_planner/bspline_opt/src/non_uniform_bspline.cpp deleted file mode 100644 index 11ae1f529..000000000 --- a/dyn_planner/bspline_opt/src/non_uniform_bspline.cpp +++ /dev/null @@ -1,714 +0,0 @@ -#include "bspline_opt/non_uniform_bspline.h" -#include - -namespace dyn_planner -{ -double NonUniformBspline::limit_vel_; -double NonUniformBspline::limit_acc_; -double NonUniformBspline::limit_ratio_; - -// control points is a (n+1)x3 matrix -NonUniformBspline::NonUniformBspline(Eigen::MatrixXd points, int order, double interval_, bool zero) -{ - this->p_ = order; - - control_points_ = points; - this->n_ = points.rows() - 1; - - this->m_ = this->n_ + this->p_ + 1; - - // calculate knots vector - this->interval_ = interval_; - this->u_ = Eigen::VectorXd::Zero(this->m_ + 1); - for (int i = 0; i <= this->m_; ++i) - { - if (i <= this->p_) - this->u_(i) = double(-this->p_ + i) * this->interval_; - - else if (i > this->p_ && i <= this->m_ - this->p_) - { - this->u_(i) = this->u_(i - 1) + this->interval_; - } - else if (i > this->m_ - this->p_) - { - this->u_(i) = this->u_(i - 1) + this->interval_; - } - } - - // show the result - // cout << "p_: " << p_ << " n: " << n << " m: " << m << endl; - // cout << "control pts:\n" << control_points_ << "\nknots:\n" << - // this->u_.transpose() << endl; cout << "M3:\n" << M[0] << "\nM4:\n" << M[1] - // << "\nM5:\n" << M[2] << endl; - - if (zero) - { - x0_ = (1 / 6.0) * (control_points_.row(0) + 4 * control_points_.row(1) + control_points_.row(2)); - v0_ = (1 / 2.0 / interval_) * (control_points_.row(2) - control_points_.row(0)); - a0_ = (1 / interval_ / interval_) * (control_points_.row(0) - 2 * control_points_.row(1) + control_points_.row(2)); - - // cout << "initial state: " << x0_.transpose() << "\n" - // << v0_.transpose() << "\n" - // << a0_.transpose() << endl; - } -} - -NonUniformBspline::~NonUniformBspline() -{ -} - -void NonUniformBspline::setKnot(Eigen::VectorXd knot) -{ - this->u_ = knot; -} - -Eigen::VectorXd NonUniformBspline::getKnot() -{ - return this->u_; -} - -void NonUniformBspline::getTimeSpan(double& um, double& um_p) -{ - um = this->u_(this->p_); - um_p = this->u_(this->m_ - this->p_); -} - -Eigen::Vector3d NonUniformBspline::evaluateDeBoor(double t) -{ - if (t < this->u_(this->p_) || t > this->u_(this->m_ - this->p_)) - { - cout << "Out of trajectory range." << endl; - // return Eigen::Vector3d::Zero(3); - if (t < this->u_(this->p_)) - t = this->u_(this->p_); - if (t > this->u_(this->m_ - this->p_)) - t = this->u_(this->m_ - this->p_); - } - - // determine which [ui,ui+1] lay in - int k = this->p_; - while (true) - { - if (this->u_(k + 1) >= t) - break; - ++k; - } - - /* deBoor's alg */ - vector d; - for (int i = 0; i <= p_; ++i) - { - d.push_back(control_points_.row(k - p_ + i)); - // cout << d[i].transpose() << endl; - } - - for (int r = 1; r <= p_; ++r) - { - for (int i = p_; i >= r; --i) - { - double alpha = (t - u_[i + k - p_]) / (u_[i + 1 + k - r] - u_[i + k - p_]); - // cout << "alpha: " << alpha << endl; - d[i] = (1 - alpha) * d[i - 1] + alpha * d[i]; - } - } - - return d[p_]; -} - -Eigen::MatrixXd NonUniformBspline::getDerivativeControlPoints() -{ - // The derivative of a b-spline is also a b-spline, its order become p_-1 - // control point Qi = p_*(Pi+1-Pi)/(ui+p_+1-ui+1) - Eigen::MatrixXd ctp = Eigen::MatrixXd::Zero(control_points_.rows() - 1, 3); - for (int i = 0; i < ctp.rows(); ++i) - { - ctp.row(i) = p_ * (control_points_.row(i + 1) - control_points_.row(i)) / (u_(i + p_ + 1) - u_(i + 1)); - } - return ctp; -} - -NonUniformBspline NonUniformBspline::getDerivative() -{ - Eigen::MatrixXd ctp = this->getDerivativeControlPoints(); - NonUniformBspline derivative = NonUniformBspline(ctp, p_ - 1, this->interval_, false); - - /* cut the first and last knot */ - Eigen::VectorXd knot(this->u_.rows() - 2); - knot = this->u_.segment(1, this->u_.rows() - 2); - derivative.setKnot(knot); - - return derivative; -} - -bool NonUniformBspline::checkFeasibility(bool show) -{ - bool fea = true; - // SETY << "[Bspline]: total points size: " << control_points_.rows() << endl; - - Eigen::MatrixXd P = control_points_; - - /* check vel feasibility and insert points */ - double max_vel = -1.0; - for (int i = 0; i < P.rows() - 1; ++i) - { - Eigen::Vector3d vel = p_ * (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1)); - if (fabs(vel(0)) > limit_vel_ + 1e-4 || fabs(vel(1)) > limit_vel_ + 1e-4 || fabs(vel(2)) > limit_vel_ + 1e-4) - { - /* insert mid point */ - if (show) - cout << "[Check]: Infeasible vel " << i << " :" << vel.transpose() << endl; - fea = false; - max_vel = max(max_vel, fabs(vel(0))); - max_vel = max(max_vel, fabs(vel(1))); - max_vel = max(max_vel, fabs(vel(2))); - } - } - - /* acc feasibility */ - double max_acc = -1.0; - for (int i = 0; i < P.rows() - 2; ++i) - { - Eigen::Vector3d acc = p_ * (p_ - 1) * - ((P.row(i + 2) - P.row(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) - - (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1))) / - (u_(i + p_ + 1) - u_(i + 2)); - - if (fabs(acc(0)) > limit_acc_ + 1e-4 || fabs(acc(1)) > limit_acc_ + 1e-4 || fabs(acc(2)) > limit_acc_ + 1e-4) - { - /* insert mid point */ - if (show) - cout << "[Check]: Infeasible acc " << i << " :" << acc.transpose() << endl; - fea = false; - max_acc = max(max_acc, fabs(acc(0))); - max_acc = max(max_acc, fabs(acc(1))); - max_acc = max(max_acc, fabs(acc(2))); - - double ma = max(fabs(acc(0)), fabs(acc(1))); - ma = max(ma, fabs(acc(2))); - - double ratio = sqrt(ma / limit_acc_) + 1e-4; - // cout << "ratio: " << ratio << endl; - } - } - - // cout << "max vel:" << max_vel << ", max acc:" << max_acc << endl; - double ratio = max(max_vel / limit_vel_, sqrt(fabs(max_acc) / limit_acc_)); - // cout << "check ratio:" << ratio << endl; - - return fea; -} - -bool NonUniformBspline::reallocateTime(bool show) -{ - // SETY << "[Bspline]: total points size: " << control_points_.rows() << endl; - // cout << "origin knots:\n" << u_.transpose() << endl; - bool fea = true; - - // double tm, tmp; - // getTimeSpan(tm, tmp); - // double to = tmp - tm; - // cout << "origin duration: " << to << endl; - - Eigen::MatrixXd P = control_points_; - - /* check vel feasibility and insert points */ - double max_vel = -1.0; - for (int i = 0; i < P.rows() - 1; ++i) - { - Eigen::Vector3d vel = p_ * (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1)); - if (fabs(vel(0)) > limit_vel_ + 1e-4 || fabs(vel(1)) > limit_vel_ + 1e-4 || fabs(vel(2)) > limit_vel_ + 1e-4) - { - fea = false; - max_vel = -1.0; - max_vel = max(max_vel, fabs(vel(0))); - max_vel = max(max_vel, fabs(vel(1))); - max_vel = max(max_vel, fabs(vel(2))); - if (show) - cout << "[Realloc]: Infeasible vel " << i << " :" << vel.transpose() << endl; - - double ratio = max_vel / limit_vel_ + 1e-4; - if (ratio > limit_ratio_) - ratio = limit_ratio_; - - double time_ori = u_(i + p_ + 1) - u_(i + 1); - double time_new = ratio * time_ori; - double delta_t = time_new - time_ori; - double t_inc = delta_t / double(p_); - - for (int j = i + 2; j <= i + p_ + 1; ++j) - { - u_(j) += double(j - i - 1) * t_inc; - if (j <= 5 && j >= 1) - { - // cout << "vel j: " << j << endl; - } - } - - for (int j = i + p_ + 2; j < u_.rows(); ++j) - { - u_(j) += delta_t; - } - } - } - - /* acc feasibility */ - double max_acc = -1.0; - for (int i = 0; i < P.rows() - 2; ++i) - { - Eigen::Vector3d acc = p_ * (p_ - 1) * - ((P.row(i + 2) - P.row(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) - - (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1))) / - (u_(i + p_ + 1) - u_(i + 2)); - - if (fabs(acc(0)) > limit_acc_ + 1e-4 || fabs(acc(1)) > limit_acc_ + 1e-4 || fabs(acc(2)) > limit_acc_ + 1e-4) - { - fea = false; - /* insert mid point */ - max_acc = -1.0; - max_acc = max(max_acc, fabs(acc(0))); - max_acc = max(max_acc, fabs(acc(1))); - max_acc = max(max_acc, fabs(acc(2))); - if (show) - cout << "[Realloc]: Infeasible acc " << i << " :" << acc.transpose() << endl; - - double ratio = sqrt(max_acc / limit_acc_) + 1e-4; - if (ratio > limit_ratio_) - ratio = limit_ratio_; - // cout << "ratio: " << ratio << endl; - - double time_ori = u_(i + p_ + 1) - u_(i + 2); - double time_new = ratio * time_ori; - double delta_t = time_new - time_ori; - double t_inc = delta_t / double(p_ - 1); - - if (i == 1 || i == 2) - { - // cout << "acc i: " << i << endl; - for (int j = 2; j <= 5; ++j) - { - u_(j) += double(j - 1) * t_inc; - } - - for (int j = 6; j < u_.rows(); ++j) - { - u_(j) += 4.0 * t_inc; - } - } - else - { - for (int j = i + 3; j <= i + p_ + 1; ++j) - { - u_(j) += double(j - i - 2) * t_inc; - if (j <= 5 && j >= 1) - { - // cout << "acc j: " << j << endl; - } - } - - for (int j = i + p_ + 2; j < u_.rows(); ++j) - { - u_(j) += delta_t; - } - } - } - } - - // cout << "new knots:\n" << u_.transpose() << endl; - // getTimeSpan(tm, tmp); - // double tn = tmp - tm; - // cout << "new duration: " << tn << endl; - // SETY << "realloc ratio: " << tn / to << endl; - - return fea; -} - -bool NonUniformBspline::adjustTime(bool show) -{ - bool fea = true; - - // double tm, tmp; - // getTimeSpan(tm, tmp); - // double to = tmp - tm; - // cout << "origin duration: " << to << endl; - - Eigen::MatrixXd P = control_points_; - - /* check vel feasibility and insert points */ - double max_vel = -1.0; - for (int i = 3; i < P.rows() - 1 - 3; ++i) - { - Eigen::Vector3d vel = p_ * (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1)); - if (fabs(vel(0)) > limit_vel_ + 1e-4 || fabs(vel(1)) > limit_vel_ + 1e-4 || fabs(vel(2)) > limit_vel_ + 1e-4) - { - fea = false; - max_vel = -1.0; - max_vel = max(max_vel, fabs(vel(0))); - max_vel = max(max_vel, fabs(vel(1))); - max_vel = max(max_vel, fabs(vel(2))); - if (show) - cout << "[Realloc]: Infeasible vel " << i << " :" << vel.transpose() << endl; - - double ratio = max_vel / limit_vel_ + 1e-4; - if (ratio > limit_ratio_) - ratio = limit_ratio_; - - double time_ori = u_(i + p_ + 1) - u_(i + 1); - double time_new = ratio * time_ori; - double delta_t = time_new - time_ori; - double t_inc = delta_t / double(p_); - - for (int j = i + 2; j <= i + p_ + 1; ++j) - { - u_(j) += double(j - i - 1) * t_inc; - if (j <= 5 && j >= 1) - { - // cout << "vel j: " << j << endl; - } - } - - for (int j = i + p_ + 2; j < u_.rows(); ++j) - { - u_(j) += delta_t; - } - } - } - - /* acc feasibility */ - double max_acc = -1.0; - for (int i = 3; i < P.rows() - 2 - 3; ++i) - { - Eigen::Vector3d acc = p_ * (p_ - 1) * - ((P.row(i + 2) - P.row(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) - - (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1))) / - (u_(i + p_ + 1) - u_(i + 2)); - - if (fabs(acc(0)) > limit_acc_ + 1e-4 || fabs(acc(1)) > limit_acc_ + 1e-4 || fabs(acc(2)) > limit_acc_ + 1e-4) - { - fea = false; - /* insert mid point */ - max_acc = -1.0; - max_acc = max(max_acc, fabs(acc(0))); - max_acc = max(max_acc, fabs(acc(1))); - max_acc = max(max_acc, fabs(acc(2))); - if (show) - cout << "[Realloc]: Infeasible acc " << i << " :" << acc.transpose() << endl; - - double ratio = sqrt(max_acc / limit_acc_) + 1e-4; - if (ratio > limit_ratio_) - ratio = limit_ratio_; - // cout << "ratio: " << ratio << endl; - - double time_ori = u_(i + p_ + 1) - u_(i + 2); - double time_new = ratio * time_ori; - double delta_t = time_new - time_ori; - double t_inc = delta_t / double(p_ - 1); - - if (i == 1 || i == 2) - { - // cout << "acc i: " << i << endl; - for (int j = 2; j <= 5; ++j) - { - u_(j) += double(j - 1) * t_inc; - } - - for (int j = 6; j < u_.rows(); ++j) - { - u_(j) += 4.0 * t_inc; - } - } - else - { - for (int j = i + 3; j <= i + p_ + 1; ++j) - { - u_(j) += double(j - i - 2) * t_inc; - if (j <= 5 && j >= 1) - { - // cout << "acc j: " << j << endl; - } - } - - for (int j = i + p_ + 2; j < u_.rows(); ++j) - { - u_(j) += delta_t; - } - } - } - } - - // cout << "new knots:\n" << u_.transpose() << endl; - // getTimeSpan(tm, tmp); - // double tn = tmp - tm; - // cout << "new duration: " << tn << endl; - // SETY << "realloc ratio: " << tn / to << endl; - - return fea; -} - -void NonUniformBspline::recomputeInit() -{ - double t1 = u_(1), t2 = u_(2), t3 = u_(3), t4 = u_(4), t5 = u_(5); - - /* write the A matrix */ - Eigen::Matrix3d A; - - /* position */ - A(0, 0) = ((t2 - t5) * (t3 - t4) * (t3 - t4)) / ((t1 - t4) * (t2 - t4) * (t2 - t5)); - A(0, 1) = ((t2 - t5) * (t3 - t4) * (t1 - t3)) / ((t1 - t4) * (t2 - t4) * (t2 - t5)) + - ((t1 - t4) * (t2 - t3) * (t3 - t5)) / ((t1 - t4) * (t2 - t4) * (t2 - t5)); - A(0, 2) = ((t1 - t4) * (t2 - t3) * (t2 - t3)) / ((t1 - t4) * (t2 - t4) * (t2 - t5)); - - /* velocity */ - A(1, 0) = 3.0 * ((t2 - t5) * (t3 - t4)) / ((t1 - t4) * (t2 - t4) * (t2 - t5)); - A(1, 1) = 3.0 * ((t1 - t4) * (t2 - t3) - (t2 - t5) * (t3 - t4)) / ((t1 - t4) * (t2 - t4) * (t2 - t5)); - A(1, 2) = -3.0 * ((t1 - t4) * (t2 - t3)) / ((t1 - t4) * (t2 - t4) * (t2 - t5)); - - /* acceleration */ - A(2, 0) = 6.0 * (t2 - t5) / ((t1 - t4) * (t2 - t4) * (t2 - t5)); - A(2, 1) = -6.0 * ((t1 - t4) + (t2 - t5)) / ((t1 - t4) * (t2 - t4) * (t2 - t5)); - A(2, 2) = 6.0 * (t1 - t4) / ((t1 - t4) * (t2 - t4) * (t2 - t5)); - - /* write B = (bx, by, bz) */ - Eigen::Matrix3d B; - Eigen::Vector3d bx, by, bz; - B.row(0) = x0_; - B.row(1) = v0_; - B.row(2) = a0_; - // cout << "B:\n" << B << endl; - - bx = B.col(0); - by = B.col(1); - bz = B.col(2); - - /* solve */ - Eigen::Vector3d px = A.colPivHouseholderQr().solve(bx); - Eigen::Vector3d py = A.colPivHouseholderQr().solve(by); - Eigen::Vector3d pz = A.colPivHouseholderQr().solve(bz); - - Eigen::Matrix3d P; - P.col(0) = px; - P.col(1) = py; - P.col(2) = pz; - - control_points_.row(0) = P.row(0); - control_points_.row(1) = P.row(1); - control_points_.row(2) = P.row(2); - - B = A * P; - // cout << "B:\n" << B << endl; -} - -// input : -// sample : 3 x (K+2) (for 3 order) for x, y, z sample -// ts -// output: -// control_pts (K+6)x3 -void NonUniformBspline::getControlPointEqu3(Eigen::MatrixXd samples, double ts, Eigen::MatrixXd& control_pts) -{ - int K = samples.cols() - 4 - 1; - - // write A - Eigen::VectorXd prow(3), vrow(3), arow(3); - prow << 1, 4, 1; - vrow << -1, 0, 1; - arow << 1, -2, 1; - - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(K + 5, K + 4); - - for (int i = 0; i < K + 2; ++i) - A.block(i, i, 1, 3) = prow.transpose(); - - A.block(K + 2, 0, 1, 3) = A.block(K + 3, K + 1, 1, 3) = vrow.transpose(); - A.block(K + 4, 0, 1, 3) = arow.transpose(); - - // cout << "A:\n" << A << endl; - A.block(0, 0, K + 2, K + 4) = (1 / 6.0) * A.block(0, 0, K + 2, K + 4); - A.block(K + 2, 0, 2, K + 4) = (1 / 2.0 / ts) * A.block(K + 2, 0, 2, K + 4); - A.row(K + 4) = (1 / ts / ts) * A.row(K + 4); - - // write b - Eigen::VectorXd bx(K + 5), by(K + 5), bz(K + 5); - for (int i = 0; i < K + 5; ++i) - { - bx(i) = samples(0, i); - by(i) = samples(1, i); - bz(i) = samples(2, i); - } - - // solve Ax = b - Eigen::VectorXd px = A.colPivHouseholderQr().solve(bx); - Eigen::VectorXd py = A.colPivHouseholderQr().solve(by); - Eigen::VectorXd pz = A.colPivHouseholderQr().solve(bz); - - // convert to control pts - control_pts.resize(K + 4, 3); - control_pts.col(0) = px; - control_pts.col(1) = py; - control_pts.col(2) = pz; -} - -void NonUniformBspline::BsplineParameterize(const double& ts, const vector& point_set, - const vector& start_end_derivative, - Eigen::MatrixXd& ctrl_pts) -{ - if (ts <= 0) - { - cout << "[B-spline]:time step error." << endl; - return; - } - - if (point_set.size() <= 3) - { - cout << "[B-spline]:point set have only " << point_set.size() << " points." << endl; - return; - } - - if (start_end_derivative.size() != 4) - { - cout << "[B-spline]:derivatives error." << endl; - } - - int K = point_set.size() - 2; - - // write A - Eigen::VectorXd prow(3), vrow(3), arow(3); - prow << 1, 4, 1; - vrow << -1, 0, 1; - arow << 1, -2, 1; - - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(K + 6, K + 4); - - for (int i = 0; i < K + 2; ++i) - A.block(i, i, 1, 3) = prow.transpose(); - A.block(K + 2, 0, 1, 3) = A.block(K + 3, K + 1, 1, 3) = vrow.transpose(); - A.block(K + 4, 0, 1, 3) = A.block(K + 5, K + 1, 1, 3) = arow.transpose(); - - // cout << "A:\n" << A << endl; - A.block(0, 0, K + 2, K + 4) = (1 / 6.0) * A.block(0, 0, K + 2, K + 4); - A.block(K + 2, 0, 2, K + 4) = (1 / 2.0 / ts) * A.block(K + 2, 0, 2, K + 4); - A.row(K + 4) = (1 / ts / ts) * A.row(K + 4); - A.row(K + 5) = (1 / ts / ts) * A.row(K + 5); - - // write b - Eigen::VectorXd bx(K + 6), by(K + 6), bz(K + 6); - for (int i = 0; i < K + 2; ++i) - { - bx(i) = point_set[i](0), by(i) = point_set[i](1), bz(i) = point_set[i](2); - } - - for (int i = 0; i < 4; ++i) - { - bx(K + 2 + i) = start_end_derivative[i](0); - by(K + 2 + i) = start_end_derivative[i](1); - bz(K + 2 + i) = start_end_derivative[i](2); - } - - // solve Ax = b - Eigen::VectorXd px = A.colPivHouseholderQr().solve(bx); - Eigen::VectorXd py = A.colPivHouseholderQr().solve(by); - Eigen::VectorXd pz = A.colPivHouseholderQr().solve(bz); - - // convert to control pts - ctrl_pts.resize(K + 4, 3); - ctrl_pts.col(0) = px; - ctrl_pts.col(1) = py; - ctrl_pts.col(2) = pz; - - cout << "[B-spline]: parameterization ok." << endl; -} - -double NonUniformBspline::getTimeSum() -{ - double tm, tmp; - getTimeSpan(tm, tmp); - return tmp - tm; -} - -double NonUniformBspline::getLength() -{ - double length = 0.0; - - double tm, tmp; - getTimeSpan(tm, tmp); - Eigen::Vector3d p_l = evaluateDeBoor(tm), p_n; - for (double t = tm + 0.01; t <= tmp; t += 0.01) - { - p_n = evaluateDeBoor(t); - length += (p_n - p_l).norm(); - p_n = p_l; - } - - return length; -} - -double NonUniformBspline::getJerk() -{ - NonUniformBspline jerk_traj = getDerivative().getDerivative().getDerivative(); - Eigen::VectorXd times = jerk_traj.getKnot(); - Eigen::MatrixXd ctrl_pts = jerk_traj.getControlPoint(); - - cout << "num knot:" << times.rows() << endl; - cout << "num ctrl pts:" << ctrl_pts.rows() << endl; - - double jerk = 0.0; - for (int i = 0; i < ctrl_pts.rows(); ++i) - { - jerk += (times(i + 1) - times(i)) * ctrl_pts(i, 0) * ctrl_pts(i, 0); - jerk += (times(i + 1) - times(i)) * ctrl_pts(i, 1) * ctrl_pts(i, 1); - jerk += (times(i + 1) - times(i)) * ctrl_pts(i, 2) * ctrl_pts(i, 2); - } - - return jerk; -} - -void NonUniformBspline::getMeanAndMaxVel(double& mean_v, double& max_v) -{ - NonUniformBspline vel = getDerivative(); - double tm, tmp; - vel.getTimeSpan(tm, tmp); - - double max_vel = -1.0, mean_vel = 0.0; - int num = 0; - for (double t = tm; t <= tmp; t += 0.01) - { - Eigen::Vector3d v3d = vel.evaluateDeBoor(t); - double vn = v3d.norm(); - - mean_vel += vn; - ++num; - if (vn > max_vel) - { - max_vel = vn; - } - } - - mean_vel = mean_vel / double(num); - mean_v = mean_vel; - max_v = max_vel; -} - -void NonUniformBspline::getMeanAndMaxAcc(double& mean_a, double& max_a) -{ - NonUniformBspline acc = getDerivative().getDerivative(); - double tm, tmp; - acc.getTimeSpan(tm, tmp); - - double max_acc = -1.0, mean_acc = 0.0; - int num = 0; - for (double t = tm; t <= tmp; t += 0.01) - { - Eigen::Vector3d a3d = acc.evaluateDeBoor(t); - double an = a3d.norm(); - - mean_acc += an; - ++num; - if (an > max_acc) - { - max_acc = an; - } - } - - mean_acc = mean_acc / double(num); - mean_a = mean_acc; - max_a = max_acc; -} -} // namespace dyn_planner diff --git a/dyn_planner/log.md b/dyn_planner/log.md deleted file mode 100644 index 2c24e5003..000000000 --- a/dyn_planner/log.md +++ /dev/null @@ -1,31 +0,0 @@ - -# Environment Rrepresentation - - - - - - -- bspline optimization using. Env - -- dealing with uncertainty. -wenchao paper - -- refactor pcd path finder - -- pnp - - -ld*(-4*p2**2*t1 + 4*p2**2*t2 - 12*p2*p3*t1**2 + 12*p2*p3*t2**2 - 80*p4*p5*t1**6 + 80*p4*p5*t2**6 - 400*p5**2*t1**7/7 + 400*p5**2*t2**7/7 - t1**5*(48*p3*p5 + 144*p4**2/5) - t1**4*(20*p2*p5 + 36*p3*p4) - t1**3*(16*p2*p4 + 12*p3**2) + t2**5*(48*p3*p5 + 144*p4**2/5) + t2**4*(20*p2*p5 + 36*p3*p4) + t2**3*(16*p2*p4 + 12*p3**2)) + p0**2 + 2*p0*p1*ti + 2*p0*p2*ti**2 + 2*p0*p3*ti**3 + 2*p0*p4*ti**4 + 2*p0*p5*ti**5 - 2*p0*qi + p1**2*ti**2 + 2*p1*p2*ti**3 + 2*p1*p3*ti**4 + 2*p1*p4*ti**5 + 2*p1*p5*ti**6 - 2*p1*qi*ti + p2**2*ti**4 + 2*p2*p3*ti**5 + 2*p2*p4*ti**6 + 2*p2*p5*ti**7 - 2*p2*qi*ti**2 + p3**2*ti**6 + 2*p3*p4*ti**7 + 2*p3*p5*ti**8 - 2*p3*qi*ti**3 + p4**2*ti**8 + 2*p4*p5*ti**9 - 2*p4*qi*ti**4 + p5**2*ti**10 - 2*p5*qi*ti**5 + qi**2 - -2*p0 + 2*p1*ti + 2*p2*ti**2 + 2*p3*ti**3 + 2*p4*ti**4 + 2*p5*ti**5 - 2*qi - -2*ti*(p0 + p1*ti + p2*ti**2 + p3*ti**3 + p4*ti**4 + p5*ti**5 - qi) - --4*ld*(2*p2*t1 - 2*p2*t2 + 3*p3*t1**2 - 3*p3*t2**2 + 4*p4*t1**3 - 4*p4*t2**3 + 5*p5*t1**4 - 5*p5*t2**4) + 2*p0*ti**2 + 2*p1*ti**3 + 2*p2*ti**4 + 2*p3*ti**5 + 2*p4*ti**6 + 2*p5*ti**7 - 2*qi*ti**2 - --12*ld*(p2*t1**2 - p2*t2**2 + 2*p3*t1**3 - 2*p3*t2**3 + 3*p4*t1**4 - 3*p4*t2**4 + 4*p5*t1**5 - 4*p5*t2**5) + 2*p0*ti**3 + 2*p1*ti**4 + 2*p2*ti**5 + 2*p3*ti**6 + 2*p4*ti**7 + 2*p5*ti**8 - 2*qi*ti**3 - --4*ld*(20*p2*t1**3 - 20*p2*t2**3 + 45*p3*t1**4 - 45*p3*t2**4 + 72*p4*t1**5 - 72*p4*t2**5 + 100*p5*t1**6 - 100*p5*t2**6)/5 + 2*p0*ti**4 + 2*p1*ti**5 + 2*p2*ti**6 + 2*p3*ti**7 + 2*p4*ti**8 + 2*p5*ti**9 - 2*qi*ti**4 - --4*ld*(35*p2*t1**4 - 35*p2*t2**4 + 84*p3*t1**5 - 84*p3*t2**5 + 140*p4*t1**6 - 140*p4*t2**6 + 200*p5*t1**7 - 200*p5*t2**7)/7 + 2*p0*ti**5 + 2*p1*ti**6 + 2*p2*ti**7 + 2*p3*ti**8 + 2*p4*ti**9 + 2*p5*ti**10 - 2*qi*ti**5 - diff --git a/dyn_planner/path_searching/CMakeLists.txt b/dyn_planner/path_searching/CMakeLists.txt deleted file mode 100644 index ce28612aa..000000000 --- a/dyn_planner/path_searching/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(path_searching) - -find_package(Eigen3 REQUIRED) -find_package(PCL 1.7 REQUIRED) - -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - visualization_msgs - plan_env -) - - -catkin_package( - INCLUDE_DIRS include - LIBRARIES path_searching - CATKIN_DEPENDS plan_env -# DEPENDS system_lib -) - -include_directories( - SYSTEM - include - ${catkin_INCLUDE_DIRS} - ${Eigen3_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS} -O3 -Wall") - -add_library( path_searching - src/kinodynamic_astar.cpp - src/astar.cpp - ) -target_link_libraries( path_searching - ${catkin_LIBRARIES} - ) diff --git a/dyn_planner/path_searching/include/path_searching/astar.h b/dyn_planner/path_searching/include/path_searching/astar.h deleted file mode 100644 index 2aa894a33..000000000 --- a/dyn_planner/path_searching/include/path_searching/astar.h +++ /dev/null @@ -1,173 +0,0 @@ -#ifndef _ASTAR_H -#define _ASTAR_H - -#include -#include -#include -#include -#include -#include -#include -// #include "grad_spline/sdf_map.h" -#include "plan_env/edt_environment.h" -#include -#include -namespace dyn_planner -{ -// #define REACH_HORIZON 1 -// #define REACH_END 2 -// #define NO_PATH 3 -#define IN_CLOSE_SET 'a' -#define IN_OPEN_SET 'b' -#define NOT_EXPAND 'c' -#define inf 1 >> 30 - -class Node -{ -public: - /* -------------------- */ - Eigen::Vector3i index; - Eigen::Vector3d position; - double g_score, f_score; - Node* parent; - char node_state; - - double time; // dyn - int time_idx; - - /* -------------------- */ - Node() - { - parent = NULL; - node_state = NOT_EXPAND; - } - ~Node(){}; -}; -typedef Node* NodePtr; - -class NodeComparator0 -{ -public: - bool operator()(NodePtr node1, NodePtr node2) - { - return node1->f_score > node2->f_score; - } -}; - -template -struct matrix_hash0 : std::unary_function -{ - std::size_t operator()(T const& matrix) const - { - size_t seed = 0; - for (size_t i = 0; i < matrix.size(); ++i) - { - auto elem = *(matrix.data() + i); - seed ^= std::hash()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2); - } - return seed; - } -}; - -class NodeHashTable0 -{ -private: - /* data */ - std::unordered_map> data_3d_; - std::unordered_map> data_4d_; - -public: - NodeHashTable0(/* args */) - { - } - ~NodeHashTable0() - { - } - void insert(Eigen::Vector3i idx, NodePtr node) - { - data_3d_.insert(make_pair(idx, node)); - } - void insert(Eigen::Vector3i idx, int time_idx, NodePtr node) - { - data_4d_.insert(make_pair(Eigen::Vector4i(idx(0), idx(1), idx(2), time_idx), node)); - } - - NodePtr find(Eigen::Vector3i idx) - { - auto iter = data_3d_.find(idx); - return iter == data_3d_.end() ? NULL : iter->second; - } - NodePtr find(Eigen::Vector3i idx, int time_idx) - { - auto iter = data_4d_.find(Eigen::Vector4i(idx(0), idx(1), idx(2), time_idx)); - return iter == data_4d_.end() ? NULL : iter->second; - } - - void clear() - { - data_3d_.clear(); - data_4d_.clear(); - } -}; - -class Astar -{ -private: - /* ---------- main data structure ---------- */ - vector path_node_pool_; - int use_node_num_, iter_num_; - NodeHashTable0 expanded_nodes_; - std::priority_queue, NodeComparator0> open_set_; - std::vector path_nodes_; - - /* ---------- record data ---------- */ - EDTEnvironment::Ptr edt_env_; - bool has_path_ = false; - - /* ---------- parameter ---------- */ - /* search */ - double lambda_heu_; - double margin_; - int allocate_num_; - double tie_breaker_; - /* map */ - double resolution_, inv_resolution_, time_resolution_, inv_time_resolution_; - Eigen::Vector3d origin_, map_size_3d_; - double time_origin_; - - /* helper */ - Eigen::Vector3i posToIndex(Eigen::Vector3d pt); - int timeToIndex(double time); - void retrievePath(NodePtr end_node); - - /* heuristic function */ - double getDiagHeu(Eigen::Vector3d x1, Eigen::Vector3d x2); - double getManhHeu(Eigen::Vector3d x1, Eigen::Vector3d x2); - double getEuclHeu(Eigen::Vector3d x1, Eigen::Vector3d x2); - -public: - Astar(){}; - ~Astar(); - - enum - { - REACH_END = 1, - NO_PATH = 2 - }; - - /* main API */ - void setParam(ros::NodeHandle& nh); - void init(); - void reset(); - int search(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt, bool dynamic = false, double time_start = -1.0); - - void setEnvironment(const EDTEnvironment::Ptr& env); - std::vector getPath(); - std::vector getVisitedNodes(); - - typedef shared_ptr Ptr; -}; - -} // namespace dyn_planner - -#endif \ No newline at end of file diff --git a/dyn_planner/path_searching/include/path_searching/kinodynamic_astar.h b/dyn_planner/path_searching/include/path_searching/kinodynamic_astar.h deleted file mode 100644 index c61077485..000000000 --- a/dyn_planner/path_searching/include/path_searching/kinodynamic_astar.h +++ /dev/null @@ -1,186 +0,0 @@ -#ifndef _KINODYNAMIC_ASTAR_H -#define _KINODYNAMIC_ASTAR_H - -#include -#include -#include -#include -#include -#include -#include -// #include "grad_spline/sdf_map.h" -#include "plan_env/edt_environment.h" -#include -#include -namespace dyn_planner -{ -// #define REACH_HORIZON 1 -// #define REACH_END 2 -// #define NO_PATH 3 -#define IN_CLOSE_SET 'a' -#define IN_OPEN_SET 'b' -#define NOT_EXPAND 'c' -#define inf 1 >> 30 - -class PathNode -{ -public: - /* -------------------- */ - Eigen::Vector3i index; - Eigen::Matrix state; - double g_score, f_score; - Eigen::Vector3d input; - double duration; - double time; // dyn - int time_idx; - PathNode* parent; - char node_state; - - /* -------------------- */ - PathNode() - { - parent = NULL; - node_state = NOT_EXPAND; - } - ~PathNode(){}; -}; -typedef PathNode* PathNodePtr; - -class NodeComparator -{ -public: - bool operator()(PathNodePtr node1, PathNodePtr node2) { return node1->f_score > node2->f_score; } -}; - -template -struct matrix_hash : std::unary_function -{ - std::size_t operator()(T const& matrix) const - { - size_t seed = 0; - for (size_t i = 0; i < matrix.size(); ++i) - { - auto elem = *(matrix.data() + i); - seed ^= std::hash()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2); - } - return seed; - } -}; - -class NodeHashTable -{ -private: - /* data */ - std::unordered_map> data_3d_; - std::unordered_map> data_4d_; - -public: - NodeHashTable(/* args */) {} - ~NodeHashTable() {} - void insert(Eigen::Vector3i idx, PathNodePtr node) { data_3d_.insert(make_pair(idx, node)); } - void insert(Eigen::Vector3i idx, int time_idx, PathNodePtr node) - { - data_4d_.insert(make_pair(Eigen::Vector4i(idx(0), idx(1), idx(2), time_idx), node)); - } - - PathNodePtr find(Eigen::Vector3i idx) - { - auto iter = data_3d_.find(idx); - return iter == data_3d_.end() ? NULL : iter->second; - } - PathNodePtr find(Eigen::Vector3i idx, int time_idx) - { - auto iter = data_4d_.find(Eigen::Vector4i(idx(0), idx(1), idx(2), time_idx)); - return iter == data_4d_.end() ? NULL : iter->second; - } - - void clear() - { - data_3d_.clear(); - data_4d_.clear(); - } -}; - -class KinodynamicAstar -{ -private: - /* ---------- main data structure ---------- */ - vector path_node_pool_; - int use_node_num_, iter_num_; - NodeHashTable expanded_nodes_; - std::priority_queue, NodeComparator> open_set_; - std::vector path_nodes_; - - /* ---------- record data ---------- */ - Eigen::Vector3d start_vel_, end_vel_, start_acc_; - Eigen::Matrix phi_; // state transit matrix - // shared_ptr sdf_map; - EDTEnvironment::Ptr edt_env_; - bool is_shot_succ_ = false; - Eigen::MatrixXd coef_shot_; - double t_shot_; - bool has_path_ = false; - - /* ---------- parameter ---------- */ - /* search */ - double max_tau_ = 0.25; - double init_max_tau_ = 0.8; - double max_vel_ = 3.0; - double max_acc_ = 3.0; - double w_time_ = 10.0; - double horizon_; - double lambda_heu_; - double margin_; - int allocate_num_; - int check_num_; - double tie_breaker_ = 1.0 + 1.0 / 10000; - /* map */ - double resolution_, inv_resolution_, time_resolution_, inv_time_resolution_; - Eigen::Vector3d origin_, map_size_3d_; - double time_origin_; - - /* helper */ - Eigen::Vector3i posToIndex(Eigen::Vector3d pt); - int timeToIndex(double time); - void retrievePath(PathNodePtr end_node); - - /* shot trajectory */ - vector cubic(double a, double b, double c, double d); - vector quartic(double a, double b, double c, double d, double e); - bool computeShotTraj(Eigen::VectorXd state1, Eigen::VectorXd state2, double time_to_goal); - double estimateHeuristic(Eigen::VectorXd x1, Eigen::VectorXd x2, double& optimal_time); - - /* state propagation */ - void stateTransit(Eigen::Matrix& state0, Eigen::Matrix& state1, - Eigen::Vector3d um, double tau); - -public: - KinodynamicAstar(){}; - ~KinodynamicAstar(); - - enum - { - REACH_HORIZON = 1, - REACH_END = 2, - NO_PATH = 3 - }; - - /* main API */ - void setParam(ros::NodeHandle& nh); - void init(); - void reset(); - int search(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel, Eigen::Vector3d start_acc, - Eigen::Vector3d end_pt, Eigen::Vector3d end_vel, bool init, bool dynamic = false, - double time_start = -1.0); - - void setEnvironment(const EDTEnvironment::Ptr& env); - std::vector getKinoTraj(double delta_t); - Eigen::MatrixXd getSamples(double& ts, int& K); - std::vector getVisitedNodes(); - - typedef shared_ptr Ptr; -}; - -} // namespace dyn_planner - -#endif \ No newline at end of file diff --git a/dyn_planner/path_searching/package.xml b/dyn_planner/path_searching/package.xml deleted file mode 100644 index a1d8434a8..000000000 --- a/dyn_planner/path_searching/package.xml +++ /dev/null @@ -1,70 +0,0 @@ - - - path_searching - 0.0.0 - The path_searching package - - - - - bzhouai - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - plan_env - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - plan_env - - - - - - - - diff --git a/dyn_planner/path_searching/src/astar.cpp b/dyn_planner/path_searching/src/astar.cpp deleted file mode 100644 index 9160fda65..000000000 --- a/dyn_planner/path_searching/src/astar.cpp +++ /dev/null @@ -1,339 +0,0 @@ -#include -#include - -using namespace std; -using namespace Eigen; - -namespace dyn_planner -{ -Astar::~Astar() -{ - for (int i = 0; i < allocate_num_; i++) - { - delete path_node_pool_[i]; - } -} - -int Astar::search(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt, bool dynamic, double time_start) -{ - /* ---------- initialize ---------- */ - NodePtr cur_node = path_node_pool_[0]; - cur_node->parent = NULL; - cur_node->position = start_pt; - cur_node->index = posToIndex(start_pt); - cur_node->g_score = 0.0; - - Eigen::Vector3d end_state(6); - Eigen::Vector3i end_index; - double time_to_goal; - - end_index = posToIndex(end_pt); - cur_node->f_score = lambda_heu_ * getEuclHeu(cur_node->position, end_pt); - cur_node->node_state = IN_OPEN_SET; - - open_set_.push(cur_node); - use_node_num_ += 1; - - if (dynamic) - { - time_origin_ = time_start; - cur_node->time = time_start; - cur_node->time_idx = timeToIndex(time_start); - expanded_nodes_.insert(cur_node->index, cur_node->time_idx, cur_node); - // cout << "time start: " << time_start << endl; - } - else - expanded_nodes_.insert(cur_node->index, cur_node); - - NodePtr neighbor = NULL; - NodePtr terminate_node = NULL; - - /* ---------- search loop ---------- */ - while (!open_set_.empty()) - { - /* ---------- get lowest f_score node ---------- */ - cur_node = open_set_.top(); - // cout << "pos: " << cur_node->state.head(3).transpose() << endl; - // cout << "time: " << cur_node->time << endl; - // cout << "dist: " << edt_env_->evaluateCoarseEDT(cur_node->state.head(3), cur_node->time) << endl; - - /* ---------- determine termination ---------- */ - - bool reach_end = abs(cur_node->index(0) - end_index(0)) <= 1 && abs(cur_node->index(1) - end_index(1)) <= 1 && - abs(cur_node->index(2) - end_index(2)) <= 1; - - if (reach_end) - { - // cout << "[Astar]:---------------------- " << use_node_num_ << endl; - // cout << "use node num: " << use_node_num_ << endl; - // cout << "iter num: " << iter_num_ << endl; - terminate_node = cur_node; - retrievePath(terminate_node); - has_path_ = true; - - return REACH_END; - } - - /* ---------- pop node and add to close set ---------- */ - open_set_.pop(); - cur_node->node_state = IN_CLOSE_SET; - iter_num_ += 1; - - /* ---------- init neighbor expansion ---------- */ - - Eigen::Vector3d cur_pos = cur_node->position; - Eigen::Vector3d pro_pos; - double pro_t; - - vector inputs; - Eigen::Vector3d d_pos; - - /* ---------- expansion loop ---------- */ - for (double dx = -resolution_; dx <= resolution_ + 1e-3; dx += resolution_) - for (double dy = -resolution_; dy <= resolution_ + 1e-3; dy += resolution_) - for (double dz = -resolution_; dz <= resolution_ + 1e-3; dz += resolution_) - { - d_pos << dx, dy, dz; - - if (d_pos.norm() < 1e-3) - continue; - - pro_pos = cur_pos + d_pos; - - /* ---------- check if in feasible space ---------- */ - /* inside map range */ - if (pro_pos(0) <= origin_(0) || pro_pos(0) >= map_size_3d_(0) || pro_pos(1) <= origin_(1) || - pro_pos(1) >= map_size_3d_(1) || pro_pos(2) <= origin_(2) || pro_pos(2) >= map_size_3d_(2)) - { - // cout << "outside map" << endl; - continue; - } - - /* not in close set */ - Eigen::Vector3i pro_id = posToIndex(pro_pos); - int pro_t_id = timeToIndex(pro_t); - - NodePtr pro_node = dynamic ? expanded_nodes_.find(pro_id, pro_t_id) : expanded_nodes_.find(pro_id); - - if (pro_node != NULL && pro_node->node_state == IN_CLOSE_SET) - { - // cout << "in closeset" << endl; - continue; - } - - /* collision free */ - // double dist = dynamic ? edt_env_->evaluateCoarseEDT(pro_pos, cur_node->time + dt) : - // edt_env_->evaluateCoarseEDT(pro_pos, -1.0); - double dist = edt_env_->evaluateCoarseEDT(pro_pos, -1.0); - if (dist <= margin_) - { - continue; - } - - /* ---------- compute cost ---------- */ - double time_to_goal, tmp_g_score, tmp_f_score; - tmp_g_score = d_pos.squaredNorm() + cur_node->g_score; - tmp_f_score = tmp_g_score + lambda_heu_ * getEuclHeu(pro_pos, end_pt); - - if (pro_node == NULL) - { - pro_node = path_node_pool_[use_node_num_]; - pro_node->index = pro_id; - pro_node->position = pro_pos; - pro_node->f_score = tmp_f_score; - pro_node->g_score = tmp_g_score; - pro_node->parent = cur_node; - pro_node->node_state = IN_OPEN_SET; - if (dynamic) - { - pro_node->time = cur_node->time + 1.0; - pro_node->time_idx = timeToIndex(pro_node->time); - } - open_set_.push(pro_node); - - if (dynamic) - expanded_nodes_.insert(pro_id, pro_node->time, pro_node); - else - expanded_nodes_.insert(pro_id, pro_node); - - use_node_num_ += 1; - if (use_node_num_ == allocate_num_) - { - cout << "run out of memory." << endl; - return NO_PATH; - } - } - else if (pro_node->node_state == IN_OPEN_SET) - { - if (tmp_g_score < pro_node->g_score) - { - // pro_node->index = pro_id; - pro_node->position = pro_pos; - pro_node->f_score = tmp_f_score; - pro_node->g_score = tmp_g_score; - pro_node->parent = cur_node; - if (dynamic) - pro_node->time = cur_node->time + 1.0; - } - } - else - { - cout << "error type in searching: " << pro_node->node_state << endl; - } - - /* ---------- ---------- */ - } - } - - /* ---------- open set empty, no path ---------- */ - cout << "open set empty, no path!" << endl; - cout << "use node num: " << use_node_num_ << endl; - cout << "iter num: " << iter_num_ << endl; - return NO_PATH; -} - -void Astar::setParam(ros::NodeHandle& nh) -{ - nh.param("astar/resolution_astar", resolution_, -1.0); - nh.param("astar/time_resolution", time_resolution_, -1.0); - nh.param("astar/lambda_heu", lambda_heu_, -1.0); - nh.param("astar/margin", margin_, -1.0); - nh.param("astar/allocate_num", allocate_num_, -1); - tie_breaker_ = 1.0 + 1.0 / 10000; - - cout << "margin:" << margin_ << endl; -} - -void Astar::retrievePath(NodePtr end_node) -{ - NodePtr cur_node = end_node; - path_nodes_.push_back(cur_node); - - while (cur_node->parent != NULL) - { - cur_node = cur_node->parent; - path_nodes_.push_back(cur_node); - } - - reverse(path_nodes_.begin(), path_nodes_.end()); -} - -std::vector Astar::getPath() -{ - vector path; - for (int i = 0; i < path_nodes_.size(); ++i) - { - path.push_back(path_nodes_[i]->position); - } - return path; -} - -double Astar::getDiagHeu(Eigen::Vector3d x1, Eigen::Vector3d x2) -{ - double dx = fabs(x1(0) - x2(0)); - double dy = fabs(x1(1) - x2(1)); - double dz = fabs(x1(2) - x2(2)); - - double h; - int diag = min(min(dx, dy), dz); - dx -= diag; - dy -= diag; - dz -= diag; - - if (dx < 1e-4) - { - h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dy, dz) + 1.0 * abs(dy - dz); - } - if (dy < 1e-4) - { - h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dz) + 1.0 * abs(dx - dz); - } - if (dz < 1e-4) - { - h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dy) + 1.0 * abs(dx - dy); - } - return tie_breaker_ * h; -} - -double Astar::getManhHeu(Eigen::Vector3d x1, Eigen::Vector3d x2) -{ - double dx = fabs(x1(0) - x2(0)); - double dy = fabs(x1(1) - x2(1)); - double dz = fabs(x1(2) - x2(2)); - - return tie_breaker_ * (dx + dy + dz); -} - -double Astar::getEuclHeu(Eigen::Vector3d x1, Eigen::Vector3d x2) -{ - return tie_breaker_ * (x2 - x1).norm(); -} - -void Astar::init() -{ - /* ---------- map params ---------- */ - this->inv_resolution_ = 1.0 / resolution_; - inv_time_resolution_ = 1.0 / time_resolution_; - edt_env_->getMapRegion(origin_, map_size_3d_); - - cout << "origin_: " << origin_.transpose() << endl; - cout << "map size: " << map_size_3d_.transpose() << endl; - - /* ---------- pre-allocated node ---------- */ - path_node_pool_.resize(allocate_num_); - for (int i = 0; i < allocate_num_; i++) - { - path_node_pool_[i] = new Node; - } - - use_node_num_ = 0; - iter_num_ = 0; -} - -void Astar::setEnvironment(const EDTEnvironment::Ptr& env) -{ - this->edt_env_ = env; -} - -void Astar::reset() -{ - expanded_nodes_.clear(); - path_nodes_.clear(); - - std::priority_queue, NodeComparator0> empty_queue; - open_set_.swap(empty_queue); - - for (int i = 0; i < use_node_num_; i++) - { - NodePtr node = path_node_pool_[i]; - node->parent = NULL; - node->node_state = NOT_EXPAND; - } - - use_node_num_ = 0; - iter_num_ = 0; -} - -std::vector Astar::getVisitedNodes() -{ - vector visited; - visited.assign(path_node_pool_.begin(), path_node_pool_.begin() + use_node_num_ - 1); - return visited; -} - -Eigen::Vector3i Astar::posToIndex(Eigen::Vector3d pt) -{ - Vector3i idx = ((pt - origin_) * inv_resolution_).array().floor().cast(); - - // idx << floor((pt(0) - origin_(0)) * inv_resolution_), floor((pt(1) - origin_(1)) * inv_resolution_), - // floor((pt(2) - origin_(2)) * inv_resolution_); - - return idx; -} - -int Astar::timeToIndex(double time) -{ - int idx = floor((time - time_origin_) * inv_time_resolution_); -} - -} // namespace dyn_planner diff --git a/dyn_planner/path_searching/src/kinodynamic_astar.cpp b/dyn_planner/path_searching/src/kinodynamic_astar.cpp deleted file mode 100644 index 6cb40ef62..000000000 --- a/dyn_planner/path_searching/src/kinodynamic_astar.cpp +++ /dev/null @@ -1,747 +0,0 @@ -#include -#include - -using namespace std; -using namespace Eigen; - -namespace dyn_planner -{ -KinodynamicAstar::~KinodynamicAstar() -{ - for (int i = 0; i < allocate_num_; i++) - { - delete path_node_pool_[i]; - } -} - -int KinodynamicAstar::search(Eigen::Vector3d start_pt, Eigen::Vector3d start_v, Eigen::Vector3d start_a, - Eigen::Vector3d end_pt, Eigen::Vector3d end_v, bool init, bool dynamic, double time_start) -{ - start_vel_ = start_v; - start_acc_ = start_a; - - /* ---------- initialize ---------- */ - PathNodePtr cur_node = path_node_pool_[0]; - cur_node->parent = NULL; - cur_node->state.head(3) = start_pt; - cur_node->state.tail(3) = start_v; - cur_node->index = posToIndex(start_pt); - cur_node->g_score = 0.0; - - Eigen::VectorXd end_state(6); - Eigen::Vector3i end_index; - double time_to_goal; - - end_state.head(3) = end_pt; - end_state.tail(3) = end_v; - end_index = posToIndex(end_pt); - cur_node->f_score = lambda_heu_ * estimateHeuristic(cur_node->state, end_state, time_to_goal); - cur_node->node_state = IN_OPEN_SET; - - open_set_.push(cur_node); - use_node_num_ += 1; - - if (dynamic) - { - time_origin_ = time_start; - cur_node->time = time_start; - cur_node->time_idx = timeToIndex(time_start); - expanded_nodes_.insert(cur_node->index, cur_node->time_idx, cur_node); - // cout << "time start: " << time_start << endl; - } - else - expanded_nodes_.insert(cur_node->index, cur_node); - - PathNodePtr neighbor = NULL; - PathNodePtr terminate_node = NULL; - bool init_search = init; - const int tolerance = ceil(1 / resolution_); - - /* ---------- search loop ---------- */ - while (!open_set_.empty()) - { - /* ---------- get lowest f_score node ---------- */ - cur_node = open_set_.top(); - // cout << "pos: " << cur_node->state.head(3).transpose() << endl; - // cout << "time: " << cur_node->time << endl; - // cout << "dist: " << edt_env_->evaluateCoarseEDT(cur_node->state.head(3), cur_node->time) << endl; - - /* ---------- determine termination ---------- */ - - bool near_end = abs(cur_node->index(0) - end_index(0)) <= tolerance && - abs(cur_node->index(1) - end_index(1)) <= tolerance && - abs(cur_node->index(2) - end_index(2)) <= tolerance; - bool reach_horizon = (cur_node->state.head(3) - start_pt).norm() >= horizon_; - - if (reach_horizon || near_end) - { - cout << "[Kino Astar]: used node num: " << use_node_num_ << ", iter num: " << iter_num_ << endl; - terminate_node = cur_node; - retrievePath(terminate_node); - has_path_ = true; - - if (near_end) - { - cout << "[Kino Astar]: near end." << endl; - - /* one shot trajectory */ - estimateHeuristic(cur_node->state, end_state, time_to_goal); - computeShotTraj(cur_node->state, end_state, time_to_goal); - - if (terminate_node->parent == NULL && !is_shot_succ_) - return NO_PATH; - else - return REACH_END; - } - else if (reach_horizon) - { - cout << "[Kino Astar]: Reach horizon_" << endl; - return REACH_HORIZON; - } - } - - /* ---------- pop node and add to close set ---------- */ - open_set_.pop(); - cur_node->node_state = IN_CLOSE_SET; - iter_num_ += 1; - - /* ---------- init state propagation ---------- */ - double res = 1 / 2.0, time_res = 1 / 1.0, time_res_init = 1 / 8.0; - - Eigen::Matrix cur_state = cur_node->state; - Eigen::Matrix pro_state; - vector tmp_expand_nodes; - Eigen::Vector3d um; - double pro_t; - - vector inputs; - vector durations; - - if (init_search) - { - inputs.push_back(start_acc_); - for (double tau = time_res_init * init_max_tau_; tau <= init_max_tau_; tau += time_res_init * init_max_tau_) - durations.push_back(tau); - } - else - { - for (double ax = -max_acc_; ax <= max_acc_ + 1e-3; ax += max_acc_ * res) - for (double ay = -max_acc_; ay <= max_acc_ + 1e-3; ay += max_acc_ * res) - for (double az = -max_acc_; az <= max_acc_ + 1e-3; az += max_acc_ * res) - { - um << ax, ay, 0.5 * az; - inputs.push_back(um); - } - for (double tau = time_res * max_tau_; tau <= max_tau_; tau += time_res * max_tau_) - durations.push_back(tau); - } - - /* ---------- state propagation loop ---------- */ - // cout << "cur state:" << cur_state.head(3).transpose() << endl; - for (int i = 0; i < inputs.size(); ++i) - for (int j = 0; j < durations.size(); ++j) - { - init_search = false; - um = inputs[i]; - double tau = durations[j]; - stateTransit(cur_state, pro_state, um, tau); - pro_t = cur_node->time + tau; - - /* ---------- check if in free space ---------- */ - - /* inside map range */ - if (pro_state(0) <= origin_(0) || pro_state(0) >= map_size_3d_(0) || pro_state(1) <= origin_(1) || - pro_state(1) >= map_size_3d_(1) || pro_state(2) <= origin_(2) || pro_state(2) >= map_size_3d_(2)) - { - // cout << "outside map" << endl; - continue; - } - - /* not in close set */ - Eigen::Vector3i pro_id = posToIndex(pro_state.head(3)); - int pro_t_id = timeToIndex(pro_t); - - PathNodePtr pro_node = dynamic ? expanded_nodes_.find(pro_id, pro_t_id) : expanded_nodes_.find(pro_id); - - if (pro_node != NULL && pro_node->node_state == IN_CLOSE_SET) - { - // cout << "in closeset" << endl; - continue; - } - - /* vel feasibe */ - Eigen::Vector3d pro_v = pro_state.tail(3); - if (fabs(pro_v(0)) > max_vel_ || fabs(pro_v(1)) > max_vel_ || fabs(pro_v(2)) > max_vel_) - { - // cout << "vel infeasible" << endl; - continue; - } - - /* not in the same voxel */ - Eigen::Vector3i diff = pro_id - cur_node->index; - int diff_time = pro_t_id - cur_node->time_idx; - if (diff.norm() == 0 && ((!dynamic) || diff_time == 0)) - { - continue; - } - - /* collision free */ - Eigen::Vector3d pos; - Eigen::Matrix xt; - bool is_occ = false; - - for (int k = 1; k <= check_num_; ++k) - { - double dt = tau * double(k) / double(check_num_); - stateTransit(cur_state, xt, um, dt); - pos = xt.head(3); - - double dist = - dynamic ? edt_env_->evaluateCoarseEDT(pos, cur_node->time + dt) : edt_env_->evaluateCoarseEDT(pos, -1.0); - if (dist <= margin_) - { - is_occ = true; - - break; - } - } - - if (is_occ) - { - // cout << "collision" << endl; - continue; - } - - /* ---------- compute cost ---------- */ - double time_to_goal, tmp_g_score, tmp_f_score; - tmp_g_score = (um.squaredNorm() + w_time_) * tau + cur_node->g_score; - tmp_f_score = tmp_g_score + lambda_heu_ * estimateHeuristic(pro_state, end_state, time_to_goal); - - /* ---------- compare expanded node in this loop ---------- */ - - bool prune = false; - for (int j = 0; j < tmp_expand_nodes.size(); ++j) - { - PathNodePtr expand_node = tmp_expand_nodes[j]; - if ((pro_id - expand_node->index).norm() == 0 && ((!dynamic) || pro_t_id == expand_node->time_idx)) - { - prune = true; - if (tmp_f_score < expand_node->f_score) - { - expand_node->f_score = tmp_f_score; - expand_node->g_score = tmp_g_score; - expand_node->state = pro_state; - expand_node->input = um; - expand_node->duration = tau; - if (dynamic) - expand_node->time = cur_node->time + tau; - } - break; - } - } - - /* ---------- new neighbor in this loop ---------- */ - - if (!prune) - { - if (pro_node == NULL) - { - pro_node = path_node_pool_[use_node_num_]; - pro_node->index = pro_id; - pro_node->state = pro_state; - pro_node->f_score = tmp_f_score; - pro_node->g_score = tmp_g_score; - pro_node->input = um; - pro_node->duration = tau; - pro_node->parent = cur_node; - pro_node->node_state = IN_OPEN_SET; - if (dynamic) - { - pro_node->time = cur_node->time + tau; - pro_node->time_idx = timeToIndex(pro_node->time); - } - open_set_.push(pro_node); - - if (dynamic) - expanded_nodes_.insert(pro_id, pro_node->time, pro_node); - else - expanded_nodes_.insert(pro_id, pro_node); - - tmp_expand_nodes.push_back(pro_node); - - use_node_num_ += 1; - if (use_node_num_ == allocate_num_) - { - cout << "run out of memory." << endl; - return NO_PATH; - } - } - else if (pro_node->node_state == IN_OPEN_SET) - { - if (tmp_g_score < pro_node->g_score) - { - // pro_node->index = pro_id; - pro_node->state = pro_state; - pro_node->f_score = tmp_f_score; - pro_node->g_score = tmp_g_score; - pro_node->input = um; - pro_node->duration = tau; - pro_node->parent = cur_node; - if (dynamic) - pro_node->time = cur_node->time + tau; - } - } - else - { - cout << "error type in searching: " << pro_node->node_state << endl; - } - } - - /* ---------- ---------- */ - } - } - - /* ---------- open set empty, no path ---------- */ - cout << "open set empty, no path!" << endl; - cout << "use node num: " << use_node_num_ << endl; - cout << "iter num: " << iter_num_ << endl; - return NO_PATH; -} - -void KinodynamicAstar::setParam(ros::NodeHandle& nh) -{ - nh.param("search/max_tau", max_tau_, -1.0); - nh.param("search/init_max_tau", init_max_tau_, -1.0); - nh.param("search/max_vel", max_vel_, -1.0); - nh.param("search/max_acc", max_acc_, -1.0); - nh.param("search/w_time", w_time_, -1.0); - nh.param("search/horizon", horizon_, -1.0); - nh.param("search/resolution_astar", resolution_, -1.0); - nh.param("search/time_resolution", time_resolution_, -1.0); - nh.param("search/lambda_heu", lambda_heu_, -1.0); - nh.param("search/margin", margin_, -1.0); - nh.param("search/allocate_num", allocate_num_, -1); - nh.param("search/check_num", check_num_, -1); - - cout << "margin:" << margin_ << endl; -} - -void KinodynamicAstar::retrievePath(PathNodePtr end_node) -{ - PathNodePtr cur_node = end_node; - path_nodes_.push_back(cur_node); - - while (cur_node->parent != NULL) - { - cur_node = cur_node->parent; - path_nodes_.push_back(cur_node); - } - - reverse(path_nodes_.begin(), path_nodes_.end()); -} -double KinodynamicAstar::estimateHeuristic(Eigen::VectorXd x1, Eigen::VectorXd x2, double& optimal_time) -{ - const Vector3d dp = x2.head(3) - x1.head(3); - const Vector3d v0 = x1.segment(3, 3); - const Vector3d v1 = x2.segment(3, 3); - - double c1 = -36 * dp.dot(dp); - double c2 = 24 * (v0 + v1).dot(dp); - double c3 = -4 * (v0.dot(v0) + v0.dot(v1) + v1.dot(v1)); - double c4 = 0; - double c5 = w_time_; - - std::vector ts = quartic(c5, c4, c3, c2, c1); - - double v_max = max_vel_; - double t_bar = (x1.head(3) - x2.head(3)).lpNorm() / v_max; - ts.push_back(t_bar); - - double cost = 100000000; - double t_d = t_bar; - - for (auto t : ts) - { - if (t < t_bar) - continue; - double c = -c1 / (3 * t * t * t) - c2 / (2 * t * t) - c3 / t + w_time_ * t; - if (c < cost) - { - cost = c; - t_d = t; - } - } - - optimal_time = t_d; - - return 1.0 * (1 + tie_breaker_) * cost; -} - -bool KinodynamicAstar::computeShotTraj(Eigen::VectorXd state1, Eigen::VectorXd state2, double time_to_goal) -{ - /* ---------- get coefficient ---------- */ - const Vector3d p0 = state1.head(3); - const Vector3d dp = state2.head(3) - p0; - const Vector3d v0 = state1.segment(3, 3); - const Vector3d v1 = state2.segment(3, 3); - const Vector3d dv = v1 - v0; - double t_d = time_to_goal; - MatrixXd coef(3, 4); - end_vel_ = v1; - - Vector3d a = 1.0 / 6.0 * (-12.0 / (t_d * t_d * t_d) * (dp - v0 * t_d) + 6 / (t_d * t_d) * dv); - Vector3d b = 0.5 * (6.0 / (t_d * t_d) * (dp - v0 * t_d) - 2 / t_d * dv); - Vector3d c = v0; - Vector3d d = p0; - - // 1/6 * alpha * t^3 + 1/2 * beta * t^2 + v0 - // a*t^3 + b*t^2 + v0*t + p0 - coef.col(3) = a, coef.col(2) = b, coef.col(1) = c, coef.col(0) = d; - - Vector3d coord, vel, acc; - VectorXd poly1d, t, polyv, polya; - Vector3i index; - - Eigen::MatrixXd Tm(4, 4); - Tm << 0, 1, 0, 0, 0, 0, 2, 0, 0, 0, 0, 3, 0, 0, 0, 0; - - /* ---------- forward checking of trajectory ---------- */ - double t_delta = t_d / 10; - for (double time = t_delta; time <= t_d; time += t_delta) - { - t = VectorXd::Zero(4); - for (int j = 0; j < 4; j++) - t(j) = pow(time, j); - - for (int dim = 0; dim < 3; dim++) - { - poly1d = coef.row(dim); - coord(dim) = poly1d.dot(t); - vel(dim) = (Tm * poly1d).dot(t); - acc(dim) = (Tm * Tm * poly1d).dot(t); - - if (fabs(vel(dim)) > max_vel_ || fabs(acc(dim)) > max_acc_) - { - // cout << "vel:" << vel(dim) << ", acc:" << acc(dim) << endl; - // return false; - } - } - - if (coord(0) < origin_(0) || coord(0) >= map_size_3d_(0) || coord(1) < origin_(1) || coord(1) >= map_size_3d_(1) || - coord(2) < origin_(2) || coord(2) >= map_size_3d_(2)) - { - return false; - } - - if (edt_env_->evaluateCoarseEDT(coord, -1.0) <= margin_) - { - return false; - } - } - coef_shot_ = coef; - t_shot_ = t_d; - is_shot_succ_ = true; - return true; -} - -vector KinodynamicAstar::cubic(double a, double b, double c, double d) -{ - vector dts; - - double a2 = b / a; - double a1 = c / a; - double a0 = d / a; - - double Q = (3 * a1 - a2 * a2) / 9; - double R = (9 * a1 * a2 - 27 * a0 - 2 * a2 * a2 * a2) / 54; - double D = Q * Q * Q + R * R; - if (D > 0) - { - double S = std::cbrt(R + sqrt(D)); - double T = std::cbrt(R - sqrt(D)); - dts.push_back(-a2 / 3 + (S + T)); - return dts; - } - else if (D == 0) - { - double S = std::cbrt(R); - dts.push_back(-a2 / 3 + S + S); - dts.push_back(-a2 / 3 - S); - return dts; - } - else - { - double theta = acos(R / sqrt(-Q * Q * Q)); - dts.push_back(2 * sqrt(-Q) * cos(theta / 3) - a2 / 3); - dts.push_back(2 * sqrt(-Q) * cos((theta + 2 * M_PI) / 3) - a2 / 3); - dts.push_back(2 * sqrt(-Q) * cos((theta + 4 * M_PI) / 3) - a2 / 3); - return dts; - } -} - -vector KinodynamicAstar::quartic(double a, double b, double c, double d, double e) -{ - vector dts; - - double a3 = b / a; - double a2 = c / a; - double a1 = d / a; - double a0 = e / a; - - vector ys = cubic(1, -a2, a1 * a3 - 4 * a0, 4 * a2 * a0 - a1 * a1 - a3 * a3 * a0); - double y1 = ys.front(); - double r = a3 * a3 / 4 - a2 + y1; - if (r < 0) - return dts; - - double R = sqrt(r); - double D, E; - if (R != 0) - { - D = sqrt(0.75 * a3 * a3 - R * R - 2 * a2 + 0.25 * (4 * a3 * a2 - 8 * a1 - a3 * a3 * a3) / R); - E = sqrt(0.75 * a3 * a3 - R * R - 2 * a2 - 0.25 * (4 * a3 * a2 - 8 * a1 - a3 * a3 * a3) / R); - } - else - { - D = sqrt(0.75 * a3 * a3 - 2 * a2 + 2 * sqrt(y1 * y1 - 4 * a0)); - E = sqrt(0.75 * a3 * a3 - 2 * a2 - 2 * sqrt(y1 * y1 - 4 * a0)); - } - - if (!std::isnan(D)) - { - dts.push_back(-a3 / 4 + R / 2 + D / 2); - dts.push_back(-a3 / 4 + R / 2 - D / 2); - } - if (!std::isnan(E)) - { - dts.push_back(-a3 / 4 - R / 2 + E / 2); - dts.push_back(-a3 / 4 - R / 2 - E / 2); - } - - return dts; -} - -void KinodynamicAstar::init() -{ - /* ---------- map params ---------- */ - this->inv_resolution_ = 1.0 / resolution_; - inv_time_resolution_ = 1.0 / time_resolution_; - edt_env_->getMapRegion(origin_, map_size_3d_); - - cout << "origin_: " << origin_.transpose() << endl; - cout << "map size: " << map_size_3d_.transpose() << endl; - - /* ---------- pre-allocated node ---------- */ - path_node_pool_.resize(allocate_num_); - for (int i = 0; i < allocate_num_; i++) - { - path_node_pool_[i] = new PathNode; - } - - phi_ = Eigen::MatrixXd::Identity(6, 6); - use_node_num_ = 0; - iter_num_ = 0; -} - -void KinodynamicAstar::setEnvironment(const EDTEnvironment::Ptr& env) -{ - this->edt_env_ = env; -} - -void KinodynamicAstar::reset() -{ - expanded_nodes_.clear(); - path_nodes_.clear(); - - std::priority_queue, NodeComparator> empty_queue; - open_set_.swap(empty_queue); - - for (int i = 0; i < use_node_num_; i++) - { - PathNodePtr node = path_node_pool_[i]; - node->parent = NULL; - node->node_state = NOT_EXPAND; - } - - use_node_num_ = 0; - iter_num_ = 0; - is_shot_succ_ = false; -} - -std::vector KinodynamicAstar::getKinoTraj(double delta_t) -{ - vector state_list; - - /* ---------- get traj of searching ---------- */ - PathNodePtr node = path_nodes_.back(); - Matrix x0, xt; - - while (node->parent != NULL) - { - Vector3d ut = node->input; - double duration = node->duration; - x0 = node->parent->state; - - for (double t = duration; t >= -1e-5; t -= delta_t) - { - stateTransit(x0, xt, ut, t); - state_list.push_back(xt.head(3)); - } - node = node->parent; - } - reverse(state_list.begin(), state_list.end()); - - /* ---------- get traj of one shot ---------- */ - if (is_shot_succ_) - { - Vector3d coord; - VectorXd poly1d, time(4); - - for (double t = delta_t; t <= t_shot_; t += delta_t) - { - for (int j = 0; j < 4; j++) - time(j) = pow(t, j); - - for (int dim = 0; dim < 3; dim++) - { - poly1d = coef_shot_.row(dim); - coord(dim) = poly1d.dot(time); - } - state_list.push_back(coord); - } - } - - return state_list; -} - -Eigen::MatrixXd KinodynamicAstar::getSamples(double& ts, int& K) -{ - /* ---------- final trajectory time ---------- */ - double T_sum = 0.0; - if (is_shot_succ_) - T_sum += t_shot_; - - PathNodePtr node = path_nodes_.back(); - while (node->parent != NULL) - { - T_sum += node->duration; - node = node->parent; - } - // cout << "final time:" << T_sum << endl; - - /* ---------- init for sampling ---------- */ - K = floor(T_sum / ts); - ts = T_sum / (K + 1); - // cout << "K:" << K << ", ts:" << ts << endl; - - bool sample_shot_traj = is_shot_succ_; - - Eigen::VectorXd sx(K + 2), sy(K + 2), sz(K + 2); - int sample_num = 0; - node = path_nodes_.back(); - - double t; - if (sample_shot_traj) - t = t_shot_; - else - { - t = node->duration; - end_vel_ = node->state.tail(3); - } - - for (double ti = T_sum; ti > -1e-5; ti -= ts) - { - /* ---------- sample shot traj---------- */ - if (sample_shot_traj) - { - Vector3d coord; - VectorXd poly1d, time(4); - for (int j = 0; j < 4; j++) - time(j) = pow(t, j); - - for (int dim = 0; dim < 3; dim++) - { - poly1d = coef_shot_.row(dim); - coord(dim) = poly1d.dot(time); - } - - sx(sample_num) = coord(0), sy(sample_num) = coord(1), sz(sample_num) = coord(2); - ++sample_num; - t -= ts; - - /* end of segment */ - if (t < -1e-5) - { - sample_shot_traj = false; - if (node->parent != NULL) - t += node->duration; - } - } - /* ---------- sample search traj---------- */ - else - { - Eigen::Matrix x0 = node->parent->state; - Eigen::Matrix xt; - Vector3d ut = node->input; - - stateTransit(x0, xt, ut, t); - sx(sample_num) = xt(0), sy(sample_num) = xt(1), sz(sample_num) = xt(2); - ++sample_num; - - t -= ts; - // cout << "t: " << t << ", t acc: " << T_accumulate << endl; - if (t < -1e-5 && node->parent->parent != NULL) - { - node = node->parent; - t += node->duration; - } - } - } - /* ---------- return samples ---------- */ - Eigen::MatrixXd samples(3, K + 5); - samples.block(0, 0, 1, K + 2) = sx.reverse().transpose(); - samples.block(1, 0, 1, K + 2) = sy.reverse().transpose(); - samples.block(2, 0, 1, K + 2) = sz.reverse().transpose(); - samples.col(K + 2) = start_vel_; - samples.col(K + 3) = end_vel_; - samples.col(K + 4) = node->input; - - return samples; -} - -std::vector KinodynamicAstar::getVisitedNodes() -{ - vector visited; - visited.assign(path_node_pool_.begin(), path_node_pool_.begin() + use_node_num_ - 1); - return visited; -} - -Eigen::Vector3i KinodynamicAstar::posToIndex(Eigen::Vector3d pt) -{ - Vector3i idx = ((pt - origin_) * inv_resolution_).array().floor().cast(); - - // idx << floor((pt(0) - origin_(0)) * inv_resolution_), floor((pt(1) - origin_(1)) * inv_resolution_), - // floor((pt(2) - origin_(2)) * inv_resolution_); - - return idx; -} - -int KinodynamicAstar::timeToIndex(double time) -{ - int idx = floor((time - time_origin_) * inv_time_resolution_); -} - -void KinodynamicAstar::stateTransit(Eigen::Matrix& state0, Eigen::Matrix& state1, - Eigen::Vector3d um, double tau) -{ - for (int i = 0; i < 3; ++i) - phi_(i, i + 3) = tau; - - Eigen::Matrix integral; - integral.head(3) = 0.5 * pow(tau, 2) * um; - integral.tail(3) = tau * um; - - state1 = phi_ * state0 + integral; -} - -} // namespace dyn_planner diff --git a/dyn_planner/plan_env/CMakeLists.txt b/dyn_planner/plan_env/CMakeLists.txt deleted file mode 100644 index 9444d0a84..000000000 --- a/dyn_planner/plan_env/CMakeLists.txt +++ /dev/null @@ -1,40 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(plan_env) - -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - visualization_msgs -) - -find_package(Eigen3 REQUIRED) -find_package(PCL 1.7 REQUIRED) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES plan_env - CATKIN_DEPENDS roscpp std_msgs -# DEPENDS system_lib -) - -include_directories( - SYSTEM - include - ${catkin_INCLUDE_DIRS} - ${Eigen3_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -link_directories(${PCL_LIBRARY_DIRS}) - -set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS} -O3 -Wall") - -add_library( plan_env - src/sdf_map.cpp - src/edt_environment.cpp - ) -target_link_libraries( plan_env - ${catkin_LIBRARIES} - ${PCL_LIBRARIES} - ) diff --git a/dyn_planner/plan_env/include/plan_env/edt_environment.h b/dyn_planner/plan_env/include/plan_env/edt_environment.h deleted file mode 100644 index f8271940d..000000000 --- a/dyn_planner/plan_env/include/plan_env/edt_environment.h +++ /dev/null @@ -1,49 +0,0 @@ -#ifndef _EDT_ENVIRONMENT_H_ -#define _EDT_ENVIRONMENT_H_ - -#include -#include -#include -#include - -#include - -using std::cout; -using std::endl; -using std::list; -using std::shared_ptr; -using std::unique_ptr; -using std::vector; - -namespace dyn_planner -{ -class EDTEnvironment -{ -private: - /* data */ - SDFMap::Ptr sdf_map_; - double resolution_inv_; - -public: - EDTEnvironment(/* args */) {} - ~EDTEnvironment() {} - - void init(); - void setMap(SDFMap::Ptr map); - - void evaluateEDTWithGrad(const Eigen::Vector3d& pos, const double& time, double& dist, - Eigen::Vector3d& grad); - - double evaluateCoarseEDT(const Eigen::Vector3d& pos, const double& time); - - bool odomValid() { return sdf_map_->odomValid(); } - bool mapValid() { return sdf_map_->mapValid(); } - nav_msgs::Odometry getOdom() { return sdf_map_->getOdom(); } - void getMapRegion(Eigen::Vector3d& ori, Eigen::Vector3d& size) { sdf_map_->getRegion(ori, size); } - - typedef shared_ptr Ptr; -}; - -} // namespace dyn_planner - -#endif \ No newline at end of file diff --git a/dyn_planner/plan_env/include/plan_env/sdf_map.h b/dyn_planner/plan_env/include/plan_env/sdf_map.h deleted file mode 100644 index 805e01d92..000000000 --- a/dyn_planner/plan_env/include/plan_env/sdf_map.h +++ /dev/null @@ -1,105 +0,0 @@ -#ifndef _SDF_MAP_H -#define _SDF_MAP_H - -#include -#include -#include - -#include -#include -// #include -#include -#include -#include - -using namespace std; - -namespace dyn_planner -{ -class SDFMap -{ -private: - // data are saved in vector - std::vector occupancy_buffer_; // 0 is free, 1 is occupied - std::vector distance_buffer_; - std::vector distance_buffer_neg_; - std::vector tmp_buffer1_, tmp_buffer2_; - - // map property - Eigen::Vector3d min_range_, max_range_; // map range in pos - Eigen::Vector3i grid_size_; // map range in index - Eigen::Vector3i min_vec_, max_vec_; // the min and max updated range, unit is 1 - - bool isInMap(Eigen::Vector3d pos); - void posToIndex(Eigen::Vector3d pos, Eigen::Vector3i& id); - void indexToPos(Eigen::Vector3i id, Eigen::Vector3d& pos); - - template - void fillESDF(F_get_val f_get_val, F_set_val f_set_val, int start, int end, int dim); - - /* ---------- parameter ---------- */ - double inflate_, update_range_, radius_ignore_; - Eigen::Vector3d origin_, map_size_; - double resolution_sdf_, resolution_inv_; - double ceil_height_; - double update_rate_; - - /* ---------- callback ---------- */ - nav_msgs::Odometry odom_; - bool have_odom_; - - pcl::PointCloud latest_cloud_, cloud_inflate_vis_; - bool new_map_, map_valid_; - - ros::NodeHandle node_; - ros::Subscriber odom_sub_, cloud_sub_; - ros::Publisher inflate_cloud_pub_; - ros::Timer update_timer_; - - void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg); - void odomCallback(const nav_msgs::OdometryConstPtr& msg); - void updateCallback(const ros::TimerEvent& e); - - /* --------------------------------- */ - -public: - SDFMap() {} - SDFMap(Eigen::Vector3d origin, double resolution, Eigen::Vector3d map_size); - ~SDFMap() {} - void init(ros::NodeHandle& nh); - - /* get state */ - bool odomValid() { return have_odom_; } - bool mapValid() { return map_valid_; } - nav_msgs::Odometry getOdom() { return odom_; } - void getRegion(Eigen::Vector3d& ori, Eigen::Vector3d& size) { ori = origin_, size = map_size_; } - double getResolution() { return resolution_sdf_; } - double getIgnoreRadius() { return radius_ignore_; } - void getInterpolationData(const Eigen::Vector3d& pos, vector& pos_vec, - Eigen::Vector3d& diff); - - // occupancy management - void resetBuffer(Eigen::Vector3d min, Eigen::Vector3d max); - void setOccupancy(Eigen::Vector3d pos, int occ = 1); - int getOccupancy(Eigen::Vector3d pos); - int getOccupancy(Eigen::Vector3i id); - void getOccupancyMarker(visualization_msgs::Marker& m, int id, Eigen::Vector4d color); - - // distance field management - double getDistance(Eigen::Vector3d pos); - double getDistance(Eigen::Vector3i id); - double getDistWithGradTrilinear(Eigen::Vector3d pos, Eigen::Vector3d& grad); - double getDistTrilinear(Eigen::Vector3d pos); - void setUpdateRange(Eigen::Vector3d min_pos, Eigen::Vector3d max_pos); - void updateESDF3d(bool neg = false); - void getESDFMarker(vector& markers, int id, Eigen::Vector3d color); - double getMaxDistance(); - - void publishESDF(); - - typedef shared_ptr Ptr; -}; - -} // namespace dyn_planner - -#endif \ No newline at end of file diff --git a/dyn_planner/plan_env/package.xml b/dyn_planner/plan_env/package.xml deleted file mode 100644 index 3e92617cf..000000000 --- a/dyn_planner/plan_env/package.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - plan_env - 0.0.0 - The plan_env package - - - - - bzhouai - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - - - - - - - - diff --git a/dyn_planner/plan_env/src/edt_environment.cpp b/dyn_planner/plan_env/src/edt_environment.cpp deleted file mode 100644 index a7efb38e0..000000000 --- a/dyn_planner/plan_env/src/edt_environment.cpp +++ /dev/null @@ -1,63 +0,0 @@ -#include - -namespace dyn_planner -{ -/* ============================== edt_environment ============================== */ -void EDTEnvironment::init() -{ -} - -void EDTEnvironment::setMap(shared_ptr map) -{ - this->sdf_map_ = map; - resolution_inv_ = 1 / sdf_map_->getResolution(); -} - -void EDTEnvironment::evaluateEDTWithGrad(const Eigen::Vector3d& pos, const double& time, double& dist, - Eigen::Vector3d& grad) -{ - vector pos_vec; - Eigen::Vector3d diff; - sdf_map_->getInterpolationData(pos, pos_vec, diff); - - /* ---------- value from surrounding position ---------- */ - double values[2][2][2]; - Eigen::Vector3d pt; - for (int x = 0; x < 2; x++) - for (int y = 0; y < 2; y++) - for (int z = 0; z < 2; z++) - { - pt = pos_vec[4 * x + 2 * y + z]; - double d1 = sdf_map_->getDistance(pt); - values[x][y][z] = d1; - } - - /* ---------- use trilinear interpolation ---------- */ - double v00 = (1 - diff(0)) * values[0][0][0] + diff(0) * values[1][0][0]; - double v01 = (1 - diff(0)) * values[0][0][1] + diff(0) * values[1][0][1]; - double v10 = (1 - diff(0)) * values[0][1][0] + diff(0) * values[1][1][0]; - double v11 = (1 - diff(0)) * values[0][1][1] + diff(0) * values[1][1][1]; - - double v0 = (1 - diff(1)) * v00 + diff(1) * v10; - double v1 = (1 - diff(1)) * v01 + diff(1) * v11; - - dist = (1 - diff(2)) * v0 + diff(2) * v1; - - grad[2] = (v1 - v0) * resolution_inv_; - grad[1] = ((1 - diff[2]) * (v10 - v00) + diff[2] * (v11 - v01)) * resolution_inv_; - grad[0] = (1 - diff[2]) * (1 - diff[1]) * (values[1][0][0] - values[0][0][0]); - grad[0] += (1 - diff[2]) * diff[1] * (values[1][1][0] - values[0][1][0]); - grad[0] += diff[2] * (1 - diff[1]) * (values[1][0][1] - values[0][0][1]); - grad[0] += diff[2] * diff[1] * (values[1][1][1] - values[0][1][1]); - - grad[0] *= resolution_inv_; -} - -double EDTEnvironment::evaluateCoarseEDT(const Eigen::Vector3d& pos, const double& time) -{ - double d1 = sdf_map_->getDistance(pos); - return d1; -} - -// EDTEnvironment:: -} // namespace dyn_planner \ No newline at end of file diff --git a/dyn_planner/plan_env/src/sdf_map.cpp b/dyn_planner/plan_env/src/sdf_map.cpp deleted file mode 100644 index d4082a6b4..000000000 --- a/dyn_planner/plan_env/src/sdf_map.cpp +++ /dev/null @@ -1,709 +0,0 @@ -#include "plan_env/sdf_map.h" - -namespace dyn_planner -{ -SDFMap::SDFMap(Eigen::Vector3d ori, double resolution, Eigen::Vector3d size) -{ - this->origin_ = ori; - this->resolution_sdf_ = resolution; - this->resolution_inv_ = 1 / resolution_sdf_; - this->map_size_ = size; - for (int i = 0; i < 3; ++i) - grid_size_(i) = ceil(map_size_(i) / resolution_sdf_); - // cout << "grid num:" << grid_size_.transpose() << endl; - min_range_ = origin_; - max_range_ = origin_ + map_size_; - min_vec_ = Eigen::Vector3i::Zero(); - max_vec_ = grid_size_ - Eigen::Vector3i::Ones(); - - // initialize size of buffer - occupancy_buffer_.resize(grid_size_(0) * grid_size_(1) * grid_size_(2)); - distance_buffer_.resize(grid_size_(0) * grid_size_(1) * grid_size_(2)); - tmp_buffer1_.resize(grid_size_(0) * grid_size_(1) * grid_size_(2)); - tmp_buffer2_.resize(grid_size_(0) * grid_size_(1) * grid_size_(2)); - - fill(distance_buffer_.begin(), distance_buffer_.end(), 10000); - fill(occupancy_buffer_.begin(), occupancy_buffer_.end(), 0.0); -} - -void SDFMap::resetBuffer(Eigen::Vector3d min_pos, Eigen::Vector3d max_pos) -{ - min_pos(0) = max(min_pos(0), min_range_(0)); - min_pos(1) = max(min_pos(1), min_range_(1)); - min_pos(2) = max(min_pos(2), min_range_(2)); - - max_pos(0) = min(max_pos(0), max_range_(0)); - max_pos(1) = min(max_pos(1), max_range_(1)); - max_pos(2) = min(max_pos(2), max_range_(2)); - - Eigen::Vector3i min_id, max_id; - - posToIndex(min_pos, min_id); - posToIndex(max_pos - Eigen::Vector3d(resolution_sdf_ / 2, resolution_sdf_ / 2, resolution_sdf_ / 2), max_id); - - /* reset occ and dist buffer */ - for (int x = min_id(0); x <= max_id(0); ++x) - for (int y = min_id(1); y <= max_id(1); ++y) - for (int z = min_id(2); z <= max_id(2); ++z) - { - occupancy_buffer_[x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z] = 0.0; - distance_buffer_[x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z] = 10000; - } -} - -bool SDFMap::isInMap(Eigen::Vector3d pos) -{ - if (pos(0) < min_range_(0) + 1e-4 || pos(1) < min_range_(1) + 1e-4 || pos(2) < min_range_(2) + 1e-4) - { - // cout << "less than min range!" << endl; - return false; - } - - if (pos(0) > max_range_(0) - 1e-4 || pos(1) > max_range_(1) - 1e-4 || pos(2) > max_range_(2) - 1e-4) - { - // cout << "larger than max range!" << endl; - return false; - } - - return true; -} - -void SDFMap::posToIndex(Eigen::Vector3d pos, Eigen::Vector3i& id) -{ - for (int i = 0; i < 3; ++i) - id(i) = floor((pos(i) - origin_(i)) * resolution_inv_); -} - -void SDFMap::indexToPos(Eigen::Vector3i id, Eigen::Vector3d& pos) -{ - for (int i = 0; i < 3; ++i) - pos(i) = (id(i) + 0.5) * resolution_sdf_ + origin_(i); -} - -void SDFMap::setOccupancy(Eigen::Vector3d pos, int occ) -{ - if (occ != 1 && occ != 0) - { - cout << "occ value error!" << endl; - return; - } - - if (!isInMap(pos)) - return; - - Eigen::Vector3i id; - posToIndex(pos, id); - - // (x, y, z) -> x*ny*nz + y*nz + z - // cout << "..." - // << id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2) - // << endl; - // cout << "..." << occupancy_buffer_.size() << endl; - occupancy_buffer_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)] = occ; -} - -int SDFMap::getOccupancy(Eigen::Vector3d pos) -{ - if (!isInMap(pos)) - return -1; - - Eigen::Vector3i id; - posToIndex(pos, id); - - // (x, y, z) -> x*ny*nz + y*nz + z - return occupancy_buffer_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)]; -} - -int SDFMap::getOccupancy(Eigen::Vector3i id) -{ - if (id(0) < 0 || id(0) >= grid_size_(0) || id(1) < 0 || id(1) >= grid_size_(1) || id(2) < 0 || id(2) >= grid_size_(2)) - return -1; - - // (x, y, z) -> x*ny*nz + y*nz + z - return occupancy_buffer_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)]; -} - -void SDFMap::getOccupancyMarker(visualization_msgs::Marker& m, int id, Eigen::Vector4d color) -{ - m.header.frame_id = "world"; - m.id = id; - m.type = visualization_msgs::Marker::CUBE_LIST; - m.action = visualization_msgs::Marker::MODIFY; - m.scale.x = resolution_sdf_ * 0.9; - m.scale.y = resolution_sdf_ * 0.9; - m.scale.z = resolution_sdf_ * 0.9; - m.color.a = color(3); - m.color.r = color(0); - m.color.g = color(1); - m.color.b = color(2); - - // iterate the map - for (int x = 0; x < grid_size_(0); ++x) - for (int y = 0; y < grid_size_(1); ++y) - for (int z = 0; z < grid_size_(2); ++z) - { - if (1 != occupancy_buffer_[x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z]) - continue; - - Eigen::Vector3d pos; - indexToPos(Eigen::Vector3i(x, y, z), pos); - - geometry_msgs::Point p; - p.x = pos(0); - p.y = pos(1); - p.z = pos(2); - m.points.push_back(p); - } -} - -double SDFMap::getDistance(Eigen::Vector3d pos) -{ - if (!isInMap(pos)) - return -1; - - Eigen::Vector3i id; - posToIndex(pos, id); - - // (x, y, z) -> x*ny*nz + y*nz + z - return distance_buffer_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)]; -} - -double SDFMap::getDistance(Eigen::Vector3i id) -{ - id(0) = max(min(id(0), grid_size_(0) - 1), 0); - id(1) = max(min(id(1), grid_size_(1) - 1), 0); - id(2) = max(min(id(2), grid_size_(2) - 1), 0); - - // (x, y, z) -> x*ny*nz + y*nz + z - return distance_buffer_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)]; -} - -double SDFMap::getDistWithGradTrilinear(Eigen::Vector3d pos, Eigen::Vector3d& grad) -{ - if (!isInMap(pos)) - return -1; - - /* use trilinear interpolation */ - Eigen::Vector3d pos_m = pos - 0.5 * resolution_sdf_ * Eigen::Vector3d::Ones(); - - Eigen::Vector3i idx; - posToIndex(pos_m, idx); - - Eigen::Vector3d idx_pos, diff; - indexToPos(idx, idx_pos); - - diff = (pos - idx_pos) * resolution_inv_; - - double values[2][2][2]; - for (int x = 0; x < 2; x++) - { - for (int y = 0; y < 2; y++) - { - for (int z = 0; z < 2; z++) - { - Eigen::Vector3i current_idx = idx + Eigen::Vector3i(x, y, z); - values[x][y][z] = getDistance(current_idx); - } - } - } - - double v00 = (1 - diff[0]) * values[0][0][0] + diff[0] * values[1][0][0]; - double v01 = (1 - diff[0]) * values[0][0][1] + diff[0] * values[1][0][1]; - double v10 = (1 - diff[0]) * values[0][1][0] + diff[0] * values[1][1][0]; - double v11 = (1 - diff[0]) * values[0][1][1] + diff[0] * values[1][1][1]; - - double v0 = (1 - diff[1]) * v00 + diff[1] * v10; - double v1 = (1 - diff[1]) * v01 + diff[1] * v11; - - double dist = (1 - diff[2]) * v0 + diff[2] * v1; - - grad[2] = (v1 - v0) * resolution_inv_; - grad[1] = ((1 - diff[2]) * (v10 - v00) + diff[2] * (v11 - v01)) * resolution_inv_; - grad[0] = (1 - diff[2]) * (1 - diff[1]) * (values[1][0][0] - values[0][0][0]); - grad[0] += (1 - diff[2]) * diff[1] * (values[1][1][0] - values[0][1][0]); - grad[0] += diff[2] * (1 - diff[1]) * (values[1][0][1] - values[0][0][1]); - grad[0] += diff[2] * diff[1] * (values[1][1][1] - values[0][1][1]); - - grad[0] *= resolution_inv_; - - return dist; -} - -double SDFMap::getDistTrilinear(Eigen::Vector3d pos) -{ - if (!isInMap(pos)) - return -1; - - /* use trilinear interpolation */ - Eigen::Vector3d pos_m = pos - 0.5 * resolution_sdf_ * Eigen::Vector3d::Ones(); - - Eigen::Vector3i idx; - posToIndex(pos_m, idx); - - Eigen::Vector3d idx_pos, diff; - indexToPos(idx, idx_pos); - - diff = (pos - idx_pos) * resolution_inv_; - - double values[2][2][2]; - for (int x = 0; x < 2; x++) - { - for (int y = 0; y < 2; y++) - { - for (int z = 0; z < 2; z++) - { - Eigen::Vector3i current_idx = idx + Eigen::Vector3i(x, y, z); - values[x][y][z] = getDistance(current_idx); - } - } - } - - double v00 = (1 - diff[0]) * values[0][0][0] + diff[0] * values[1][0][0]; - double v01 = (1 - diff[0]) * values[0][0][1] + diff[0] * values[1][0][1]; - double v10 = (1 - diff[0]) * values[0][1][0] + diff[0] * values[1][1][0]; - double v11 = (1 - diff[0]) * values[0][1][1] + diff[0] * values[1][1][1]; - - double v0 = (1 - diff[1]) * v00 + diff[1] * v10; - double v1 = (1 - diff[1]) * v01 + diff[1] * v11; - - double dist = (1 - diff[2]) * v0 + diff[2] * v1; - - return dist; -} - -void SDFMap::setUpdateRange(Eigen::Vector3d min_pos, Eigen::Vector3d max_pos) -{ - /* chou gou shi! */ - // if (!isInMap(min_pos)) min_pos = min_range_; - // if (!isInMap(max_pos)) max_pos = max_range_; - min_pos(0) = max(min_pos(0), min_range_(0)); - min_pos(1) = max(min_pos(1), min_range_(1)); - min_pos(2) = max(min_pos(2), min_range_(2)); - - max_pos(0) = min(max_pos(0), max_range_(0)); - max_pos(1) = min(max_pos(1), max_range_(1)); - max_pos(2) = min(max_pos(2), max_range_(2)); - - posToIndex(min_pos, min_vec_); - posToIndex(max_pos - Eigen::Vector3d(resolution_sdf_ / 2, resolution_sdf_ / 2, resolution_sdf_ / 2), max_vec_); -} - -template -void SDFMap::fillESDF(F_get_val f_get_val, F_set_val f_set_val, int start, int end, int dim) -{ - int v[grid_size_(dim)]; - double z[grid_size_(dim) + 1]; - - int k = start; - v[start] = start; - z[start] = -std::numeric_limits::max(); - z[start + 1] = std::numeric_limits::max(); - - for (int q = start + 1; q <= end; q++) - { - k++; - double s; - - do - { - k--; - s = ((f_get_val(q) + q * q) - (f_get_val(v[k]) + v[k] * v[k])) / (2 * q - 2 * v[k]); - - } while (s <= z[k]); - - k++; - - v[k] = q; - z[k] = s; - z[k + 1] = std::numeric_limits::max(); - } - - k = start; - - for (int q = start; q <= end; q++) - { - while (z[k + 1] < q) - k++; - double val = (q - v[k]) * (q - v[k]) + f_get_val(v[k]); - f_set_val(q, val); - } -} - -void SDFMap::updateESDF3d(bool neg) -{ - for (int x = min_vec_[0]; x <= max_vec_[0]; x++) - { - for (int y = min_vec_[1]; y <= max_vec_[1]; y++) - { - fillESDF( - [&](int z) { - return occupancy_buffer_[x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z] == 1 ? - 0 : - std::numeric_limits::max(); - }, - [&](int z, double val) { tmp_buffer1_[x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z] = val; }, - min_vec_[2], max_vec_[2], 2); - } - } - - for (int x = min_vec_[0]; x <= max_vec_[0]; x++) - { - for (int z = min_vec_[2]; z <= max_vec_[2]; z++) - { - fillESDF( - [&](int y) { - // cout << "get xyz:" << x << ", " << y << ", " << z << endl; - return tmp_buffer1_[x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z]; - }, - [&](int y, double val) { - // cout << "set xyz:" << x << ", " << y << ", " << z << endl; - // cout << "index:" << x * grid_size_(1) * grid_size_(2) + y * - // grid_size_(2) + z << endl; cout << "buffer length:" << - // tmp_buffer2_.size() << endl; - tmp_buffer2_[x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z] = val; - }, - min_vec_[1], max_vec_[1], 1); - } - } - - for (int y = min_vec_[1]; y <= max_vec_[1]; y++) - { - for (int z = min_vec_[2]; z <= max_vec_[2]; z++) - { - fillESDF([&](int x) { return tmp_buffer2_[x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z]; }, - [&](int x, double val) { - distance_buffer_[x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z] = - resolution_sdf_ * std::sqrt(val); - }, - min_vec_[0], max_vec_[0], 0); - } - } - - if (!neg) - { - min_vec_ = Eigen::Vector3i::Zero(); - max_vec_ = grid_size_ - Eigen::Vector3i::Ones(); - return; - } - - /* ============================== negative distance field ============================== */ - tmp_buffer1_.clear(); - tmp_buffer2_.clear(); - - ros::Time t1, t2; - - for (int x = min_vec_[0]; x <= max_vec_[0]; x++) - { - for (int y = min_vec_[1]; y <= max_vec_[1]; y++) - { - fillESDF( - [&](int z) { - return occupancy_buffer_[x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z] == 0 ? - 0 : - std::numeric_limits::max(); - }, - [&](int z, double val) { tmp_buffer1_[x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z] = val; }, - min_vec_[2], max_vec_[2], 2); - } - } - - for (int x = min_vec_[0]; x <= max_vec_[0]; x++) - { - for (int z = min_vec_[2]; z <= max_vec_[2]; z++) - { - fillESDF( - [&](int y) { return tmp_buffer1_[x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z]; }, - [&](int y, double val) { tmp_buffer2_[x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z] = val; }, - min_vec_[1], max_vec_[1], 1); - } - } - - for (int y = min_vec_[1]; y <= max_vec_[1]; y++) - { - for (int z = min_vec_[2]; z <= max_vec_[2]; z++) - { - fillESDF([&](int x) { return tmp_buffer2_[x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z]; }, - [&](int x, double val) { - distance_buffer_neg_[x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z] = - resolution_sdf_ * std::sqrt(val); - }, - min_vec_[0], max_vec_[0], 0); - } - } - - /* ========== combine pos and neg DT ========== */ - for (int x = min_vec_(0); x <= max_vec_(0); ++x) - for (int y = min_vec_(1); y <= max_vec_(1); ++y) - for (int z = min_vec_(2); z <= max_vec_(2); ++z) - { - int idx = x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z; - - if (distance_buffer_neg_[idx] > 0.0) - distance_buffer_[idx] = distance_buffer_[idx] - distance_buffer_neg_[idx] + resolution_sdf_; - } - - min_vec_ = Eigen::Vector3i::Zero(); - max_vec_ = grid_size_ - Eigen::Vector3i::Ones(); -} - -void SDFMap::getESDFMarker(vector& markers, int id, Eigen::Vector3d color) -{ - double max_dist = getMaxDistance(); - - // get marker in several distance level - const int level = ceil(max_dist * resolution_inv_); - - for (int i = 0; i < level; ++i) - { - visualization_msgs::Marker m; - m.header.frame_id = "world"; - m.id = i + level * id; - m.type = visualization_msgs::Marker::CUBE_LIST; - m.action = visualization_msgs::Marker::ADD; - m.scale.x = resolution_sdf_ * 0.9; - m.scale.y = resolution_sdf_ * 0.9; - m.scale.z = resolution_sdf_ * 0.9; - m.color.r = color(0); - m.color.g = color(1); - m.color.b = color(2); - - // transparency and distance conversion - double min_a = 0.05, max_a = 0.25; - double da = (max_a - min_a) / (level - 1); - m.color.a = max_a - da * i; - // cout << "alpha:" << m.color.a << endl; - - // distance level - double delta_d = max_dist / level; - double min_d = i * delta_d - 1e-3; - double max_d = (i + 1) * delta_d - 1e-3; - - // iterate the map - for (int x = 0; x < grid_size_(0); ++x) - for (int y = 0; y < grid_size_(1); ++y) - for (int z = 0; z < grid_size_(2) - 15; ++z) - { - double dist = distance_buffer_[x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z]; - bool in_range = dist < max_d && dist >= min_d; - if (!in_range) - continue; - - Eigen::Vector3d pos; - indexToPos(Eigen::Vector3i(x, y, z), pos); - - geometry_msgs::Point p; - p.x = pos(0); - p.y = pos(1); - p.z = pos(2); - m.points.push_back(p); - } - markers.push_back(m); - } -} - -double SDFMap::getMaxDistance() -{ - // get the max distance - double max_dist = -1; - for (int i = 0; i < int(distance_buffer_.size()); ++i) - { - if (distance_buffer_[i] > max_dist) - max_dist = distance_buffer_[i]; - } - // cout << "Max distance is:" << max_dist << endl; - return max_dist; -} - -void SDFMap::cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) -{ - /* need odom_ for center radius sensing */ - if (!have_odom_) - { - cout << "no odom_" << endl; - return; - } - - pcl::fromROSMsg(*msg, latest_cloud_); - - // if ((int)latest_cloud_.points.size() == 0) return; - - new_map_ = true; -} - -void SDFMap::odomCallback(const nav_msgs::OdometryConstPtr& msg) -{ - if (msg->child_frame_id == "X" || msg->child_frame_id == "O") - return; - - odom_ = *msg; - odom_.header.frame_id = "world"; - have_odom_ = true; -} - -void SDFMap::updateCallback(const ros::TimerEvent& e) -{ - if (!new_map_) - { - // cout << "no new map." << endl; - return; - } - map_valid_ = true; - new_map_ = false; - - if (latest_cloud_.points.size() == 0) - return; - - Eigen::Vector3d center(odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z); - if (isnan(center(0)) || isnan(center(1)) || isnan(center(2))) - return; - - /* ---------- inflate cloud and insert to SDFMap ---------- */ - Eigen::Vector3d disp(update_range_, update_range_, update_range_ / 2.0); - this->resetBuffer(center - disp, center + disp); - - cloud_inflate_vis_.clear(); - pcl::PointXYZ pt, pt_inf; - Eigen::Vector3d p3d, p3d_inf; - const int ifn = ceil(inflate_ * resolution_inv_); - - for (size_t i = 0; i < latest_cloud_.points.size(); ++i) - { - pt = latest_cloud_.points[i]; - p3d(0) = pt.x, p3d(1) = pt.y, p3d(2) = pt.z; - - /* point inside update range */ - if ((center - p3d).norm() < update_range_) - { - /* inflate the point */ - for (int x = -ifn; x <= ifn; ++x) - for (int y = -ifn; y <= ifn; ++y) - for (int z = -ifn; z <= ifn; ++z) - { - p3d_inf(0) = pt_inf.x = pt.x + x * resolution_sdf_; - p3d_inf(1) = pt_inf.y = pt.y + y * resolution_sdf_; - p3d_inf(2) = pt_inf.z = pt.z + 0.5 * z * resolution_sdf_; - - this->setOccupancy(p3d_inf); - - if (pt_inf.z < 2.0) - cloud_inflate_vis_.push_back(pt_inf); - } - } - } - cloud_inflate_vis_.width = cloud_inflate_vis_.points.size(); - cloud_inflate_vis_.height = 1; - cloud_inflate_vis_.is_dense = true; - cloud_inflate_vis_.header.frame_id = "world"; - cloud_inflate_vis_.header.seq = latest_cloud_.header.seq; - cloud_inflate_vis_.header.stamp = latest_cloud_.header.stamp; - sensor_msgs::PointCloud2 map_inflate_vis; - pcl::toROSMsg(cloud_inflate_vis_, map_inflate_vis); - - inflate_cloud_pub_.publish(map_inflate_vis); - - /* ---------- add ceil ---------- */ - if (ceil_height_ > 0.0) - { - for (double cx = center(0) - update_range_; cx <= center(0) + update_range_; cx += resolution_sdf_) - for (double cy = center(1) - update_range_; cy <= center(1) + update_range_; cy += resolution_sdf_) - { - this->setOccupancy(Eigen::Vector3d(cx, cy, ceil_height_)); - } - } - - /* ---------- update ESDF ---------- */ - this->setUpdateRange(center - disp, center + disp); - this->updateESDF3d(true); -} - -void SDFMap::init(ros::NodeHandle& nh) -{ - node_ = nh; - - /* ---------- param ---------- */ - node_.param("sdf_map/origin_x", origin_(0), -20.0); - node_.param("sdf_map/origin_y", origin_(1), -20.0); - node_.param("sdf_map/origin_z", origin_(2), 0.0); - - node_.param("sdf_map/map_size_x", map_size_(0), 40.0); - node_.param("sdf_map/map_size_y", map_size_(1), 40.0); - node_.param("sdf_map/map_size_z", map_size_(2), 5.0); - - node_.param("sdf_map/resolution_sdf", resolution_sdf_, 0.2); - node_.param("sdf_map/ceil_height", ceil_height_, 2.0); - node_.param("sdf_map/update_rate", update_rate_, 10.0); - node_.param("sdf_map/update_range", update_range_, 5.0); - node_.param("sdf_map/inflate", inflate_, 0.2); - node_.param("sdf_map/radius_ignore", radius_ignore_, 0.2); - - cout << "origin_: " << origin_.transpose() << endl; - cout << "map size: " << map_size_.transpose() << endl; - cout << "resolution: " << resolution_sdf_ << endl; - - /* ---------- sub and pub ---------- */ - odom_sub_ = node_.subscribe("/odom_world", 10, &SDFMap::odomCallback, this); - cloud_sub_ = node_.subscribe("/laser_cloud_surround", 1, &SDFMap::cloudCallback, this); - - update_timer_ = node_.createTimer(ros::Duration(0.1), &SDFMap::updateCallback, this); - - inflate_cloud_pub_ = node_.advertise("/sdf_map/inflate_cloud", 1); - - /* ---------- setting ---------- */ - have_odom_ = false; - new_map_ = false; - map_valid_ = false; - - resolution_inv_ = 1 / resolution_sdf_; - for (int i = 0; i < 3; ++i) - grid_size_(i) = ceil(map_size_(i) / resolution_sdf_); - // SETY << "grid num:" << grid_size_.transpose() << REC; - min_range_ = origin_; - max_range_ = origin_ + map_size_; - min_vec_ = Eigen::Vector3i::Zero(); - max_vec_ = grid_size_ - Eigen::Vector3i::Ones(); - - // initialize size of buffer - occupancy_buffer_.resize(grid_size_(0) * grid_size_(1) * grid_size_(2)); - distance_buffer_.resize(grid_size_(0) * grid_size_(1) * grid_size_(2)); - distance_buffer_neg_.resize(grid_size_(0) * grid_size_(1) * grid_size_(2)); - tmp_buffer1_.resize(grid_size_(0) * grid_size_(1) * grid_size_(2)); - tmp_buffer2_.resize(grid_size_(0) * grid_size_(1) * grid_size_(2)); - - fill(distance_buffer_.begin(), distance_buffer_.end(), 10000); - fill(distance_buffer_neg_.begin(), distance_buffer_neg_.end(), 10000); - fill(occupancy_buffer_.begin(), occupancy_buffer_.end(), 0.0); -} - -void SDFMap::getInterpolationData(const Eigen::Vector3d& pos, vector& pos_vec, Eigen::Vector3d& diff) -{ - if (!isInMap(pos)) - { - // cout << "pos invalid for interpolation." << endl; - } - - /* interpolation position */ - Eigen::Vector3d pos_m = pos - 0.5 * resolution_sdf_ * Eigen::Vector3d::Ones(); - - Eigen::Vector3i idx; - posToIndex(pos_m, idx); - - Eigen::Vector3d idx_pos; - indexToPos(idx, idx_pos); - - diff = (pos - idx_pos) * resolution_inv_; - - pos_vec.clear(); - - for (int x = 0; x < 2; x++) - for (int y = 0; y < 2; y++) - for (int z = 0; z < 2; z++) - { - Eigen::Vector3i current_idx = idx + Eigen::Vector3i(x, y, z); - Eigen::Vector3d current_pos; - indexToPos(current_idx, current_pos); - pos_vec.push_back(current_pos); - } -} - -// SDFMap:: -} // namespace dyn_planner diff --git a/dyn_planner/plan_manage/CMakeLists.txt b/dyn_planner/plan_manage/CMakeLists.txt deleted file mode 100644 index 918efb5ec..000000000 --- a/dyn_planner/plan_manage/CMakeLists.txt +++ /dev/null @@ -1,64 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(plan_manage) - -set(CMAKE_BUILD_TYPE "Release") -set(CMAKE_CXX_FLAGS "-std=c++11") -set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") - -find_package(Eigen3 REQUIRED) -find_package(PCL 1.7 REQUIRED) - -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - geometry_msgs - quadrotor_msgs - plan_env - path_searching - bspline_opt - traj_utils - message_generation -) - -# Generate messages in the 'msg' folder -add_message_files( - FILES - Bspline.msg - ) - -# Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES plan_manage - CATKIN_DEPENDS plan_env path_searching bspline_opt traj_utils message_runtime -# DEPENDS system_lib -) - -include_directories( - include - SYSTEM - ${catkin_INCLUDE_DIRS} ${PROJECT_SOURCE_DIR}/include - ${EIGEN3_INCLUDE_DIR} - ${PCL_INCLUDE_DIRS} -) - - -add_executable(dyn_planner_node - src/dyn_planner_node.cpp - src/planning_fsm.cpp - src/dyn_planner_manager.cpp - ) -target_link_libraries(dyn_planner_node - ${catkin_LIBRARIES} - ) - -add_executable(traj_server -src/traj_server.cpp) -target_link_libraries(traj_server ${catkin_LIBRARIES}) -add_dependencies(traj_server ${${PROJECT_NAME}_EXPORTED_TARGETS}) \ No newline at end of file diff --git a/dyn_planner/plan_manage/README.md b/dyn_planner/plan_manage/README.md deleted file mode 100644 index 94e3b6a68..000000000 --- a/dyn_planner/plan_manage/README.md +++ /dev/null @@ -1,2 +0,0 @@ -# dyn_planner - diff --git a/dyn_planner/plan_manage/config/traj.rviz b/dyn_planner/plan_manage/config/traj.rviz deleted file mode 100644 index 5f7508c09..000000000 --- a/dyn_planner/plan_manage/config/traj.rviz +++ /dev/null @@ -1,551 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Mapping1/simulation_map1 - - /Tracking1 - Splitter Ratio: 0.5 - Tree Height: 435 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679016 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: simulation_map -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Class: rviz/Axes - Enabled: true - Length: 10 - Name: Axes - Radius: 0.100000001 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.0299999993 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 40 - Reference Frame: - Value: true - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /astar/visited - Name: visited node - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /hybridastar/path - Name: primitive - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /planning/state - Name: position_cmd - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /planning/path - Name: trajectory - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /planning/traj - Name: traj - Namespaces: - {} - Queue Size: 1000 - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud - Color: 255; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: start_goal - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.5 - Style: Spheres - Topic: /start_goal - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /filter/traj - Name: filter_traj - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Angle Tolerance: 0.00999999978 - Class: rviz/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.300000012 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Keep: 1 - Name: uart_odom - Position Tolerance: 0 - Shape: - Alpha: 1 - Axes Length: 0.300000012 - Axes Radius: 0.0500000007 - Color: 0; 255; 0 - Head Length: 0.300000012 - Head Radius: 0.100000001 - Shaft Length: 1 - Shaft Radius: 0.0500000007 - Value: Axes - Topic: /uart_odom/out_odom - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.100000001 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.300000012 - Head Radius: 0.100000001 - Name: opti_track - Shaft Length: 1 - Shaft Radius: 0.0500000007 - Shape: Arrow - Topic: /Robot_1/pose - Unreliable: false - Value: true - Enabled: true - Name: Estimation - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /sdf_map/occ - Name: my_occupancy - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: sdf_map/dist - Name: my_distance - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 0.200000003 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 4.90999985 - Min Value: -1.88999999 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: simulation_map - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.200000003 - Style: Boxes - Topic: /random_forest/all_map - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.75925088 - Min Value: -0.922850013 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 187; 187; 187 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 1 - Min Color: 0; 0; 0 - Min Intensity: 0.30000037 - Name: real_map - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.100000001 - Style: Boxes - Topic: /laser_cloud_surround - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 1.29999995 - Min Value: -0.922850013 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: inflate_map - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 10 - Size (m): 0.200000003 - Style: Boxes - Topic: /sdf_map/inflate_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization/robot - Name: robot - Namespaces: - mesh: true - Queue Size: 100 - Value: true - Enabled: true - Name: Simulation - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /dynamic/obj - Name: dynamic_obj - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Axes Length: 1.5 - Axes Radius: 0.200000003 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.300000012 - Head Radius: 0.100000001 - Name: obj_pose_0 - Shaft Length: 1 - Shaft Radius: 0.0500000007 - Shape: Axes - Topic: /dynamic/pose_0 - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 1.5 - Axes Radius: 0.200000003 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.300000012 - Head Radius: 0.100000001 - Name: obj_pose_1 - Shaft Length: 1 - Shaft Radius: 0.0500000007 - Shape: Axes - Topic: /dynamic/pose_1 - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 1.5 - Axes Radius: 0.200000003 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.300000012 - Head Radius: 0.100000001 - Name: obj_pose_2 - Shaft Length: 1 - Shaft Radius: 0.0500000007 - Shape: Axes - Topic: /dynamic/pose_2 - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 1.5 - Axes Radius: 0.200000003 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.300000012 - Head Radius: 0.100000001 - Name: obj_pose_3 - Shaft Length: 1 - Shaft Radius: 0.0500000007 - Shape: Axes - Topic: /dynamic/pose_3 - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 1.5 - Axes Radius: 0.200000003 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.300000012 - Head Radius: 0.100000001 - Name: obj_pose_4 - Shaft Length: 1 - Shaft Radius: 0.0500000007 - Shape: Axes - Topic: /dynamic/pose_4 - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 1.5 - Axes Radius: 0.200000003 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.300000012 - Head Radius: 0.100000001 - Name: obj_pose_5 - Shaft Length: 1 - Shaft Radius: 0.0500000007 - Shape: Axes - Topic: /dynamic/pose_5 - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 1.5 - Axes Radius: 0.200000003 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.300000012 - Head Radius: 0.100000001 - Name: obj_pose_6 - Shaft Length: 1 - Shaft Radius: 0.0500000007 - Shape: Axes - Topic: /dynamic/pose_6 - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 1.5 - Axes Radius: 0.200000003 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.300000012 - Head Radius: 0.100000001 - Name: obj_pose_7 - Shaft Length: 1 - Shaft Radius: 0.0500000007 - Shape: Axes - Topic: /dynamic/pose_7 - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 1.5 - Axes Radius: 0.200000003 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.300000012 - Head Radius: 0.100000001 - Name: obj_pose_8 - Shaft Length: 1 - Shaft Radius: 0.0500000007 - Shape: Axes - Topic: /dynamic/pose_8 - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 1.5 - Axes Radius: 0.200000003 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.300000012 - Head Radius: 0.100000001 - Name: obj_pose_9 - Shaft Length: 1 - Shaft Radius: 0.0500000007 - Shape: Axes - Topic: /dynamic/pose_9 - Unreliable: false - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /planning_vis/trajectory - Name: prediction - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: Tracking - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - - Class: rviz_plugins/Goal3DTool - Topic: goal - Value: true - Views: - Current: - Class: rviz/XYOrbit - Distance: 22.6529598 - Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.0439407825 - Y: -12.0625811 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 1.00039804 - Target Frame: - Value: XYOrbit (rviz) - Yaw: 4.70358562 - Saved: ~ -Window Geometry: - Displays: - collapsed: true - Height: 1056 - Hide Left Dock: true - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001710000037efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000028000001c5000000da00fffffffb0000000a0056006900650077007300000001f3000001b3000000b200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000001d1000000b50000000000000000fb0000000a0049006d00610067006502000001a2000001e1000000f8000000b5fb0000000a0049006d00610067006501000001d1000000b50000000000000000000000010000010f00000385fc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000035200fffffffb0000000800540069006d00650100000000000004500000000000000000000007800000039400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: 0 - Y: 24 diff --git a/dyn_planner/plan_manage/include/plan_manage/backward.hpp b/dyn_planner/plan_manage/include/plan_manage/backward.hpp deleted file mode 100644 index 53eea4fea..000000000 --- a/dyn_planner/plan_manage/include/plan_manage/backward.hpp +++ /dev/null @@ -1,3804 +0,0 @@ -/* - * backward.hpp - * Copyright 2013 Google Inc. All Rights Reserved. - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#ifndef H_6B9572DA_A64B_49E6_B234_051480991C89 -#define H_6B9572DA_A64B_49E6_B234_051480991C89 - -#ifndef __cplusplus -# error "It's not going to compile without a C++ compiler..." -#endif - -#if defined(BACKWARD_CXX11) -#elif defined(BACKWARD_CXX98) -#else -# if __cplusplus >= 201103L -# define BACKWARD_CXX11 -# define BACKWARD_ATLEAST_CXX11 -# define BACKWARD_ATLEAST_CXX98 -# else -# define BACKWARD_CXX98 -# define BACKWARD_ATLEAST_CXX98 -# endif -#endif - -// You can define one of the following (or leave it to the auto-detection): -// -// #define BACKWARD_SYSTEM_LINUX -// - specialization for linux -// -// #define BACKWARD_SYSTEM_DARWIN -// - specialization for Mac OS X 10.5 and later. -// -// #define BACKWARD_SYSTEM_UNKNOWN -// - placebo implementation, does nothing. -// -#if defined(BACKWARD_SYSTEM_LINUX) -#elif defined(BACKWARD_SYSTEM_DARWIN) -#elif defined(BACKWARD_SYSTEM_UNKNOWN) -#else -# if defined(__linux) || defined(__linux__) -# define BACKWARD_SYSTEM_LINUX -# elif defined(__APPLE__) -# define BACKWARD_SYSTEM_DARWIN -# else -# define BACKWARD_SYSTEM_UNKNOWN -# endif -#endif - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#if defined(BACKWARD_SYSTEM_LINUX) - -// On linux, backtrace can back-trace or "walk" the stack using the following -// libraries: -// -// #define BACKWARD_HAS_UNWIND 1 -// - unwind comes from libgcc, but I saw an equivalent inside clang itself. -// - with unwind, the stacktrace is as accurate as it can possibly be, since -// this is used by the C++ runtine in gcc/clang for stack unwinding on -// exception. -// - normally libgcc is already linked to your program by default. -// -// #define BACKWARD_HAS_BACKTRACE == 1 -// - backtrace seems to be a little bit more portable than libunwind, but on -// linux, it uses unwind anyway, but abstract away a tiny information that is -// sadly really important in order to get perfectly accurate stack traces. -// - backtrace is part of the (e)glib library. -// -// The default is: -// #define BACKWARD_HAS_UNWIND == 1 -// -// Note that only one of the define should be set to 1 at a time. -// -# if BACKWARD_HAS_UNWIND == 1 -# elif BACKWARD_HAS_BACKTRACE == 1 -# else -# undef BACKWARD_HAS_UNWIND -# define BACKWARD_HAS_UNWIND 1 -# undef BACKWARD_HAS_BACKTRACE -# define BACKWARD_HAS_BACKTRACE 0 -# endif - -// On linux, backward can extract detailed information about a stack trace -// using one of the following libraries: -// -// #define BACKWARD_HAS_DW 1 -// - libdw gives you the most juicy details out of your stack traces: -// - object filename -// - function name -// - source filename -// - line and column numbers -// - source code snippet (assuming the file is accessible) -// - variables name and values (if not optimized out) -// - You need to link with the lib "dw": -// - apt-get install libdw-dev -// - g++/clang++ -ldw ... -// -// #define BACKWARD_HAS_BFD 1 -// - With libbfd, you get a fair amount of details: -// - object filename -// - function name -// - source filename -// - line numbers -// - source code snippet (assuming the file is accessible) -// - You need to link with the lib "bfd": -// - apt-get install binutils-dev -// - g++/clang++ -lbfd ... -// -// #define BACKWARD_HAS_DWARF 1 -// - libdwarf gives you the most juicy details out of your stack traces: -// - object filename -// - function name -// - source filename -// - line and column numbers -// - source code snippet (assuming the file is accessible) -// - variables name and values (if not optimized out) -// - You need to link with the lib "dwarf": -// - apt-get install libdwarf-dev -// - g++/clang++ -ldwarf ... -// -// #define BACKWARD_HAS_BACKTRACE_SYMBOL 1 -// - backtrace provides minimal details for a stack trace: -// - object filename -// - function name -// - backtrace is part of the (e)glib library. -// -// The default is: -// #define BACKWARD_HAS_BACKTRACE_SYMBOL == 1 -// -// Note that only one of the define should be set to 1 at a time. -// -# if BACKWARD_HAS_DW == 1 -# elif BACKWARD_HAS_BFD == 1 -# elif BACKWARD_HAS_DWARF == 1 -# elif BACKWARD_HAS_BACKTRACE_SYMBOL == 1 -# else -# undef BACKWARD_HAS_DW -# define BACKWARD_HAS_DW 0 -# undef BACKWARD_HAS_BFD -# define BACKWARD_HAS_BFD 0 -# undef BACKWARD_HAS_DWARF -# define BACKWARD_HAS_DWARF 0 -# undef BACKWARD_HAS_BACKTRACE_SYMBOL -# define BACKWARD_HAS_BACKTRACE_SYMBOL 1 -# endif - -# include -# include -# ifdef __ANDROID__ -// Old Android API levels define _Unwind_Ptr in both link.h and unwind.h -// Rename the one in link.h as we are not going to be using it -# define _Unwind_Ptr _Unwind_Ptr_Custom -# include -# undef _Unwind_Ptr -# else -# include -# endif -# include -# include -# include -# include - -# if BACKWARD_HAS_BFD == 1 -// NOTE: defining PACKAGE{,_VERSION} is required before including -// bfd.h on some platforms, see also: -// https://sourceware.org/bugzilla/show_bug.cgi?id=14243 -# ifndef PACKAGE -# define PACKAGE -# endif -# ifndef PACKAGE_VERSION -# define PACKAGE_VERSION -# endif -# include -# ifndef _GNU_SOURCE -# define _GNU_SOURCE -# include -# undef _GNU_SOURCE -# else -# include -# endif -# endif - -# if BACKWARD_HAS_DW == 1 -# include -# include -# include -# endif - -# if BACKWARD_HAS_DWARF == 1 -# include -# include -# include -# include -# include -# ifndef _GNU_SOURCE -# define _GNU_SOURCE -# include -# undef _GNU_SOURCE -# else -# include -# endif -# endif - -# if (BACKWARD_HAS_BACKTRACE == 1) || (BACKWARD_HAS_BACKTRACE_SYMBOL == 1) - // then we shall rely on backtrace -# include -# endif - -#endif // defined(BACKWARD_SYSTEM_LINUX) - -#if defined(BACKWARD_SYSTEM_DARWIN) -// On Darwin, backtrace can back-trace or "walk" the stack using the following -// libraries: -// -// #define BACKWARD_HAS_UNWIND 1 -// - unwind comes from libgcc, but I saw an equivalent inside clang itself. -// - with unwind, the stacktrace is as accurate as it can possibly be, since -// this is used by the C++ runtine in gcc/clang for stack unwinding on -// exception. -// - normally libgcc is already linked to your program by default. -// -// #define BACKWARD_HAS_BACKTRACE == 1 -// - backtrace is available by default, though it does not produce as much information -// as another library might. -// -// The default is: -// #define BACKWARD_HAS_UNWIND == 1 -// -// Note that only one of the define should be set to 1 at a time. -// -# if BACKWARD_HAS_UNWIND == 1 -# elif BACKWARD_HAS_BACKTRACE == 1 -# else -# undef BACKWARD_HAS_UNWIND -# define BACKWARD_HAS_UNWIND 1 -# undef BACKWARD_HAS_BACKTRACE -# define BACKWARD_HAS_BACKTRACE 0 -# endif - -// On Darwin, backward can extract detailed information about a stack trace -// using one of the following libraries: -// -// #define BACKWARD_HAS_BACKTRACE_SYMBOL 1 -// - backtrace provides minimal details for a stack trace: -// - object filename -// - function name -// -// The default is: -// #define BACKWARD_HAS_BACKTRACE_SYMBOL == 1 -// -# if BACKWARD_HAS_BACKTRACE_SYMBOL == 1 -# else -# undef BACKWARD_HAS_BACKTRACE_SYMBOL -# define BACKWARD_HAS_BACKTRACE_SYMBOL 1 -# endif - -# include -# include -# include -# include -# include -# include - -# if (BACKWARD_HAS_BACKTRACE == 1) || (BACKWARD_HAS_BACKTRACE_SYMBOL == 1) -# include -# endif -#endif // defined(BACKWARD_SYSTEM_DARWIN) - -#if BACKWARD_HAS_UNWIND == 1 - -# include -// while gcc's unwind.h defines something like that: -// extern _Unwind_Ptr _Unwind_GetIP (struct _Unwind_Context *); -// extern _Unwind_Ptr _Unwind_GetIPInfo (struct _Unwind_Context *, int *); -// -// clang's unwind.h defines something like this: -// uintptr_t _Unwind_GetIP(struct _Unwind_Context* __context); -// -// Even if the _Unwind_GetIPInfo can be linked to, it is not declared, worse we -// cannot just redeclare it because clang's unwind.h doesn't define _Unwind_Ptr -// anyway. -// -// Luckily we can play on the fact that the guard macros have a different name: -#ifdef __CLANG_UNWIND_H -// In fact, this function still comes from libgcc (on my different linux boxes, -// clang links against libgcc). -# include -extern "C" uintptr_t _Unwind_GetIPInfo(_Unwind_Context*, int*); -#endif - -#endif // BACKWARD_HAS_UNWIND == 1 - -#ifdef BACKWARD_ATLEAST_CXX11 -# include -# include // for std::swap - namespace backward { - namespace details { - template - struct hashtable { - typedef std::unordered_map type; - }; - using std::move; - } // namespace details - } // namespace backward -#else // NOT BACKWARD_ATLEAST_CXX11 -# define nullptr NULL -# define override -# include - namespace backward { - namespace details { - template - struct hashtable { - typedef std::map type; - }; - template - const T& move(const T& v) { return v; } - template - T& move(T& v) { return v; } - } // namespace details - } // namespace backward -#endif // BACKWARD_ATLEAST_CXX11 - -namespace backward { - -namespace system_tag { - struct linux_tag; // seems that I cannot call that "linux" because the name - // is already defined... so I am adding _tag everywhere. - struct darwin_tag; - struct unknown_tag; - -#if defined(BACKWARD_SYSTEM_LINUX) - typedef linux_tag current_tag; -#elif defined(BACKWARD_SYSTEM_DARWIN) - typedef darwin_tag current_tag; -#elif defined(BACKWARD_SYSTEM_UNKNOWN) - typedef unknown_tag current_tag; -#else -# error "May I please get my system defines?" -#endif -} // namespace system_tag - - -namespace trace_resolver_tag { -#if defined(BACKWARD_SYSTEM_LINUX) - struct libdw; - struct libbfd; - struct libdwarf; - struct backtrace_symbol; - -# if BACKWARD_HAS_DW == 1 - typedef libdw current; -# elif BACKWARD_HAS_BFD == 1 - typedef libbfd current; -# elif BACKWARD_HAS_DWARF == 1 - typedef libdwarf current; -# elif BACKWARD_HAS_BACKTRACE_SYMBOL == 1 - typedef backtrace_symbol current; -# else -# error "You shall not pass, until you know what you want." -# endif -#elif defined(BACKWARD_SYSTEM_DARWIN) - struct backtrace_symbol; - -# if BACKWARD_HAS_BACKTRACE_SYMBOL == 1 - typedef backtrace_symbol current; -# else -# error "You shall not pass, until you know what you want." -# endif -#endif -} // namespace trace_resolver_tag - - -namespace details { - -template - struct rm_ptr { typedef T type; }; - -template - struct rm_ptr { typedef T type; }; - -template - struct rm_ptr { typedef const T type; }; - -template -struct deleter { - template - void operator()(U& ptr) const { - (*F)(ptr); - } -}; - -template -struct default_delete { - void operator()(T& ptr) const { - delete ptr; - } -}; - -template > -class handle { - struct dummy; - T _val; - bool _empty; - -#ifdef BACKWARD_ATLEAST_CXX11 - handle(const handle&) = delete; - handle& operator=(const handle&) = delete; -#endif - -public: - ~handle() { - if (!_empty) { - Deleter()(_val); - } - } - - explicit handle(): _val(), _empty(true) {} - explicit handle(T val): _val(val), _empty(false) { if(!_val) _empty = true; } - -#ifdef BACKWARD_ATLEAST_CXX11 - handle(handle&& from): _empty(true) { - swap(from); - } - handle& operator=(handle&& from) { - swap(from); return *this; - } -#else - explicit handle(const handle& from): _empty(true) { - // some sort of poor man's move semantic. - swap(const_cast(from)); - } - handle& operator=(const handle& from) { - // some sort of poor man's move semantic. - swap(const_cast(from)); return *this; - } -#endif - - void reset(T new_val) { - handle tmp(new_val); - swap(tmp); - } - operator const dummy*() const { - if (_empty) { - return nullptr; - } - return reinterpret_cast(_val); - } - T get() { - return _val; - } - T release() { - _empty = true; - return _val; - } - void swap(handle& b) { - using std::swap; - swap(b._val, _val); // can throw, we are safe here. - swap(b._empty, _empty); // should not throw: if you cannot swap two - // bools without throwing... It's a lost cause anyway! - } - - T operator->() { return _val; } - const T operator->() const { return _val; } - - typedef typename rm_ptr::type& ref_t; - typedef const typename rm_ptr::type& const_ref_t; - ref_t operator*() { return *_val; } - const_ref_t operator*() const { return *_val; } - ref_t operator[](size_t idx) { return _val[idx]; } - - // Watch out, we've got a badass over here - T* operator&() { - _empty = false; - return &_val; - } -}; - -// Default demangler implementation (do nothing). -template -struct demangler_impl { - static std::string demangle(const char* funcname) { - return funcname; - } -}; - -#if defined(BACKWARD_SYSTEM_LINUX) || defined(BACKWARD_SYSTEM_DARWIN) - -template <> -struct demangler_impl { - demangler_impl(): _demangle_buffer_length(0) {} - - std::string demangle(const char* funcname) { - using namespace details; - char* result = abi::__cxa_demangle(funcname, - _demangle_buffer.release(), &_demangle_buffer_length, nullptr); - if(result) { - _demangle_buffer.reset(result); - return result; - } - return funcname; - } - -private: - details::handle _demangle_buffer; - size_t _demangle_buffer_length; -}; - -#endif // BACKWARD_SYSTEM_LINUX || BACKWARD_SYSTEM_DARWIN - -struct demangler: - public demangler_impl {}; - -} // namespace details - -/*************** A TRACE ***************/ - -struct Trace { - void* addr; - size_t idx; - - Trace(): - addr(nullptr), idx(0) {} - - explicit Trace(void* _addr, size_t _idx): - addr(_addr), idx(_idx) {} -}; - -struct ResolvedTrace: public Trace { - - struct SourceLoc { - std::string function; - std::string filename; - unsigned line; - unsigned col; - - SourceLoc(): line(0), col(0) {} - - bool operator==(const SourceLoc& b) const { - return function == b.function - && filename == b.filename - && line == b.line - && col == b.col; - } - - bool operator!=(const SourceLoc& b) const { - return !(*this == b); - } - }; - - // In which binary object this trace is located. - std::string object_filename; - - // The function in the object that contain the trace. This is not the same - // as source.function which can be an function inlined in object_function. - std::string object_function; - - // The source location of this trace. It is possible for filename to be - // empty and for line/col to be invalid (value 0) if this information - // couldn't be deduced, for example if there is no debug information in the - // binary object. - SourceLoc source; - - // An optionals list of "inliners". All the successive sources location - // from where the source location of the trace (the attribute right above) - // is inlined. It is especially useful when you compiled with optimization. - typedef std::vector source_locs_t; - source_locs_t inliners; - - ResolvedTrace(): - Trace() {} - ResolvedTrace(const Trace& mini_trace): - Trace(mini_trace) {} -}; - -/*************** STACK TRACE ***************/ - -// default implemention. -template -class StackTraceImpl { -public: - size_t size() const { return 0; } - Trace operator[](size_t) { return Trace(); } - size_t load_here(size_t=0) { return 0; } - size_t load_from(void*, size_t=0) { return 0; } - size_t thread_id() const { return 0; } - void skip_n_firsts(size_t) { } -}; - -class StackTraceImplBase { -public: - StackTraceImplBase(): _thread_id(0), _skip(0) {} - - size_t thread_id() const { - return _thread_id; - } - - void skip_n_firsts(size_t n) { _skip = n; } - -protected: - void load_thread_info() { -#ifdef BACKWARD_SYSTEM_LINUX -#ifndef __ANDROID__ - _thread_id = static_cast(syscall(SYS_gettid)); -#else - _thread_id = static_cast(gettid()); -#endif - if (_thread_id == static_cast(getpid())) { - // If the thread is the main one, let's hide that. - // I like to keep little secret sometimes. - _thread_id = 0; - } -#elif defined(BACKWARD_SYSTEM_DARWIN) - _thread_id = reinterpret_cast(pthread_self()); - if (pthread_main_np() == 1) { - // If the thread is the main one, let's hide that. - _thread_id = 0; - } -#endif - } - - size_t skip_n_firsts() const { return _skip; } - -private: - size_t _thread_id; - size_t _skip; -}; - -class StackTraceImplHolder: public StackTraceImplBase { -public: - size_t size() const { - return _stacktrace.size() ? _stacktrace.size() - skip_n_firsts() : 0; - } - Trace operator[](size_t idx) const { - if (idx >= size()) { - return Trace(); - } - return Trace(_stacktrace[idx + skip_n_firsts()], idx); - } - void* const* begin() const { - if (size()) { - return &_stacktrace[skip_n_firsts()]; - } - return nullptr; - } - -protected: - std::vector _stacktrace; -}; - - -#if BACKWARD_HAS_UNWIND == 1 - -namespace details { - -template -class Unwinder { -public: - size_t operator()(F& f, size_t depth) { - _f = &f; - _index = -1; - _depth = depth; - _Unwind_Backtrace(&this->backtrace_trampoline, this); - return static_cast(_index); - } - -private: - F* _f; - ssize_t _index; - size_t _depth; - - static _Unwind_Reason_Code backtrace_trampoline( - _Unwind_Context* ctx, void *self) { - return (static_cast(self))->backtrace(ctx); - } - - _Unwind_Reason_Code backtrace(_Unwind_Context* ctx) { - if (_index >= 0 && static_cast(_index) >= _depth) - return _URC_END_OF_STACK; - - int ip_before_instruction = 0; - uintptr_t ip = _Unwind_GetIPInfo(ctx, &ip_before_instruction); - - if (!ip_before_instruction) { - // calculating 0-1 for unsigned, looks like a possible bug to sanitiziers, so let's do it explicitly: - if (ip==0) { - ip = std::numeric_limits::max(); // set it to 0xffff... (as from casting 0-1) - } else { - ip -= 1; // else just normally decrement it (no overflow/underflow will happen) - } - } - - if (_index >= 0) { // ignore first frame. - (*_f)(static_cast(_index), reinterpret_cast(ip)); - } - _index += 1; - return _URC_NO_REASON; - } -}; - -template -size_t unwind(F f, size_t depth) { - Unwinder unwinder; - return unwinder(f, depth); -} - -} // namespace details - - -template <> -class StackTraceImpl: public StackTraceImplHolder { -public: - __attribute__ ((noinline)) // TODO use some macro - size_t load_here(size_t depth=32) { - load_thread_info(); - if (depth == 0) { - return 0; - } - _stacktrace.resize(depth); - size_t trace_cnt = details::unwind(callback(*this), depth); - _stacktrace.resize(trace_cnt); - skip_n_firsts(0); - return size(); - } - size_t load_from(void* addr, size_t depth=32) { - load_here(depth + 8); - - for (size_t i = 0; i < _stacktrace.size(); ++i) { - if (_stacktrace[i] == addr) { - skip_n_firsts(i); - break; - } - } - - _stacktrace.resize(std::min(_stacktrace.size(), - skip_n_firsts() + depth)); - return size(); - } - -private: - struct callback { - StackTraceImpl& self; - callback(StackTraceImpl& _self): self(_self) {} - - void operator()(size_t idx, void* addr) { - self._stacktrace[idx] = addr; - } - }; -}; - - -#else // BACKWARD_HAS_UNWIND == 0 - -template <> -class StackTraceImpl: public StackTraceImplHolder { -public: - __attribute__ ((noinline)) // TODO use some macro - size_t load_here(size_t depth=32) { - load_thread_info(); - if (depth == 0) { - return 0; - } - _stacktrace.resize(depth + 1); - size_t trace_cnt = backtrace(&_stacktrace[0], _stacktrace.size()); - _stacktrace.resize(trace_cnt); - skip_n_firsts(1); - return size(); - } - - size_t load_from(void* addr, size_t depth=32) { - load_here(depth + 8); - - for (size_t i = 0; i < _stacktrace.size(); ++i) { - if (_stacktrace[i] == addr) { - skip_n_firsts(i); - _stacktrace[i] = (void*)( (uintptr_t)_stacktrace[i] + 1); - break; - } - } - - _stacktrace.resize(std::min(_stacktrace.size(), - skip_n_firsts() + depth)); - return size(); - } -}; - -#endif // BACKWARD_HAS_UNWIND - -class StackTrace: - public StackTraceImpl {}; - -/*************** TRACE RESOLVER ***************/ - -template -class TraceResolverImpl; - -#ifdef BACKWARD_SYSTEM_UNKNOWN - -template <> -class TraceResolverImpl { -public: - template - void load_stacktrace(ST&) {} - ResolvedTrace resolve(ResolvedTrace t) { - return t; - } -}; - -#endif - -class TraceResolverImplBase { -protected: - std::string demangle(const char* funcname) { - return _demangler.demangle(funcname); - } - -private: - details::demangler _demangler; -}; - -#ifdef BACKWARD_SYSTEM_LINUX - -template -class TraceResolverLinuxImpl; - -#if BACKWARD_HAS_BACKTRACE_SYMBOL == 1 - -template <> -class TraceResolverLinuxImpl: - public TraceResolverImplBase { -public: - template - void load_stacktrace(ST& st) { - using namespace details; - if (st.size() == 0) { - return; - } - _symbols.reset( - backtrace_symbols(st.begin(), (int)st.size()) - ); - } - - ResolvedTrace resolve(ResolvedTrace trace) { - char* filename = _symbols[trace.idx]; - char* funcname = filename; - while (*funcname && *funcname != '(') { - funcname += 1; - } - trace.object_filename.assign(filename, funcname); // ok even if funcname is the ending \0 (then we assign entire string) - - if (*funcname) { // if it's not end of string (e.g. from last frame ip==0) - funcname += 1; - char* funcname_end = funcname; - while (*funcname_end && *funcname_end != ')' && *funcname_end != '+') { - funcname_end += 1; - } - *funcname_end = '\0'; - trace.object_function = this->demangle(funcname); - trace.source.function = trace.object_function; // we cannot do better. - } - return trace; - } - -private: - details::handle _symbols; -}; - -#endif // BACKWARD_HAS_BACKTRACE_SYMBOL == 1 - -#if BACKWARD_HAS_BFD == 1 - -template <> -class TraceResolverLinuxImpl: - public TraceResolverImplBase { - static std::string read_symlink(std::string const & symlink_path) { - std::string path; - path.resize(100); - - while(true) { - ssize_t len = ::readlink(symlink_path.c_str(), &*path.begin(), path.size()); - if(len < 0) { - return ""; - } - if (static_cast(len) == path.size()) { - path.resize(path.size() * 2); - } - else { - path.resize(static_cast(len)); - break; - } - } - - return path; - } -public: - TraceResolverLinuxImpl(): _bfd_loaded(false) {} - - template - void load_stacktrace(ST&) {} - - ResolvedTrace resolve(ResolvedTrace trace) { - Dl_info symbol_info; - - // trace.addr is a virtual address in memory pointing to some code. - // Let's try to find from which loaded object it comes from. - // The loaded object can be yourself btw. - if (!dladdr(trace.addr, &symbol_info)) { - return trace; // dat broken trace... - } - - std::string argv0; - { - std::ifstream ifs("/proc/self/cmdline"); - std::getline(ifs, argv0, '\0'); - } - std::string tmp; - if(symbol_info.dli_fname == argv0) { - tmp = read_symlink("/proc/self/exe"); - symbol_info.dli_fname = tmp.c_str(); - } - - // Now we get in symbol_info: - // .dli_fname: - // pathname of the shared object that contains the address. - // .dli_fbase: - // where the object is loaded in memory. - // .dli_sname: - // the name of the nearest symbol to trace.addr, we expect a - // function name. - // .dli_saddr: - // the exact address corresponding to .dli_sname. - - if (symbol_info.dli_sname) { - trace.object_function = demangle(symbol_info.dli_sname); - } - - if (!symbol_info.dli_fname) { - return trace; - } - - trace.object_filename = symbol_info.dli_fname; - bfd_fileobject& fobj = load_object_with_bfd(symbol_info.dli_fname); - if (!fobj.handle) { - return trace; // sad, we couldn't load the object :( - } - - - find_sym_result* details_selected; // to be filled. - - // trace.addr is the next instruction to be executed after returning - // from the nested stack frame. In C++ this usually relate to the next - // statement right after the function call that leaded to a new stack - // frame. This is not usually what you want to see when printing out a - // stacktrace... - find_sym_result details_call_site = find_symbol_details(fobj, - trace.addr, symbol_info.dli_fbase); - details_selected = &details_call_site; - -#if BACKWARD_HAS_UNWIND == 0 - // ...this is why we also try to resolve the symbol that is right - // before the return address. If we are lucky enough, we will get the - // line of the function that was called. But if the code is optimized, - // we might get something absolutely not related since the compiler - // can reschedule the return address with inline functions and - // tail-call optimisation (among other things that I don't even know - // or cannot even dream about with my tiny limited brain). - find_sym_result details_adjusted_call_site = find_symbol_details(fobj, - (void*) (uintptr_t(trace.addr) - 1), - symbol_info.dli_fbase); - - // In debug mode, we should always get the right thing(TM). - if (details_call_site.found && details_adjusted_call_site.found) { - // Ok, we assume that details_adjusted_call_site is a better estimation. - details_selected = &details_adjusted_call_site; - trace.addr = (void*) (uintptr_t(trace.addr) - 1); - } - - if (details_selected == &details_call_site && details_call_site.found) { - // we have to re-resolve the symbol in order to reset some - // internal state in BFD... so we can call backtrace_inliners - // thereafter... - details_call_site = find_symbol_details(fobj, trace.addr, - symbol_info.dli_fbase); - } -#endif // BACKWARD_HAS_UNWIND - - if (details_selected->found) { - if (details_selected->filename) { - trace.source.filename = details_selected->filename; - } - trace.source.line = details_selected->line; - - if (details_selected->funcname) { - // this time we get the name of the function where the code is - // located, instead of the function were the address is - // located. In short, if the code was inlined, we get the - // function correspoding to the code. Else we already got in - // trace.function. - trace.source.function = demangle(details_selected->funcname); - - if (!symbol_info.dli_sname) { - // for the case dladdr failed to find the symbol name of - // the function, we might as well try to put something - // here. - trace.object_function = trace.source.function; - } - } - - // Maybe the source of the trace got inlined inside the function - // (trace.source.function). Let's see if we can get all the inlined - // calls along the way up to the initial call site. - trace.inliners = backtrace_inliners(fobj, *details_selected); - -#if 0 - if (trace.inliners.size() == 0) { - // Maybe the trace was not inlined... or maybe it was and we - // are lacking the debug information. Let's try to make the - // world better and see if we can get the line number of the - // function (trace.source.function) now. - // - // We will get the location of where the function start (to be - // exact: the first instruction that really start the - // function), not where the name of the function is defined. - // This can be quite far away from the name of the function - // btw. - // - // If the source of the function is the same as the source of - // the trace, we cannot say if the trace was really inlined or - // not. However, if the filename of the source is different - // between the function and the trace... we can declare it as - // an inliner. This is not 100% accurate, but better than - // nothing. - - if (symbol_info.dli_saddr) { - find_sym_result details = find_symbol_details(fobj, - symbol_info.dli_saddr, - symbol_info.dli_fbase); - - if (details.found) { - ResolvedTrace::SourceLoc diy_inliner; - diy_inliner.line = details.line; - if (details.filename) { - diy_inliner.filename = details.filename; - } - if (details.funcname) { - diy_inliner.function = demangle(details.funcname); - } else { - diy_inliner.function = trace.source.function; - } - if (diy_inliner != trace.source) { - trace.inliners.push_back(diy_inliner); - } - } - } - } -#endif - } - - return trace; - } - -private: - bool _bfd_loaded; - - typedef details::handle - > bfd_handle_t; - - typedef details::handle bfd_symtab_t; - - - struct bfd_fileobject { - bfd_handle_t handle; - bfd_vma base_addr; - bfd_symtab_t symtab; - bfd_symtab_t dynamic_symtab; - }; - - typedef details::hashtable::type - fobj_bfd_map_t; - fobj_bfd_map_t _fobj_bfd_map; - - bfd_fileobject& load_object_with_bfd(const std::string& filename_object) { - using namespace details; - - if (!_bfd_loaded) { - using namespace details; - bfd_init(); - _bfd_loaded = true; - } - - fobj_bfd_map_t::iterator it = - _fobj_bfd_map.find(filename_object); - if (it != _fobj_bfd_map.end()) { - return it->second; - } - - // this new object is empty for now. - bfd_fileobject& r = _fobj_bfd_map[filename_object]; - - // we do the work temporary in this one; - bfd_handle_t bfd_handle; - - int fd = open(filename_object.c_str(), O_RDONLY); - bfd_handle.reset( - bfd_fdopenr(filename_object.c_str(), "default", fd) - ); - if (!bfd_handle) { - close(fd); - return r; - } - - if (!bfd_check_format(bfd_handle.get(), bfd_object)) { - return r; // not an object? You lose. - } - - if ((bfd_get_file_flags(bfd_handle.get()) & HAS_SYMS) == 0) { - return r; // that's what happen when you forget to compile in debug. - } - - ssize_t symtab_storage_size = - bfd_get_symtab_upper_bound(bfd_handle.get()); - - ssize_t dyn_symtab_storage_size = - bfd_get_dynamic_symtab_upper_bound(bfd_handle.get()); - - if (symtab_storage_size <= 0 && dyn_symtab_storage_size <= 0) { - return r; // weird, is the file is corrupted? - } - - bfd_symtab_t symtab, dynamic_symtab; - ssize_t symcount = 0, dyn_symcount = 0; - - if (symtab_storage_size > 0) { - symtab.reset( - static_cast(malloc(static_cast(symtab_storage_size))) - ); - symcount = bfd_canonicalize_symtab( - bfd_handle.get(), symtab.get() - ); - } - - if (dyn_symtab_storage_size > 0) { - dynamic_symtab.reset( - static_cast(malloc(static_cast(dyn_symtab_storage_size))) - ); - dyn_symcount = bfd_canonicalize_dynamic_symtab( - bfd_handle.get(), dynamic_symtab.get() - ); - } - - - if (symcount <= 0 && dyn_symcount <= 0) { - return r; // damned, that's a stripped file that you got there! - } - - r.handle = move(bfd_handle); - r.symtab = move(symtab); - r.dynamic_symtab = move(dynamic_symtab); - return r; - } - - struct find_sym_result { - bool found; - const char* filename; - const char* funcname; - unsigned int line; - }; - - struct find_sym_context { - TraceResolverLinuxImpl* self; - bfd_fileobject* fobj; - void* addr; - void* base_addr; - find_sym_result result; - }; - - find_sym_result find_symbol_details(bfd_fileobject& fobj, void* addr, - void* base_addr) { - find_sym_context context; - context.self = this; - context.fobj = &fobj; - context.addr = addr; - context.base_addr = base_addr; - context.result.found = false; - bfd_map_over_sections(fobj.handle.get(), &find_in_section_trampoline, - static_cast(&context)); - return context.result; - } - - static void find_in_section_trampoline(bfd*, asection* section, - void* data) { - find_sym_context* context = static_cast(data); - context->self->find_in_section( - reinterpret_cast(context->addr), - reinterpret_cast(context->base_addr), - *context->fobj, - section, context->result - ); - } - - void find_in_section(bfd_vma addr, bfd_vma base_addr, - bfd_fileobject& fobj, asection* section, find_sym_result& result) - { - if (result.found) return; - - if ((bfd_get_section_flags(fobj.handle.get(), section) - & SEC_ALLOC) == 0) - return; // a debug section is never loaded automatically. - - bfd_vma sec_addr = bfd_get_section_vma(fobj.handle.get(), section); - bfd_size_type size = bfd_get_section_size(section); - - // are we in the boundaries of the section? - if (addr < sec_addr || addr >= sec_addr + size) { - addr -= base_addr; // oups, a relocated object, lets try again... - if (addr < sec_addr || addr >= sec_addr + size) { - return; - } - } - -#if defined(__clang__) -#pragma clang diagnostic push -#pragma clang diagnostic ignored "-Wzero-as-null-pointer-constant" -#endif - if (!result.found && fobj.symtab) { - result.found = bfd_find_nearest_line(fobj.handle.get(), section, - fobj.symtab.get(), addr - sec_addr, &result.filename, - &result.funcname, &result.line); - } - - if (!result.found && fobj.dynamic_symtab) { - result.found = bfd_find_nearest_line(fobj.handle.get(), section, - fobj.dynamic_symtab.get(), addr - sec_addr, - &result.filename, &result.funcname, &result.line); - } -#if defined(__clang__) -#pragma clang diagnostic pop -#endif - - } - - ResolvedTrace::source_locs_t backtrace_inliners(bfd_fileobject& fobj, - find_sym_result previous_result) { - // This function can be called ONLY after a SUCCESSFUL call to - // find_symbol_details. The state is global to the bfd_handle. - ResolvedTrace::source_locs_t results; - while (previous_result.found) { - find_sym_result result; - result.found = bfd_find_inliner_info(fobj.handle.get(), - &result.filename, &result.funcname, &result.line); - - if (result.found) /* and not ( - cstrings_eq(previous_result.filename, result.filename) - and cstrings_eq(previous_result.funcname, result.funcname) - and result.line == previous_result.line - )) */ { - ResolvedTrace::SourceLoc src_loc; - src_loc.line = result.line; - if (result.filename) { - src_loc.filename = result.filename; - } - if (result.funcname) { - src_loc.function = demangle(result.funcname); - } - results.push_back(src_loc); - } - previous_result = result; - } - return results; - } - - bool cstrings_eq(const char* a, const char* b) { - if (!a || !b) { - return false; - } - return strcmp(a, b) == 0; - } - -}; -#endif // BACKWARD_HAS_BFD == 1 - -#if BACKWARD_HAS_DW == 1 - -template <> -class TraceResolverLinuxImpl: - public TraceResolverImplBase { -public: - TraceResolverLinuxImpl(): _dwfl_handle_initialized(false) {} - - template - void load_stacktrace(ST&) {} - - ResolvedTrace resolve(ResolvedTrace trace) { - using namespace details; - - Dwarf_Addr trace_addr = (Dwarf_Addr) trace.addr; - - if (!_dwfl_handle_initialized) { - // initialize dwfl... - _dwfl_cb.reset(new Dwfl_Callbacks); - _dwfl_cb->find_elf = &dwfl_linux_proc_find_elf; - _dwfl_cb->find_debuginfo = &dwfl_standard_find_debuginfo; - _dwfl_cb->debuginfo_path = 0; - - _dwfl_handle.reset(dwfl_begin(_dwfl_cb.get())); - _dwfl_handle_initialized = true; - - if (!_dwfl_handle) { - return trace; - } - - // ...from the current process. - dwfl_report_begin(_dwfl_handle.get()); - int r = dwfl_linux_proc_report (_dwfl_handle.get(), getpid()); - dwfl_report_end(_dwfl_handle.get(), NULL, NULL); - if (r < 0) { - return trace; - } - } - - if (!_dwfl_handle) { - return trace; - } - - // find the module (binary object) that contains the trace's address. - // This is not using any debug information, but the addresses ranges of - // all the currently loaded binary object. - Dwfl_Module* mod = dwfl_addrmodule(_dwfl_handle.get(), trace_addr); - if (mod) { - // now that we found it, lets get the name of it, this will be the - // full path to the running binary or one of the loaded library. - const char* module_name = dwfl_module_info (mod, - 0, 0, 0, 0, 0, 0, 0); - if (module_name) { - trace.object_filename = module_name; - } - // We also look after the name of the symbol, equal or before this - // address. This is found by walking the symtab. We should get the - // symbol corresponding to the function (mangled) containing the - // address. If the code corresponding to the address was inlined, - // this is the name of the out-most inliner function. - const char* sym_name = dwfl_module_addrname(mod, trace_addr); - if (sym_name) { - trace.object_function = demangle(sym_name); - } - } - - // now let's get serious, and find out the source location (file and - // line number) of the address. - - // This function will look in .debug_aranges for the address and map it - // to the location of the compilation unit DIE in .debug_info and - // return it. - Dwarf_Addr mod_bias = 0; - Dwarf_Die* cudie = dwfl_module_addrdie(mod, trace_addr, &mod_bias); - -#if 1 - if (!cudie) { - // Sadly clang does not generate the section .debug_aranges, thus - // dwfl_module_addrdie will fail early. Clang doesn't either set - // the lowpc/highpc/range info for every compilation unit. - // - // So in order to save the world: - // for every compilation unit, we will iterate over every single - // DIEs. Normally functions should have a lowpc/highpc/range, which - // we will use to infer the compilation unit. - - // note that this is probably badly inefficient. - while ((cudie = dwfl_module_nextcu(mod, cudie, &mod_bias))) { - Dwarf_Die die_mem; - Dwarf_Die* fundie = find_fundie_by_pc(cudie, - trace_addr - mod_bias, &die_mem); - if (fundie) { - break; - } - } - } -#endif - -//#define BACKWARD_I_DO_NOT_RECOMMEND_TO_ENABLE_THIS_HORRIBLE_PIECE_OF_CODE -#ifdef BACKWARD_I_DO_NOT_RECOMMEND_TO_ENABLE_THIS_HORRIBLE_PIECE_OF_CODE - if (!cudie) { - // If it's still not enough, lets dive deeper in the shit, and try - // to save the world again: for every compilation unit, we will - // load the corresponding .debug_line section, and see if we can - // find our address in it. - - Dwarf_Addr cfi_bias; - Dwarf_CFI* cfi_cache = dwfl_module_eh_cfi(mod, &cfi_bias); - - Dwarf_Addr bias; - while ((cudie = dwfl_module_nextcu(mod, cudie, &bias))) { - if (dwarf_getsrc_die(cudie, trace_addr - bias)) { - - // ...but if we get a match, it might be a false positive - // because our (address - bias) might as well be valid in a - // different compilation unit. So we throw our last card on - // the table and lookup for the address into the .eh_frame - // section. - - handle frame; - dwarf_cfi_addrframe(cfi_cache, trace_addr - cfi_bias, &frame); - if (frame) { - break; - } - } - } - } -#endif - - if (!cudie) { - return trace; // this time we lost the game :/ - } - - // Now that we have a compilation unit DIE, this function will be able - // to load the corresponding section in .debug_line (if not already - // loaded) and hopefully find the source location mapped to our - // address. - Dwarf_Line* srcloc = dwarf_getsrc_die(cudie, trace_addr - mod_bias); - - if (srcloc) { - const char* srcfile = dwarf_linesrc(srcloc, 0, 0); - if (srcfile) { - trace.source.filename = srcfile; - } - int line = 0, col = 0; - dwarf_lineno(srcloc, &line); - dwarf_linecol(srcloc, &col); - trace.source.line = line; - trace.source.col = col; - } - - deep_first_search_by_pc(cudie, trace_addr - mod_bias, - inliners_search_cb(trace)); - if (trace.source.function.size() == 0) { - // fallback. - trace.source.function = trace.object_function; - } - - return trace; - } - -private: - typedef details::handle > - dwfl_handle_t; - details::handle > - _dwfl_cb; - dwfl_handle_t _dwfl_handle; - bool _dwfl_handle_initialized; - - // defined here because in C++98, template function cannot take locally - // defined types... grrr. - struct inliners_search_cb { - void operator()(Dwarf_Die* die) { - switch (dwarf_tag(die)) { - const char* name; - case DW_TAG_subprogram: - if ((name = dwarf_diename(die))) { - trace.source.function = name; - } - break; - - case DW_TAG_inlined_subroutine: - ResolvedTrace::SourceLoc sloc; - Dwarf_Attribute attr_mem; - - if ((name = dwarf_diename(die))) { - sloc.function = name; - } - if ((name = die_call_file(die))) { - sloc.filename = name; - } - - Dwarf_Word line = 0, col = 0; - dwarf_formudata(dwarf_attr(die, DW_AT_call_line, - &attr_mem), &line); - dwarf_formudata(dwarf_attr(die, DW_AT_call_column, - &attr_mem), &col); - sloc.line = (unsigned)line; - sloc.col = (unsigned)col; - - trace.inliners.push_back(sloc); - break; - }; - } - ResolvedTrace& trace; - inliners_search_cb(ResolvedTrace& t): trace(t) {} - }; - - - static bool die_has_pc(Dwarf_Die* die, Dwarf_Addr pc) { - Dwarf_Addr low, high; - - // continuous range - if (dwarf_hasattr(die, DW_AT_low_pc) && - dwarf_hasattr(die, DW_AT_high_pc)) { - if (dwarf_lowpc(die, &low) != 0) { - return false; - } - if (dwarf_highpc(die, &high) != 0) { - Dwarf_Attribute attr_mem; - Dwarf_Attribute* attr = dwarf_attr(die, DW_AT_high_pc, &attr_mem); - Dwarf_Word value; - if (dwarf_formudata(attr, &value) != 0) { - return false; - } - high = low + value; - } - return pc >= low && pc < high; - } - - // non-continuous range. - Dwarf_Addr base; - ptrdiff_t offset = 0; - while ((offset = dwarf_ranges(die, offset, &base, &low, &high)) > 0) { - if (pc >= low && pc < high) { - return true; - } - } - return false; - } - - static Dwarf_Die* find_fundie_by_pc(Dwarf_Die* parent_die, Dwarf_Addr pc, - Dwarf_Die* result) { - if (dwarf_child(parent_die, result) != 0) { - return 0; - } - - Dwarf_Die* die = result; - do { - switch (dwarf_tag(die)) { - case DW_TAG_subprogram: - case DW_TAG_inlined_subroutine: - if (die_has_pc(die, pc)) { - return result; - } - }; - bool declaration = false; - Dwarf_Attribute attr_mem; - dwarf_formflag(dwarf_attr(die, DW_AT_declaration, - &attr_mem), &declaration); - if (!declaration) { - // let's be curious and look deeper in the tree, - // function are not necessarily at the first level, but - // might be nested inside a namespace, structure etc. - Dwarf_Die die_mem; - Dwarf_Die* indie = find_fundie_by_pc(die, pc, &die_mem); - if (indie) { - *result = die_mem; - return result; - } - } - } while (dwarf_siblingof(die, result) == 0); - return 0; - } - - template - static bool deep_first_search_by_pc(Dwarf_Die* parent_die, - Dwarf_Addr pc, CB cb) { - Dwarf_Die die_mem; - if (dwarf_child(parent_die, &die_mem) != 0) { - return false; - } - - bool branch_has_pc = false; - Dwarf_Die* die = &die_mem; - do { - bool declaration = false; - Dwarf_Attribute attr_mem; - dwarf_formflag(dwarf_attr(die, DW_AT_declaration, &attr_mem), &declaration); - if (!declaration) { - // let's be curious and look deeper in the tree, function are - // not necessarily at the first level, but might be nested - // inside a namespace, structure, a function, an inlined - // function etc. - branch_has_pc = deep_first_search_by_pc(die, pc, cb); - } - if (!branch_has_pc) { - branch_has_pc = die_has_pc(die, pc); - } - if (branch_has_pc) { - cb(die); - } - } while (dwarf_siblingof(die, &die_mem) == 0); - return branch_has_pc; - } - - static const char* die_call_file(Dwarf_Die *die) { - Dwarf_Attribute attr_mem; - Dwarf_Sword file_idx = 0; - - dwarf_formsdata(dwarf_attr(die, DW_AT_call_file, &attr_mem), - &file_idx); - - if (file_idx == 0) { - return 0; - } - - Dwarf_Die die_mem; - Dwarf_Die* cudie = dwarf_diecu(die, &die_mem, 0, 0); - if (!cudie) { - return 0; - } - - Dwarf_Files* files = 0; - size_t nfiles; - dwarf_getsrcfiles(cudie, &files, &nfiles); - if (!files) { - return 0; - } - - return dwarf_filesrc(files, file_idx, 0, 0); - } - -}; -#endif // BACKWARD_HAS_DW == 1 - -#if BACKWARD_HAS_DWARF == 1 - -template <> -class TraceResolverLinuxImpl: - public TraceResolverImplBase { - static std::string read_symlink(std::string const & symlink_path) { - std::string path; - path.resize(100); - - while(true) { - ssize_t len = ::readlink(symlink_path.c_str(), - &*path.begin(), path.size()); - if(len < 0) { - return ""; - } - if ((size_t)len == path.size()) { - path.resize(path.size() * 2); - } - else { - path.resize(len); - break; - } - } - - return path; - } -public: - TraceResolverLinuxImpl(): _dwarf_loaded(false) {} - - template - void load_stacktrace(ST&) {} - - ResolvedTrace resolve(ResolvedTrace trace) { - // trace.addr is a virtual address in memory pointing to some code. - // Let's try to find from which loaded object it comes from. - // The loaded object can be yourself btw. - - Dl_info symbol_info; - int dladdr_result = 0; -#ifndef __ANDROID__ - link_map *link_map; - // We request the link map so we can get information about offsets - dladdr_result = dladdr1(trace.addr, &symbol_info, - reinterpret_cast(&link_map), RTLD_DL_LINKMAP); -#else - // Android doesn't have dladdr1. Don't use the linker map. - dladdr_result = dladdr(trace.addr, &symbol_info); -#endif - if (!dladdr_result) { - return trace; // dat broken trace... - } - - std::string argv0; - { - std::ifstream ifs("/proc/self/cmdline"); - std::getline(ifs, argv0, '\0'); - } - std::string tmp; - if(symbol_info.dli_fname == argv0) { - tmp = read_symlink("/proc/self/exe"); - symbol_info.dli_fname = tmp.c_str(); - } - - // Now we get in symbol_info: - // .dli_fname: - // pathname of the shared object that contains the address. - // .dli_fbase: - // where the object is loaded in memory. - // .dli_sname: - // the name of the nearest symbol to trace.addr, we expect a - // function name. - // .dli_saddr: - // the exact address corresponding to .dli_sname. - // - // And in link_map: - // .l_addr: - // difference between the address in the ELF file and the address - // in memory - // l_name: - // absolute pathname where the object was found - - if (symbol_info.dli_sname) { - trace.object_function = demangle(symbol_info.dli_sname); - } - - if (!symbol_info.dli_fname) { - return trace; - } - - trace.object_filename = symbol_info.dli_fname; - dwarf_fileobject& fobj = load_object_with_dwarf(symbol_info.dli_fname); - if (!fobj.dwarf_handle) { - return trace; // sad, we couldn't load the object :( - } - -#ifndef __ANDROID__ - // Convert the address to a module relative one by looking at - // the module's loading address in the link map - Dwarf_Addr address = reinterpret_cast(trace.addr) - - reinterpret_cast(link_map->l_addr); -#else - Dwarf_Addr address = reinterpret_cast(trace.addr); -#endif - - if (trace.object_function.empty()) { - symbol_cache_t::iterator it = - fobj.symbol_cache.lower_bound(address); - - if (it != fobj.symbol_cache.end()) { - if (it->first != address) { - if (it != fobj.symbol_cache.begin()) { - --it; - } - } - trace.object_function = demangle(it->second.c_str()); - } - } - - // Get the Compilation Unit DIE for the address - Dwarf_Die die = find_die(fobj, address); - - if (!die) { - return trace; // this time we lost the game :/ - } - - // libdwarf doesn't give us direct access to its objects, it always - // allocates a copy for the caller. We keep that copy alive in a cache - // and we deallocate it later when it's no longer required. - die_cache_entry& die_object = get_die_cache(fobj, die); - if (die_object.isEmpty()) - return trace; // We have no line section for this DIE - - die_linemap_t::iterator it = - die_object.line_section.lower_bound(address); - - if (it != die_object.line_section.end()) { - if (it->first != address) { - if (it == die_object.line_section.begin()) { - // If we are on the first item of the line section - // but the address does not match it means that - // the address is below the range of the DIE. Give up. - return trace; - } else { - --it; - } - } - } else { - return trace; // We didn't find the address. - } - - // Get the Dwarf_Line that the address points to and call libdwarf - // to get source file, line and column info. - Dwarf_Line line = die_object.line_buffer[it->second]; - Dwarf_Error error = DW_DLE_NE; - - char* filename; - if (dwarf_linesrc(line, &filename, &error) - == DW_DLV_OK) { - trace.source.filename = std::string(filename); - dwarf_dealloc(fobj.dwarf_handle.get(), filename, DW_DLA_STRING); - } - - Dwarf_Unsigned number = 0; - if (dwarf_lineno(line, &number, &error) == DW_DLV_OK) { - trace.source.line = number; - } else { - trace.source.line = 0; - } - - if (dwarf_lineoff_b(line, &number, &error) == DW_DLV_OK) { - trace.source.col = number; - } else { - trace.source.col = 0; - } - - std::vector namespace_stack; - deep_first_search_by_pc(fobj, die, address, namespace_stack, - inliners_search_cb(trace, fobj, die)); - - dwarf_dealloc(fobj.dwarf_handle.get(), die, DW_DLA_DIE); - - return trace; - } - -public: - static int close_dwarf(Dwarf_Debug dwarf) { - return dwarf_finish(dwarf, NULL); - } - -private: - bool _dwarf_loaded; - - typedef details::handle - > dwarf_file_t; - - typedef details::handle - > dwarf_elf_t; - - typedef details::handle - > dwarf_handle_t; - - typedef std::map die_linemap_t; - - typedef std::map die_specmap_t; - - struct die_cache_entry { - die_specmap_t spec_section; - die_linemap_t line_section; - Dwarf_Line* line_buffer; - Dwarf_Signed line_count; - Dwarf_Line_Context line_context; - - inline bool isEmpty() { - return line_buffer == NULL || - line_count == 0 || - line_context == NULL || - line_section.empty(); - } - - die_cache_entry() : - line_buffer(0), line_count(0), line_context(0) {} - - ~die_cache_entry() - { - if (line_context) { - dwarf_srclines_dealloc_b(line_context); - } - } - }; - - typedef std::map die_cache_t; - - typedef std::map symbol_cache_t; - - struct dwarf_fileobject { - dwarf_file_t file_handle; - dwarf_elf_t elf_handle; - dwarf_handle_t dwarf_handle; - symbol_cache_t symbol_cache; - - // Die cache - die_cache_t die_cache; - die_cache_entry* current_cu; - }; - - typedef details::hashtable::type - fobj_dwarf_map_t; - fobj_dwarf_map_t _fobj_dwarf_map; - - static bool cstrings_eq(const char* a, const char* b) { - if (!a || !b) { - return false; - } - return strcmp(a, b) == 0; - } - - dwarf_fileobject& load_object_with_dwarf( - const std::string& filename_object) { - - if (!_dwarf_loaded) { - // Set the ELF library operating version - // If that fails there's nothing we can do - _dwarf_loaded = elf_version(EV_CURRENT) != EV_NONE; - } - - fobj_dwarf_map_t::iterator it = - _fobj_dwarf_map.find(filename_object); - if (it != _fobj_dwarf_map.end()) { - return it->second; - } - - // this new object is empty for now - dwarf_fileobject& r = _fobj_dwarf_map[filename_object]; - - dwarf_file_t file_handle; - file_handle.reset(open(filename_object.c_str(), O_RDONLY)); - if (file_handle < 0) { - return r; - } - - // Try to get an ELF handle. We need to read the ELF sections - // because we want to see if there is a .gnu_debuglink section - // that points to a split debug file - dwarf_elf_t elf_handle; - elf_handle.reset(elf_begin(file_handle.get(), ELF_C_READ, NULL)); - if (!elf_handle) { - return r; - } - - const char* e_ident = elf_getident(elf_handle.get(), 0); - if (!e_ident) { - return r; - } - - // Get the number of sections - // We use the new APIs as elf_getshnum is deprecated - size_t shdrnum = 0; - if (elf_getshdrnum(elf_handle.get(), &shdrnum) == -1) { - return r; - } - - // Get the index to the string section - size_t shdrstrndx = 0; - if (elf_getshdrstrndx (elf_handle.get(), &shdrstrndx) == -1) { - return r; - } - - std::string debuglink; - // Iterate through the ELF sections to try to get a gnu_debuglink - // note and also to cache the symbol table. - // We go the preprocessor way to avoid having to create templated - // classes or using gelf (which might throw a compiler error if 64 bit - // is not supported -#define ELF_GET_DATA(ARCH) \ - Elf_Scn *elf_section = 0; \ - Elf_Data *elf_data = 0; \ - Elf##ARCH##_Shdr* section_header = 0; \ - Elf_Scn *symbol_section = 0; \ - size_t symbol_count = 0; \ - size_t symbol_strings = 0; \ - Elf##ARCH##_Sym *symbol = 0; \ - const char* section_name = 0; \ - \ - while ((elf_section = elf_nextscn(elf_handle.get(), elf_section)) \ - != NULL) { \ - section_header = elf##ARCH##_getshdr(elf_section); \ - if (section_header == NULL) { \ - return r; \ - } \ - \ - if ((section_name = elf_strptr( \ - elf_handle.get(), shdrstrndx, \ - section_header->sh_name)) == NULL) { \ - return r; \ - } \ - \ - if (cstrings_eq(section_name, ".gnu_debuglink")) { \ - elf_data = elf_getdata(elf_section, NULL); \ - if (elf_data && elf_data->d_size > 0) { \ - debuglink = std::string( \ - reinterpret_cast(elf_data->d_buf)); \ - } \ - } \ - \ - switch(section_header->sh_type) { \ - case SHT_SYMTAB: \ - symbol_section = elf_section; \ - symbol_count = section_header->sh_size / \ - section_header->sh_entsize; \ - symbol_strings = section_header->sh_link; \ - break; \ - \ - /* We use .dynsyms as a last resort, we prefer .symtab */ \ - case SHT_DYNSYM: \ - if (!symbol_section) { \ - symbol_section = elf_section; \ - symbol_count = section_header->sh_size / \ - section_header->sh_entsize; \ - symbol_strings = section_header->sh_link; \ - } \ - break; \ - } \ - } \ - \ - if (symbol_section && symbol_count && symbol_strings) { \ - elf_data = elf_getdata(symbol_section, NULL); \ - symbol = reinterpret_cast(elf_data->d_buf); \ - for (size_t i = 0; i < symbol_count; ++i) { \ - int type = ELF##ARCH##_ST_TYPE(symbol->st_info); \ - if (type == STT_FUNC && symbol->st_value > 0) { \ - r.symbol_cache[symbol->st_value] = std::string( \ - elf_strptr(elf_handle.get(), \ - symbol_strings, symbol->st_name)); \ - } \ - ++symbol; \ - } \ - } \ - - - if (e_ident[EI_CLASS] == ELFCLASS32) { - ELF_GET_DATA(32) - } else if (e_ident[EI_CLASS] == ELFCLASS64) { - // libelf might have been built without 64 bit support -#if __LIBELF64 - ELF_GET_DATA(64) -#endif - } - - if (!debuglink.empty()) { - // We have a debuglink section! Open an elf instance on that - // file instead. If we can't open the file, then return - // the elf handle we had already opened. - dwarf_file_t debuglink_file; - debuglink_file.reset(open(debuglink.c_str(), O_RDONLY)); - if (debuglink_file.get() > 0) { - dwarf_elf_t debuglink_elf; - debuglink_elf.reset( - elf_begin(debuglink_file.get(),ELF_C_READ, NULL) - ); - - // If we have a valid elf handle, return the new elf handle - // and file handle and discard the original ones - if (debuglink_elf) { - elf_handle = move(debuglink_elf); - file_handle = move(debuglink_file); - } - } - } - - // Ok, we have a valid ELF handle, let's try to get debug symbols - Dwarf_Debug dwarf_debug; - Dwarf_Error error = DW_DLE_NE; - dwarf_handle_t dwarf_handle; - - int dwarf_result = dwarf_elf_init(elf_handle.get(), - DW_DLC_READ, NULL, NULL, &dwarf_debug, &error); - - // We don't do any special handling for DW_DLV_NO_ENTRY specially. - // If we get an error, or the file doesn't have debug information - // we just return. - if (dwarf_result != DW_DLV_OK) { - return r; - } - - dwarf_handle.reset(dwarf_debug); - - r.file_handle = move(file_handle); - r.elf_handle = move(elf_handle); - r.dwarf_handle = move(dwarf_handle); - - return r; - } - - die_cache_entry& get_die_cache(dwarf_fileobject& fobj, Dwarf_Die die) - { - Dwarf_Error error = DW_DLE_NE; - - // Get the die offset, we use it as the cache key - Dwarf_Off die_offset; - if (dwarf_dieoffset(die, &die_offset, &error) != DW_DLV_OK) { - die_offset = 0; - } - - die_cache_t::iterator it = fobj.die_cache.find(die_offset); - - if (it != fobj.die_cache.end()) { - fobj.current_cu = &it->second; - return it->second; - } - - die_cache_entry& de = fobj.die_cache[die_offset]; - fobj.current_cu = &de; - - Dwarf_Addr line_addr; - Dwarf_Small table_count; - - // The addresses in the line section are not fully sorted (they might - // be sorted by block of code belonging to the same file), which makes - // it necessary to do so before searching is possible. - // - // As libdwarf allocates a copy of everything, let's get the contents - // of the line section and keep it around. We also create a map of - // program counter to line table indices so we can search by address - // and get the line buffer index. - // - // To make things more difficult, the same address can span more than - // one line, so we need to keep the index pointing to the first line - // by using insert instead of the map's [ operator. - - // Get the line context for the DIE - if (dwarf_srclines_b(die, 0, &table_count, &de.line_context, &error) - == DW_DLV_OK) { - // Get the source lines for this line context, to be deallocated - // later - if (dwarf_srclines_from_linecontext( - de.line_context, &de.line_buffer, &de.line_count, &error) - == DW_DLV_OK) { - - // Add all the addresses to our map - for (int i = 0; i < de.line_count; i++) { - if (dwarf_lineaddr(de.line_buffer[i], &line_addr, &error) - != DW_DLV_OK) { - line_addr = 0; - } - de.line_section.insert( - std::pair(line_addr, i)); - } - } - } - - // For each CU, cache the function DIEs that contain the - // DW_AT_specification attribute. When building with -g3 the function - // DIEs are separated in declaration and specification, with the - // declaration containing only the name and parameters and the - // specification the low/high pc and other compiler attributes. - // - // We cache those specifications so we don't skip over the declarations, - // because they have no pc, and we can do namespace resolution for - // DWARF function names. - Dwarf_Debug dwarf = fobj.dwarf_handle.get(); - Dwarf_Die current_die = 0; - if (dwarf_child(die, ¤t_die, &error) == DW_DLV_OK) { - for(;;) { - Dwarf_Die sibling_die = 0; - - Dwarf_Half tag_value; - dwarf_tag(current_die, &tag_value, &error); - - if (tag_value == DW_TAG_subprogram || - tag_value == DW_TAG_inlined_subroutine) { - - Dwarf_Bool has_attr = 0; - if (dwarf_hasattr(current_die, DW_AT_specification, - &has_attr, &error) == DW_DLV_OK) { - if (has_attr) { - Dwarf_Attribute attr_mem; - if (dwarf_attr(current_die, DW_AT_specification, - &attr_mem, &error) == DW_DLV_OK) { - Dwarf_Off spec_offset = 0; - if (dwarf_formref(attr_mem, - &spec_offset, &error) == DW_DLV_OK) { - Dwarf_Off spec_die_offset; - if (dwarf_dieoffset(current_die, - &spec_die_offset, &error) - == DW_DLV_OK) { - de.spec_section[spec_offset] = - spec_die_offset; - } - } - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - } - } - } - - int result = dwarf_siblingof( - dwarf, current_die, &sibling_die, &error); - if (result == DW_DLV_ERROR) { - break; - } else if (result == DW_DLV_NO_ENTRY) { - break; - } - - if (current_die != die) { - dwarf_dealloc(dwarf, current_die, DW_DLA_DIE); - current_die = 0; - } - - current_die = sibling_die; - } - } - return de; - } - - static Dwarf_Die get_referenced_die( - Dwarf_Debug dwarf, Dwarf_Die die, Dwarf_Half attr, bool global) { - Dwarf_Error error = DW_DLE_NE; - Dwarf_Attribute attr_mem; - - Dwarf_Die found_die = NULL; - if (dwarf_attr(die, attr, &attr_mem, &error) == DW_DLV_OK) { - Dwarf_Off offset; - int result = 0; - if (global) { - result = dwarf_global_formref(attr_mem, &offset, &error); - } else { - result = dwarf_formref(attr_mem, &offset, &error); - } - - if (result == DW_DLV_OK) { - if (dwarf_offdie(dwarf, offset, &found_die, &error) - != DW_DLV_OK) { - found_die = NULL; - } - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - } - return found_die; - } - - static std::string get_referenced_die_name( - Dwarf_Debug dwarf, Dwarf_Die die, Dwarf_Half attr, bool global) { - Dwarf_Error error = DW_DLE_NE; - std::string value; - - Dwarf_Die found_die = get_referenced_die(dwarf, die, attr, global); - - if (found_die) { - char *name; - if (dwarf_diename(found_die, &name, &error) == DW_DLV_OK) { - if (name) { - value = std::string(name); - } - dwarf_dealloc(dwarf, name, DW_DLA_STRING); - } - dwarf_dealloc(dwarf, found_die, DW_DLA_DIE); - } - - return value; - } - - // Returns a spec DIE linked to the passed one. The caller should - // deallocate the DIE - static Dwarf_Die get_spec_die(dwarf_fileobject& fobj, Dwarf_Die die) { - Dwarf_Debug dwarf = fobj.dwarf_handle.get(); - Dwarf_Error error = DW_DLE_NE; - Dwarf_Off die_offset; - if (fobj.current_cu && dwarf_die_CU_offset(die, &die_offset, &error) - == DW_DLV_OK) { - die_specmap_t::iterator it = - fobj.current_cu->spec_section.find(die_offset); - - // If we have a DIE that completes the current one, check if - // that one has the pc we are looking for - if (it != fobj.current_cu->spec_section.end()) { - Dwarf_Die spec_die = 0; - if (dwarf_offdie(dwarf, it->second, &spec_die, &error) - == DW_DLV_OK) { - return spec_die; - } - } - } - - // Maybe we have an abstract origin DIE with the function information? - return get_referenced_die( - fobj.dwarf_handle.get(), die, DW_AT_abstract_origin, true); - - } - - static bool die_has_pc(dwarf_fileobject& fobj, Dwarf_Die die, Dwarf_Addr pc) - { - Dwarf_Addr low_pc = 0, high_pc = 0; - Dwarf_Half high_pc_form = 0; - Dwarf_Form_Class return_class; - Dwarf_Error error = DW_DLE_NE; - Dwarf_Debug dwarf = fobj.dwarf_handle.get(); - bool has_lowpc = false; - bool has_highpc = false; - bool has_ranges = false; - - if (dwarf_lowpc(die, &low_pc, &error) == DW_DLV_OK) { - // If we have a low_pc check if there is a high pc. - // If we don't have a high pc this might mean we have a base - // address for the ranges list or just an address. - has_lowpc = true; - - if (dwarf_highpc_b( - die, &high_pc, &high_pc_form, &return_class, &error) - == DW_DLV_OK) { - // We do have a high pc. In DWARF 4+ this is an offset from the - // low pc, but in earlier versions it's an absolute address. - - has_highpc = true; - // In DWARF 2/3 this would be a DW_FORM_CLASS_ADDRESS - if (return_class == DW_FORM_CLASS_CONSTANT) { - high_pc = low_pc + high_pc; - } - - // We have low and high pc, check if our address - // is in that range - return pc >= low_pc && pc < high_pc; - } - } else { - // Reset the low_pc, in case dwarf_lowpc failing set it to some - // undefined value. - low_pc = 0; - } - - // Check if DW_AT_ranges is present and search for the PC in the - // returned ranges list. We always add the low_pc, as it not set it will - // be 0, in case we had a DW_AT_low_pc and DW_AT_ranges pair - bool result = false; - - Dwarf_Attribute attr; - if (dwarf_attr(die, DW_AT_ranges, &attr, &error) == DW_DLV_OK) { - - Dwarf_Off offset; - if (dwarf_global_formref(attr, &offset, &error) == DW_DLV_OK) { - Dwarf_Ranges *ranges; - Dwarf_Signed ranges_count = 0; - Dwarf_Unsigned byte_count = 0; - - if (dwarf_get_ranges_a(dwarf, offset, die, &ranges, - &ranges_count, &byte_count, &error) == DW_DLV_OK) { - has_ranges = ranges_count != 0; - for (int i = 0; i < ranges_count; i++) { - if (ranges[i].dwr_addr1 != 0 && - pc >= ranges[i].dwr_addr1 + low_pc && - pc < ranges[i].dwr_addr2 + low_pc) { - result = true; - break; - } - } - dwarf_ranges_dealloc(dwarf, ranges, ranges_count); - } - } - } - - // Last attempt. We might have a single address set as low_pc. - if (!result && low_pc != 0 && pc == low_pc) { - result = true; - } - - // If we don't have lowpc, highpc and ranges maybe this DIE is a - // declaration that relies on a DW_AT_specification DIE that happens - // later. Use the specification cache we filled when we loaded this CU. - if (!result && (!has_lowpc && !has_highpc && !has_ranges)) { - Dwarf_Die spec_die = get_spec_die(fobj, die); - if (spec_die) { - result = die_has_pc(fobj, spec_die, pc); - dwarf_dealloc(dwarf, spec_die, DW_DLA_DIE); - } - } - - return result; - } - - static void get_type(Dwarf_Debug dwarf, Dwarf_Die die, std::string& type) { - Dwarf_Error error = DW_DLE_NE; - - Dwarf_Die child = 0; - if (dwarf_child(die, &child, &error) == DW_DLV_OK) { - get_type(dwarf, child, type); - } - - if (child) { - type.insert(0, "::"); - dwarf_dealloc(dwarf, child, DW_DLA_DIE); - } - - char *name; - if (dwarf_diename(die, &name, &error) == DW_DLV_OK) { - type.insert(0, std::string(name)); - dwarf_dealloc(dwarf, name, DW_DLA_STRING); - } else { - type.insert(0,""); - } - } - - static std::string get_type_by_signature(Dwarf_Debug dwarf, Dwarf_Die die) { - Dwarf_Error error = DW_DLE_NE; - - Dwarf_Sig8 signature; - Dwarf_Bool has_attr = 0; - if (dwarf_hasattr(die, DW_AT_signature, - &has_attr, &error) == DW_DLV_OK) { - if (has_attr) { - Dwarf_Attribute attr_mem; - if (dwarf_attr(die, DW_AT_signature, - &attr_mem, &error) == DW_DLV_OK) { - if (dwarf_formsig8(attr_mem, &signature, &error) - != DW_DLV_OK) { - return std::string(""); - } - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - } - } - - Dwarf_Unsigned next_cu_header; - Dwarf_Sig8 tu_signature; - std::string result; - bool found = false; - - while (dwarf_next_cu_header_d(dwarf, 0, 0, 0, 0, 0, 0, 0, &tu_signature, - 0, &next_cu_header, 0, &error) == DW_DLV_OK) { - - if (strncmp(signature.signature, tu_signature.signature, 8) == 0) { - Dwarf_Die type_cu_die = 0; - if (dwarf_siblingof_b(dwarf, 0, 0, &type_cu_die, &error) - == DW_DLV_OK) { - Dwarf_Die child_die = 0; - if (dwarf_child(type_cu_die, &child_die, &error) - == DW_DLV_OK) { - get_type(dwarf, child_die, result); - found = !result.empty(); - dwarf_dealloc(dwarf, child_die, DW_DLA_DIE); - } - dwarf_dealloc(dwarf, type_cu_die, DW_DLA_DIE); - } - } - } - - if (found) { - while (dwarf_next_cu_header_d(dwarf, 0, 0, 0, 0, 0, 0, 0, 0, 0, - &next_cu_header, 0, &error) == DW_DLV_OK) { - // Reset the cu header state. Unfortunately, libdwarf's - // next_cu_header API keeps its own iterator per Dwarf_Debug - // that can't be reset. We need to keep fetching elements until - // the end. - } - } else { - // If we couldn't resolve the type just print out the signature - std::ostringstream string_stream; - string_stream << "<0x" << - std::hex << std::setfill('0'); - for (int i = 0; i < 8; ++i) { - string_stream << std::setw(2) << std::hex - << (int)(unsigned char)(signature.signature[i]); - } - string_stream << ">"; - result = string_stream.str(); - } - return result; - } - - struct type_context_t { - bool is_const; - bool is_typedef; - bool has_type; - bool has_name; - std::string text; - - type_context_t() : - is_const(false), is_typedef(false), - has_type(false), has_name(false) {} - }; - - // Types are resolved from right to left: we get the variable name first - // and then all specifiers (like const or pointer) in a chain of DW_AT_type - // DIEs. Call this function recursively until we get a complete type - // string. - static void set_parameter_string( - dwarf_fileobject& fobj, Dwarf_Die die, type_context_t &context) { - char *name; - Dwarf_Error error = DW_DLE_NE; - - // typedefs contain also the base type, so we skip it and only - // print the typedef name - if (!context.is_typedef) { - if (dwarf_diename(die, &name, &error) == DW_DLV_OK) { - if (!context.text.empty()) { - context.text.insert(0, " "); - } - context.text.insert(0, std::string(name)); - dwarf_dealloc(fobj.dwarf_handle.get(), name, DW_DLA_STRING); - } - } else { - context.is_typedef = false; - context.has_type = true; - if (context.is_const) { - context.text.insert(0, "const "); - context.is_const = false; - } - } - - bool next_type_is_const = false; - bool is_keyword = true; - - Dwarf_Half tag = 0; - Dwarf_Bool has_attr = 0; - if (dwarf_tag(die, &tag, &error) == DW_DLV_OK) { - switch(tag) { - case DW_TAG_structure_type: - case DW_TAG_union_type: - case DW_TAG_class_type: - case DW_TAG_enumeration_type: - context.has_type = true; - if (dwarf_hasattr(die, DW_AT_signature, - &has_attr, &error) == DW_DLV_OK) { - // If we have a signature it means the type is defined - // in .debug_types, so we need to load the DIE pointed - // at by the signature and resolve it - if (has_attr) { - std::string type = - get_type_by_signature(fobj.dwarf_handle.get(), die); - if (context.is_const) - type.insert(0, "const "); - - if (!context.text.empty()) - context.text.insert(0, " "); - context.text.insert(0, type); - } - - // Treat enums like typedefs, and skip printing its - // base type - context.is_typedef = (tag == DW_TAG_enumeration_type); - } - break; - case DW_TAG_const_type: - next_type_is_const = true; - break; - case DW_TAG_pointer_type: - context.text.insert(0, "*"); - break; - case DW_TAG_reference_type: - context.text.insert(0, "&"); - break; - case DW_TAG_restrict_type: - context.text.insert(0, "restrict "); - break; - case DW_TAG_rvalue_reference_type: - context.text.insert(0, "&&"); - break; - case DW_TAG_volatile_type: - context.text.insert(0, "volatile "); - break; - case DW_TAG_typedef: - // Propagate the const-ness to the next type - // as typedefs are linked to its base type - next_type_is_const = context.is_const; - context.is_typedef = true; - context.has_type = true; - break; - case DW_TAG_base_type: - context.has_type = true; - break; - case DW_TAG_formal_parameter: - context.has_name = true; - break; - default: - is_keyword = false; - break; - } - } - - if (!is_keyword && context.is_const) { - context.text.insert(0, "const "); - } - - context.is_const = next_type_is_const; - - Dwarf_Die ref = get_referenced_die( - fobj.dwarf_handle.get(), die, DW_AT_type, true); - if (ref) { - set_parameter_string(fobj, ref, context); - dwarf_dealloc(fobj.dwarf_handle.get(), ref, DW_DLA_DIE); - } - - if (!context.has_type && context.has_name) { - context.text.insert(0, "void "); - context.has_type = true; - } - } - - // Resolve the function return type and parameters - static void set_function_parameters(std::string& function_name, - std::vector& ns, - dwarf_fileobject& fobj, Dwarf_Die die) { - Dwarf_Debug dwarf = fobj.dwarf_handle.get(); - Dwarf_Error error = DW_DLE_NE; - Dwarf_Die current_die = 0; - std::string parameters; - bool has_spec = true; - // Check if we have a spec DIE. If we do we use it as it contains - // more information, like parameter names. - Dwarf_Die spec_die = get_spec_die(fobj, die); - if (!spec_die) { - has_spec = false; - spec_die = die; - } - - std::vector::const_iterator it = ns.begin(); - std::string ns_name; - for (it = ns.begin(); it < ns.end(); ++it) { - ns_name.append(*it).append("::"); - } - - if (!ns_name.empty()) { - function_name.insert(0, ns_name); - } - - // See if we have a function return type. It can be either on the - // current die or in its spec one (usually true for inlined functions) - std::string return_type = - get_referenced_die_name(dwarf, die, DW_AT_type, true); - if (return_type.empty()) { - return_type = - get_referenced_die_name(dwarf, spec_die, DW_AT_type, true); - } - if (!return_type.empty()) { - return_type.append(" "); - function_name.insert(0, return_type); - } - - if (dwarf_child(spec_die, ¤t_die, &error) == DW_DLV_OK) { - for(;;) { - Dwarf_Die sibling_die = 0; - - Dwarf_Half tag_value; - dwarf_tag(current_die, &tag_value, &error); - - if (tag_value == DW_TAG_formal_parameter) { - // Ignore artificial (ie, compiler generated) parameters - bool is_artificial = false; - Dwarf_Attribute attr_mem; - if (dwarf_attr( - current_die, DW_AT_artificial, &attr_mem, &error) - == DW_DLV_OK) { - Dwarf_Bool flag = 0; - if (dwarf_formflag(attr_mem, &flag, &error) - == DW_DLV_OK) { - is_artificial = flag != 0; - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - } - - if (!is_artificial) { - type_context_t context; - set_parameter_string(fobj, current_die, context); - - if (parameters.empty()) { - parameters.append("("); - } else { - parameters.append(", "); - } - parameters.append(context.text); - } - } - - int result = dwarf_siblingof( - dwarf, current_die, &sibling_die, &error); - if (result == DW_DLV_ERROR) { - break; - } else if (result == DW_DLV_NO_ENTRY) { - break; - } - - if (current_die != die) { - dwarf_dealloc(dwarf, current_die, DW_DLA_DIE); - current_die = 0; - } - - current_die = sibling_die; - } - } - if (parameters.empty()) - parameters = "("; - parameters.append(")"); - - // If we got a spec DIE we need to deallocate it - if (has_spec) - dwarf_dealloc(dwarf, spec_die, DW_DLA_DIE); - - function_name.append(parameters); - } - - // defined here because in C++98, template function cannot take locally - // defined types... grrr. - struct inliners_search_cb { - void operator()(Dwarf_Die die, std::vector& ns) { - Dwarf_Error error = DW_DLE_NE; - Dwarf_Half tag_value; - Dwarf_Attribute attr_mem; - Dwarf_Debug dwarf = fobj.dwarf_handle.get(); - - dwarf_tag(die, &tag_value, &error); - - switch (tag_value) { - char* name; - case DW_TAG_subprogram: - if (!trace.source.function.empty()) - break; - if (dwarf_diename(die, &name, &error) == DW_DLV_OK) { - trace.source.function = std::string(name); - dwarf_dealloc(dwarf, name, DW_DLA_STRING); - } else { - // We don't have a function name in this DIE. - // Check if there is a referenced non-defining - // declaration. - trace.source.function = get_referenced_die_name( - dwarf, die, DW_AT_abstract_origin, true); - if (trace.source.function.empty()) { - trace.source.function = get_referenced_die_name( - dwarf, die, DW_AT_specification, true); - } - } - - // Append the function parameters, if available - set_function_parameters( - trace.source.function, ns, fobj, die); - - // If the object function name is empty, it's possible that - // there is no dynamic symbol table (maybe the executable - // was stripped or not built with -rdynamic). See if we have - // a DWARF linkage name to use instead. We try both - // linkage_name and MIPS_linkage_name because the MIPS tag - // was the unofficial one until it was adopted in DWARF4. - // Old gcc versions generate MIPS_linkage_name - if (trace.object_function.empty()) { - details::demangler demangler; - - if (dwarf_attr(die, DW_AT_linkage_name, - &attr_mem, &error) != DW_DLV_OK) { - if (dwarf_attr(die, DW_AT_MIPS_linkage_name, - &attr_mem, &error) != DW_DLV_OK) { - break; - } - } - - char* linkage; - if (dwarf_formstring(attr_mem, &linkage, &error) - == DW_DLV_OK) { - trace.object_function = demangler.demangle(linkage); - dwarf_dealloc(dwarf, linkage, DW_DLA_STRING); - } - dwarf_dealloc(dwarf, name, DW_DLA_ATTR); - } - break; - - case DW_TAG_inlined_subroutine: - ResolvedTrace::SourceLoc sloc; - - if (dwarf_diename(die, &name, &error) == DW_DLV_OK) { - sloc.function = std::string(name); - dwarf_dealloc(dwarf, name, DW_DLA_STRING); - } else { - // We don't have a name for this inlined DIE, it could - // be that there is an abstract origin instead. - // Get the DW_AT_abstract_origin value, which is a - // reference to the source DIE and try to get its name - sloc.function = get_referenced_die_name( - dwarf, die, DW_AT_abstract_origin, true); - } - - set_function_parameters(sloc.function, ns, fobj, die); - - std::string file = die_call_file(dwarf, die, cu_die); - if (!file.empty()) - sloc.filename = file; - - Dwarf_Unsigned number = 0; - if (dwarf_attr(die, DW_AT_call_line, &attr_mem, &error) - == DW_DLV_OK) { - if (dwarf_formudata(attr_mem, &number, &error) - == DW_DLV_OK) { - sloc.line = number; - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - } - - if (dwarf_attr(die, DW_AT_call_column, &attr_mem, &error) - == DW_DLV_OK) { - if (dwarf_formudata(attr_mem, &number, &error) - == DW_DLV_OK) { - sloc.col = number; - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - } - - trace.inliners.push_back(sloc); - break; - }; - } - ResolvedTrace& trace; - dwarf_fileobject& fobj; - Dwarf_Die cu_die; - inliners_search_cb(ResolvedTrace& t, dwarf_fileobject& f, Dwarf_Die c) - : trace(t), fobj(f), cu_die(c) {} - }; - - static Dwarf_Die find_fundie_by_pc(dwarf_fileobject& fobj, - Dwarf_Die parent_die, Dwarf_Addr pc, Dwarf_Die result) { - Dwarf_Die current_die = 0; - Dwarf_Error error = DW_DLE_NE; - Dwarf_Debug dwarf = fobj.dwarf_handle.get(); - - if (dwarf_child(parent_die, ¤t_die, &error) != DW_DLV_OK) { - return NULL; - } - - for(;;) { - Dwarf_Die sibling_die = 0; - Dwarf_Half tag_value; - dwarf_tag(current_die, &tag_value, &error); - - switch (tag_value) { - case DW_TAG_subprogram: - case DW_TAG_inlined_subroutine: - if (die_has_pc(fobj, current_die, pc)) { - return current_die; - } - }; - bool declaration = false; - Dwarf_Attribute attr_mem; - if (dwarf_attr(current_die, DW_AT_declaration, &attr_mem, &error) - == DW_DLV_OK) { - Dwarf_Bool flag = 0; - if (dwarf_formflag(attr_mem, &flag, &error) == DW_DLV_OK) { - declaration = flag != 0; - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - } - - if (!declaration) { - // let's be curious and look deeper in the tree, functions are - // not necessarily at the first level, but might be nested - // inside a namespace, structure, a function, an inlined - // function etc. - Dwarf_Die die_mem = 0; - Dwarf_Die indie = find_fundie_by_pc( - fobj, current_die, pc, die_mem); - if (indie) { - result = die_mem; - return result; - } - } - - int res = dwarf_siblingof( - dwarf, current_die, &sibling_die, &error); - if (res == DW_DLV_ERROR) { - return NULL; - } else if (res == DW_DLV_NO_ENTRY) { - break; - } - - if (current_die != parent_die) { - dwarf_dealloc(dwarf, current_die, DW_DLA_DIE); - current_die = 0; - } - - current_die = sibling_die; - } - return NULL; - } - - template - static bool deep_first_search_by_pc(dwarf_fileobject& fobj, - Dwarf_Die parent_die, Dwarf_Addr pc, - std::vector& ns, CB cb) { - Dwarf_Die current_die = 0; - Dwarf_Debug dwarf = fobj.dwarf_handle.get(); - Dwarf_Error error = DW_DLE_NE; - - if (dwarf_child(parent_die, ¤t_die, &error) != DW_DLV_OK) { - return false; - } - - bool branch_has_pc = false; - bool has_namespace = false; - for(;;) { - Dwarf_Die sibling_die = 0; - - Dwarf_Half tag; - if (dwarf_tag(current_die, &tag, &error) == DW_DLV_OK) { - if (tag == DW_TAG_namespace || tag == DW_TAG_class_type) { - char* ns_name = NULL; - if (dwarf_diename(current_die, &ns_name, &error) - == DW_DLV_OK) { - if (ns_name) { - ns.push_back(std::string(ns_name)); - } else { - ns.push_back(""); - } - dwarf_dealloc(dwarf, ns_name, DW_DLA_STRING); - } else { - ns.push_back(""); - } - has_namespace = true; - } - } - - bool declaration = false; - Dwarf_Attribute attr_mem; - if (tag != DW_TAG_class_type && - dwarf_attr(current_die, DW_AT_declaration, &attr_mem, &error) - == DW_DLV_OK) { - Dwarf_Bool flag = 0; - if (dwarf_formflag(attr_mem, &flag, &error) == DW_DLV_OK) { - declaration = flag != 0; - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - } - - if (!declaration) { - // let's be curious and look deeper in the tree, function are - // not necessarily at the first level, but might be nested - // inside a namespace, structure, a function, an inlined - // function etc. - branch_has_pc = deep_first_search_by_pc( - fobj, current_die, pc, ns, cb); - } - - if (!branch_has_pc) { - branch_has_pc = die_has_pc(fobj, current_die, pc); - } - - if (branch_has_pc) { - cb(current_die, ns); - } - - int result = dwarf_siblingof( - dwarf, current_die, &sibling_die, &error); - if (result == DW_DLV_ERROR) { - return false; - } else if (result == DW_DLV_NO_ENTRY) { - break; - } - - if (current_die != parent_die) { - dwarf_dealloc(dwarf, current_die, DW_DLA_DIE); - current_die = 0; - } - - if (has_namespace) { - has_namespace = false; - ns.pop_back(); - } - current_die = sibling_die; - } - - if (has_namespace) { - ns.pop_back(); - } - return branch_has_pc; - } - - static std::string die_call_file( - Dwarf_Debug dwarf, Dwarf_Die die, Dwarf_Die cu_die) { - Dwarf_Attribute attr_mem; - Dwarf_Error error = DW_DLE_NE; - Dwarf_Signed file_index; - - std::string file; - - if (dwarf_attr(die, DW_AT_call_file, &attr_mem, &error) == DW_DLV_OK) { - if (dwarf_formsdata(attr_mem, &file_index, &error) != DW_DLV_OK) { - file_index = 0; - } - dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR); - - if (file_index == 0) { - return file; - } - - char **srcfiles = 0; - Dwarf_Signed file_count = 0; - if (dwarf_srcfiles(cu_die, &srcfiles, &file_count, &error) - == DW_DLV_OK) { - if (file_index <= file_count) - file = std::string(srcfiles[file_index - 1]); - - // Deallocate all strings! - for (int i = 0; i < file_count; ++i) { - dwarf_dealloc(dwarf, srcfiles[i], DW_DLA_STRING); - } - dwarf_dealloc(dwarf, srcfiles, DW_DLA_LIST); - } - } - return file; - } - - - Dwarf_Die find_die(dwarf_fileobject& fobj, Dwarf_Addr addr) - { - // Let's get to work! First see if we have a debug_aranges section so - // we can speed up the search - - Dwarf_Debug dwarf = fobj.dwarf_handle.get(); - Dwarf_Error error = DW_DLE_NE; - Dwarf_Arange *aranges; - Dwarf_Signed arange_count; - - Dwarf_Die returnDie; - bool found = false; - if (dwarf_get_aranges( - dwarf, &aranges, &arange_count, &error) != DW_DLV_OK) { - aranges = NULL; - } - - if (aranges) { - // We have aranges. Get the one where our address is. - Dwarf_Arange arange; - if (dwarf_get_arange( - aranges, arange_count, addr, &arange, &error) - == DW_DLV_OK) { - - // We found our address. Get the compilation-unit DIE offset - // represented by the given address range. - Dwarf_Off cu_die_offset; - if (dwarf_get_cu_die_offset(arange, &cu_die_offset, &error) - == DW_DLV_OK) { - // Get the DIE at the offset returned by the aranges search. - // We set is_info to 1 to specify that the offset is from - // the .debug_info section (and not .debug_types) - int dwarf_result = dwarf_offdie_b( - dwarf, cu_die_offset, 1, &returnDie, &error); - - found = dwarf_result == DW_DLV_OK; - } - dwarf_dealloc(dwarf, arange, DW_DLA_ARANGE); - } - } - - if (found) - return returnDie; // The caller is responsible for freeing the die - - // The search for aranges failed. Try to find our address by scanning - // all compilation units. - Dwarf_Unsigned next_cu_header; - Dwarf_Half tag = 0; - returnDie = 0; - - while (!found && dwarf_next_cu_header_d(dwarf, 1, 0, 0, 0, 0, 0, 0, 0, 0, - &next_cu_header, 0, &error) == DW_DLV_OK) { - - if (returnDie) - dwarf_dealloc(dwarf, returnDie, DW_DLA_DIE); - - if (dwarf_siblingof(dwarf, 0, &returnDie, &error) == DW_DLV_OK) { - if ((dwarf_tag(returnDie, &tag, &error) == DW_DLV_OK) - && tag == DW_TAG_compile_unit) { - if (die_has_pc(fobj, returnDie, addr)) { - found = true; - } - } - } - } - - if (found) { - while (dwarf_next_cu_header_d(dwarf, 1, 0, 0, 0, 0, 0, 0, 0, 0, - &next_cu_header, 0, &error) == DW_DLV_OK) { - // Reset the cu header state. Libdwarf's next_cu_header API - // keeps its own iterator per Dwarf_Debug that can't be reset. - // We need to keep fetching elements until the end. - } - } - - if (found) - return returnDie; - - // We couldn't find any compilation units with ranges or a high/low pc. - // Try again by looking at all DIEs in all compilation units. - Dwarf_Die cudie; - while (dwarf_next_cu_header_d(dwarf, 1, 0, 0, 0, 0, 0, 0, 0, 0, - &next_cu_header, 0, &error) == DW_DLV_OK) { - if (dwarf_siblingof(dwarf, 0, &cudie, &error) == DW_DLV_OK) { - Dwarf_Die die_mem = 0; - Dwarf_Die resultDie = find_fundie_by_pc( - fobj, cudie, addr, die_mem); - - if (resultDie) { - found = true; - break; - } - } - } - - if (found) { - while (dwarf_next_cu_header_d(dwarf, 1, 0, 0, 0, 0, 0, 0, 0, 0, - &next_cu_header, 0, &error) == DW_DLV_OK) { - // Reset the cu header state. Libdwarf's next_cu_header API - // keeps its own iterator per Dwarf_Debug that can't be reset. - // We need to keep fetching elements until the end. - } - } - - if (found) - return cudie; - - // We failed. - return NULL; - } -}; -#endif // BACKWARD_HAS_DWARF == 1 - -template<> -class TraceResolverImpl: - public TraceResolverLinuxImpl {}; - -#endif // BACKWARD_SYSTEM_LINUX - -#ifdef BACKWARD_SYSTEM_DARWIN - -template -class TraceResolverDarwinImpl; - -template <> -class TraceResolverDarwinImpl: - public TraceResolverImplBase { -public: - template - void load_stacktrace(ST& st) { - using namespace details; - if (st.size() == 0) { - return; - } - _symbols.reset( - backtrace_symbols(st.begin(), st.size()) - ); - } - - ResolvedTrace resolve(ResolvedTrace trace) { - // parse: - // + - char* filename = _symbols[trace.idx]; - - // skip " " - while(*filename && *filename != ' ') filename++; - while(*filename == ' ') filename++; - - // find start of from end ( may contain a space) - char* p = filename + strlen(filename) - 1; - // skip to start of " + " - while(p > filename && *p != ' ') p--; - while(p > filename && *p == ' ') p--; - while(p > filename && *p != ' ') p--; - while(p > filename && *p == ' ') p--; - char *funcname_end = p + 1; - - // skip to start of "" - while(p > filename && *p != ' ') p--; - char *funcname = p + 1; - - // skip to start of " " - while(p > filename && *p == ' ') p--; - while(p > filename && *p != ' ') p--; - while(p > filename && *p == ' ') p--; - - // skip "", handling the case where it contains a - char* filename_end = p + 1; - if (p == filename) { - // something went wrong, give up - filename_end = filename + strlen(filename); - funcname = filename_end; - } - trace.object_filename.assign(filename, filename_end); // ok even if filename_end is the ending \0 (then we assign entire string) - - if (*funcname) { // if it's not end of string - *funcname_end = '\0'; - - trace.object_function = this->demangle(funcname); - trace.object_function += " "; - trace.object_function += (funcname_end + 1); - trace.source.function = trace.object_function; // we cannot do better. - } - return trace; - } - -private: - details::handle _symbols; -}; - -template<> -class TraceResolverImpl: - public TraceResolverDarwinImpl {}; - -#endif // BACKWARD_SYSTEM_DARWIN - -class TraceResolver: - public TraceResolverImpl {}; - -/*************** CODE SNIPPET ***************/ - -class SourceFile { -public: - typedef std::vector > lines_t; - - SourceFile() {} - SourceFile(const std::string& path): _file(new std::ifstream(path.c_str())) {} - bool is_open() const { return _file->is_open(); } - - lines_t& get_lines(unsigned line_start, unsigned line_count, lines_t& lines) { - using namespace std; - // This function make uses of the dumbest algo ever: - // 1) seek(0) - // 2) read lines one by one and discard until line_start - // 3) read line one by one until line_start + line_count - // - // If you are getting snippets many time from the same file, it is - // somewhat a waste of CPU, feel free to benchmark and propose a - // better solution ;) - - _file->clear(); - _file->seekg(0); - string line; - unsigned line_idx; - - for (line_idx = 1; line_idx < line_start; ++line_idx) { - std::getline(*_file, line); - if (!*_file) { - return lines; - } - } - - // think of it like a lambda in C++98 ;) - // but look, I will reuse it two times! - // What a good boy am I. - struct isspace { - bool operator()(char c) { - return std::isspace(c); - } - }; - - bool started = false; - for (; line_idx < line_start + line_count; ++line_idx) { - getline(*_file, line); - if (!*_file) { - return lines; - } - if (!started) { - if (std::find_if(line.begin(), line.end(), - not_isspace()) == line.end()) - continue; - started = true; - } - lines.push_back(make_pair(line_idx, line)); - } - - lines.erase( - std::find_if(lines.rbegin(), lines.rend(), - not_isempty()).base(), lines.end() - ); - return lines; - } - - lines_t get_lines(unsigned line_start, unsigned line_count) { - lines_t lines; - return get_lines(line_start, line_count, lines); - } - - // there is no find_if_not in C++98, lets do something crappy to - // workaround. - struct not_isspace { - bool operator()(char c) { - return !std::isspace(c); - } - }; - // and define this one here because C++98 is not happy with local defined - // struct passed to template functions, fuuuu. - struct not_isempty { - bool operator()(const lines_t::value_type& p) { - return !(std::find_if(p.second.begin(), p.second.end(), - not_isspace()) == p.second.end()); - } - }; - - void swap(SourceFile& b) { - _file.swap(b._file); - } - -#ifdef BACKWARD_ATLEAST_CXX11 - SourceFile(SourceFile&& from): _file(nullptr) { - swap(from); - } - SourceFile& operator=(SourceFile&& from) { - swap(from); return *this; - } -#else - explicit SourceFile(const SourceFile& from) { - // some sort of poor man's move semantic. - swap(const_cast(from)); - } - SourceFile& operator=(const SourceFile& from) { - // some sort of poor man's move semantic. - swap(const_cast(from)); return *this; - } -#endif - -private: - details::handle - > _file; - -#ifdef BACKWARD_ATLEAST_CXX11 - SourceFile(const SourceFile&) = delete; - SourceFile& operator=(const SourceFile&) = delete; -#endif -}; - -class SnippetFactory { -public: - typedef SourceFile::lines_t lines_t; - - lines_t get_snippet(const std::string& filename, - unsigned line_start, unsigned context_size) { - - SourceFile& src_file = get_src_file(filename); - unsigned start = line_start - context_size / 2; - return src_file.get_lines(start, context_size); - } - - lines_t get_combined_snippet( - const std::string& filename_a, unsigned line_a, - const std::string& filename_b, unsigned line_b, - unsigned context_size) { - SourceFile& src_file_a = get_src_file(filename_a); - SourceFile& src_file_b = get_src_file(filename_b); - - lines_t lines = src_file_a.get_lines(line_a - context_size / 4, - context_size / 2); - src_file_b.get_lines(line_b - context_size / 4, context_size / 2, - lines); - return lines; - } - - lines_t get_coalesced_snippet(const std::string& filename, - unsigned line_a, unsigned line_b, unsigned context_size) { - SourceFile& src_file = get_src_file(filename); - - using std::min; using std::max; - unsigned a = min(line_a, line_b); - unsigned b = max(line_a, line_b); - - if ((b - a) < (context_size / 3)) { - return src_file.get_lines((a + b - context_size + 1) / 2, - context_size); - } - - lines_t lines = src_file.get_lines(a - context_size / 4, - context_size / 2); - src_file.get_lines(b - context_size / 4, context_size / 2, lines); - return lines; - } - - -private: - typedef details::hashtable::type src_files_t; - src_files_t _src_files; - - SourceFile& get_src_file(const std::string& filename) { - src_files_t::iterator it = _src_files.find(filename); - if (it != _src_files.end()) { - return it->second; - } - SourceFile& new_src_file = _src_files[filename]; - new_src_file = SourceFile(filename); - return new_src_file; - } -}; - -/*************** PRINTER ***************/ - -namespace ColorMode { - enum type { - automatic, - never, - always - }; -} - -class cfile_streambuf: public std::streambuf { -public: - cfile_streambuf(FILE *_sink): sink(_sink) {} - int_type underflow() override { return traits_type::eof(); } - int_type overflow(int_type ch) override { - if (traits_type::not_eof(ch) && fwrite(&ch, sizeof ch, 1, sink) == 1) { - return ch; - } - return traits_type::eof(); - } - - std::streamsize xsputn(const char_type* s, std::streamsize count) override { - return static_cast(fwrite(s, sizeof *s, static_cast(count), sink)); - } - -#ifdef BACKWARD_ATLEAST_CXX11 -public: - cfile_streambuf(const cfile_streambuf&) = delete; - cfile_streambuf& operator=(const cfile_streambuf&) = delete; -#else -private: - cfile_streambuf(const cfile_streambuf &); - cfile_streambuf &operator= (const cfile_streambuf &); -#endif - -private: - FILE *sink; - std::vector buffer; -}; - -#ifdef BACKWARD_SYSTEM_LINUX - -namespace Color { - enum type { - yellow = 33, - purple = 35, - reset = 39 - }; -} // namespace Color - -class Colorize { -public: - Colorize(std::ostream& os): - _os(os), _reset(false), _enabled(false) {} - - void activate(ColorMode::type mode) { - _enabled = mode == ColorMode::always; - } - - void activate(ColorMode::type mode, FILE* fp) { - activate(mode, fileno(fp)); - } - - void set_color(Color::type ccode) { - if (!_enabled) return; - - // I assume that the terminal can handle basic colors. Seriously I - // don't want to deal with all the termcap shit. - _os << "\033[" << static_cast(ccode) << "m"; - _reset = (ccode != Color::reset); - } - - ~Colorize() { - if (_reset) { - set_color(Color::reset); - } - } - -private: - void activate(ColorMode::type mode, int fd) { - activate(mode == ColorMode::automatic && isatty(fd) ? ColorMode::always : mode); - } - - std::ostream& _os; - bool _reset; - bool _enabled; -}; - -#else // ndef BACKWARD_SYSTEM_LINUX - -namespace Color { - enum type { - yellow = 0, - purple = 0, - reset = 0 - }; -} // namespace Color - -class Colorize { -public: - Colorize(std::ostream&) {} - void activate(ColorMode::type) {} - void activate(ColorMode::type, FILE*) {} - void set_color(Color::type) {} -}; - -#endif // BACKWARD_SYSTEM_LINUX - -class Printer { -public: - - bool snippet; - ColorMode::type color_mode; - bool address; - bool object; - int inliner_context_size; - int trace_context_size; - - Printer(): - snippet(true), - color_mode(ColorMode::automatic), - address(false), - object(false), - inliner_context_size(5), - trace_context_size(7) - {} - - template - FILE* print(ST& st, FILE* fp = stderr) { - cfile_streambuf obuf(fp); - std::ostream os(&obuf); - Colorize colorize(os); - colorize.activate(color_mode, fp); - print_stacktrace(st, os, colorize); - return fp; - } - - template - std::ostream& print(ST& st, std::ostream& os) { - Colorize colorize(os); - colorize.activate(color_mode); - print_stacktrace(st, os, colorize); - return os; - } - - template - FILE* print(IT begin, IT end, FILE* fp = stderr, size_t thread_id = 0) { - cfile_streambuf obuf(fp); - std::ostream os(&obuf); - Colorize colorize(os); - colorize.activate(color_mode, fp); - print_stacktrace(begin, end, os, thread_id, colorize); - return fp; - } - - template - std::ostream& print(IT begin, IT end, std::ostream& os, size_t thread_id = 0) { - Colorize colorize(os); - colorize.activate(color_mode); - print_stacktrace(begin, end, os, thread_id, colorize); - return os; - } - -private: - TraceResolver _resolver; - SnippetFactory _snippets; - - template - void print_stacktrace(ST& st, std::ostream& os, Colorize& colorize) { - print_header(os, st.thread_id()); - _resolver.load_stacktrace(st); - for (size_t trace_idx = st.size(); trace_idx > 0; --trace_idx) { - print_trace(os, _resolver.resolve(st[trace_idx-1]), colorize); - } - } - - template - void print_stacktrace(IT begin, IT end, std::ostream& os, size_t thread_id, Colorize& colorize) { - print_header(os, thread_id); - for (; begin != end; ++begin) { - print_trace(os, *begin, colorize); - } - } - - void print_header(std::ostream& os, size_t thread_id) { - os << "Stack trace (most recent call last)"; - if (thread_id) { - os << " in thread " << thread_id; - } - os << ":\n"; - } - - void print_trace(std::ostream& os, const ResolvedTrace& trace, - Colorize& colorize) { - os << "#" - << std::left << std::setw(2) << trace.idx - << std::right; - bool already_indented = true; - - if (!trace.source.filename.size() || object) { - os << " Object \"" - << trace.object_filename - << "\", at " - << trace.addr - << ", in " - << trace.object_function - << "\n"; - already_indented = false; - } - - for (size_t inliner_idx = trace.inliners.size(); - inliner_idx > 0; --inliner_idx) { - if (!already_indented) { - os << " "; - } - const ResolvedTrace::SourceLoc& inliner_loc - = trace.inliners[inliner_idx-1]; - print_source_loc(os, " | ", inliner_loc); - if (snippet) { - print_snippet(os, " | ", inliner_loc, - colorize, Color::purple, inliner_context_size); - } - already_indented = false; - } - - if (trace.source.filename.size()) { - if (!already_indented) { - os << " "; - } - print_source_loc(os, " ", trace.source, trace.addr); - if (snippet) { - print_snippet(os, " ", trace.source, - colorize, Color::yellow, trace_context_size); - } - } - } - - void print_snippet(std::ostream& os, const char* indent, - const ResolvedTrace::SourceLoc& source_loc, - Colorize& colorize, Color::type color_code, - int context_size) - { - using namespace std; - typedef SnippetFactory::lines_t lines_t; - - lines_t lines = _snippets.get_snippet(source_loc.filename, - source_loc.line, static_cast(context_size)); - - for (lines_t::const_iterator it = lines.begin(); - it != lines.end(); ++it) { - if (it-> first == source_loc.line) { - colorize.set_color(color_code); - os << indent << ">"; - } else { - os << indent << " "; - } - os << std::setw(4) << it->first - << ": " - << it->second - << "\n"; - if (it-> first == source_loc.line) { - colorize.set_color(Color::reset); - } - } - } - - void print_source_loc(std::ostream& os, const char* indent, - const ResolvedTrace::SourceLoc& source_loc, - void* addr=nullptr) { - os << indent - << "Source \"" - << source_loc.filename - << "\", line " - << source_loc.line - << ", in " - << source_loc.function; - - if (address && addr != nullptr) { - os << " [" << addr << "]"; - } - os << "\n"; - } -}; - -/*************** SIGNALS HANDLING ***************/ - -#if defined(BACKWARD_SYSTEM_LINUX) || defined(BACKWARD_SYSTEM_DARWIN) - - -class SignalHandling { -public: - static std::vector make_default_signals() { - const int posix_signals[] = { - // Signals for which the default action is "Core". - SIGABRT, // Abort signal from abort(3) - SIGBUS, // Bus error (bad memory access) - SIGFPE, // Floating point exception - SIGILL, // Illegal Instruction - SIGIOT, // IOT trap. A synonym for SIGABRT - SIGQUIT, // Quit from keyboard - SIGSEGV, // Invalid memory reference - SIGSYS, // Bad argument to routine (SVr4) - SIGTRAP, // Trace/breakpoint trap - SIGXCPU, // CPU time limit exceeded (4.2BSD) - SIGXFSZ, // File size limit exceeded (4.2BSD) -#if defined(BACKWARD_SYSTEM_DARWIN) - SIGEMT, // emulation instruction executed -#endif - }; - return std::vector(posix_signals, posix_signals + sizeof posix_signals / sizeof posix_signals[0] ); - } - - SignalHandling(const std::vector& posix_signals = make_default_signals()): - _loaded(false) { - bool success = true; - - const size_t stack_size = 1024 * 1024 * 8; - _stack_content.reset(static_cast(malloc(stack_size))); - if (_stack_content) { - stack_t ss; - ss.ss_sp = _stack_content.get(); - ss.ss_size = stack_size; - ss.ss_flags = 0; - if (sigaltstack(&ss, nullptr) < 0) { - success = false; - } - } else { - success = false; - } - - for (size_t i = 0; i < posix_signals.size(); ++i) { - struct sigaction action; - memset(&action, 0, sizeof action); - action.sa_flags = static_cast(SA_SIGINFO | SA_ONSTACK | SA_NODEFER | - SA_RESETHAND); - sigfillset(&action.sa_mask); - sigdelset(&action.sa_mask, posix_signals[i]); -#if defined(__clang__) -#pragma clang diagnostic push -#pragma clang diagnostic ignored "-Wdisabled-macro-expansion" -#endif - action.sa_sigaction = &sig_handler; -#if defined(__clang__) -#pragma clang diagnostic pop -#endif - - int r = sigaction(posix_signals[i], &action, nullptr); - if (r < 0) success = false; - } - - _loaded = success; - } - - bool loaded() const { return _loaded; } - - static void handleSignal(int, siginfo_t* info, void* _ctx) { - ucontext_t *uctx = static_cast(_ctx); - - StackTrace st; - void* error_addr = nullptr; -#ifdef REG_RIP // x86_64 - error_addr = reinterpret_cast(uctx->uc_mcontext.gregs[REG_RIP]); -#elif defined(REG_EIP) // x86_32 - error_addr = reinterpret_cast(uctx->uc_mcontext.gregs[REG_EIP]); -#elif defined(__arm__) - error_addr = reinterpret_cast(uctx->uc_mcontext.arm_pc); -#elif defined(__aarch64__) - error_addr = reinterpret_cast(uctx->uc_mcontext.pc); -#elif defined(__ppc__) || defined(__powerpc) || defined(__powerpc__) || defined(__POWERPC__) - error_addr = reinterpret_cast(uctx->uc_mcontext.regs->nip); -#elif defined(__s390x__) - error_addr = reinterpret_cast(uctx->uc_mcontext.psw.addr); -#elif defined(__APPLE__) && defined(__x86_64__) - error_addr = reinterpret_cast(uctx->uc_mcontext->__ss.__rip); -#elif defined(__APPLE__) - error_addr = reinterpret_cast(uctx->uc_mcontext->__ss.__eip); -#else -# warning ":/ sorry, ain't know no nothing none not of your architecture!" -#endif - if (error_addr) { - st.load_from(error_addr, 32); - } else { - st.load_here(32); - } - - Printer printer; - printer.address = true; - printer.print(st, stderr); - -#if _XOPEN_SOURCE >= 700 || _POSIX_C_SOURCE >= 200809L - psiginfo(info, nullptr); -#else - (void)info; -#endif - } - -private: - details::handle _stack_content; - bool _loaded; - -#ifdef __GNUC__ - __attribute__((noreturn)) -#endif - static void sig_handler(int signo, siginfo_t* info, void* _ctx) { - handleSignal(signo, info, _ctx); - - // try to forward the signal. - raise(info->si_signo); - - // terminate the process immediately. - puts("watf? exit"); - _exit(EXIT_FAILURE); - } -}; - -#endif // BACKWARD_SYSTEM_LINUX || BACKWARD_SYSTEM_DARWIN - -#ifdef BACKWARD_SYSTEM_UNKNOWN - -class SignalHandling { -public: - SignalHandling(const std::vector& = std::vector()) {} - bool init() { return false; } - bool loaded() { return false; } -}; - -#endif // BACKWARD_SYSTEM_UNKNOWN - -} // namespace backward - -#endif /* H_GUARD */ diff --git a/dyn_planner/plan_manage/include/plan_manage/dyn_planner_manager.h b/dyn_planner/plan_manage/include/plan_manage/dyn_planner_manager.h deleted file mode 100644 index 01a3e5dec..000000000 --- a/dyn_planner/plan_manage/include/plan_manage/dyn_planner_manager.h +++ /dev/null @@ -1,87 +0,0 @@ -#ifndef _KGB_TRAJECTORY_GENERATOR_H_ -#define _KGB_TRAJECTORY_GENERATOR_H_ - -#include -#include -#include -#include -#include -#include - -namespace dyn_planner -{ -class DynPlannerManager -{ -private: - /* algorithm */ - // shared_ptr path_finder; - - EDTEnvironment::Ptr edt_env_; - - Astar::Ptr path_finder0_; - - KinodynamicAstar::Ptr path_finder_; - - BsplineOptimizer::Ptr bspline_optimizer_; - - double time_sample_; - double max_vel_; - - /* processing time */ - double time_search_ = 0.0; - double time_optimize_ = 0.0; - double time_adjust_ = 0.0; - - /* helper function */ - Eigen::Vector3d getFarPoint(const vector& path, Eigen::Vector3d x1, Eigen::Vector3d x2); - -public: - DynPlannerManager() - { - } - ~DynPlannerManager(); - - /* ---------- main API ---------- */ - /* generated traj */ - int traj_id_, dynamic_; - double traj_duration_, t_start_, t_end_, margin_, time_start_; - ros::Time time_traj_start_; - Eigen::Vector3d pos_traj_start_; - NonUniformBspline traj_pos_, traj_vel_, traj_acc_; - - /* guided optimization */ - NonUniformBspline traj_init_; - vector> guide_paths_; - vector guide_pts_; - - bool generateTrajectory(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel, Eigen::Vector3d start_acc, - Eigen::Vector3d end_pt, Eigen::Vector3d end_vel); // front-end && back-end - - bool orthoGradReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel, Eigen::Vector3d end_pt, - Eigen::Vector3d end_vel); // gradient-based replan using orthogonal gradient - - bool guidedGradReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel, Eigen::Vector3d end_pt, - Eigen::Vector3d end_vel); // gradient-based replan using guiding points - - void retrieveTrajectory(); - - void setParam(ros::NodeHandle& nh); - void setPathFinder0(const Astar::Ptr& finder); - void setPathFinder(const KinodynamicAstar::Ptr& finder); - void setOptimizer(const BsplineOptimizer::Ptr& optimizer); - void setEnvironment(const EDTEnvironment::Ptr& env); - - bool checkTrajCollision(); - - /* ---------- evaluation ---------- */ - void getSolvingTime(double& ts, double& to, double& ta); - void getCostCurve(vector& cost, vector& time) - { - bspline_optimizer_->getCostCurve(cost, time); - } - - typedef shared_ptr Ptr; -}; -} // namespace dyn_planner - -#endif \ No newline at end of file diff --git a/dyn_planner/plan_manage/include/plan_manage/planning_fsm.h b/dyn_planner/plan_manage/include/plan_manage/planning_fsm.h deleted file mode 100644 index 202cf5ef4..000000000 --- a/dyn_planner/plan_manage/include/plan_manage/planning_fsm.h +++ /dev/null @@ -1,102 +0,0 @@ -#ifndef _PLANNING_FSM_H_ -#define _PLANNING_FSM_H_ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include "plan_manage/Bspline.h" - -using std::vector; - -namespace dyn_planner -{ -class PlanningFSM -{ -private: - /* ---------- flag ---------- */ - bool trigger_, have_goal_; - enum EXEC_STATE - { - INIT, - WAIT_GOAL, - GEN_NEW_TRAJ, - REPLAN_TRAJ, - EXEC_TRAJ - }; - EXEC_STATE exec_state_; - - enum FLIGHT_TYPE - { - MANUAL_GOAL = 1, - PRESET_GOAL = 2 - }; - - void changeExecState(EXEC_STATE new_state, string pos_call); - void printExecState(); - - /* ---------- planning utils ---------- */ - SDFMap::Ptr sdf_map_; - - EDTEnvironment::Ptr edt_env_; - - Astar::Ptr path_finder0_; - KinodynamicAstar::Ptr path_finder_; - BsplineOptimizer::Ptr bspline_optimizer_; - - DynPlannerManager::Ptr planner_manager_; - - PlanningVisualization::Ptr visualization_; - - /* ---------- parameter ---------- */ - int flight_type_; // 1 mannual select, 2 hard code - double thresh_no_replan_, thresh_replan_; - double waypoints_[10][3]; - int wp_num_; - - /* ---------- planning api ---------- */ - Eigen::Vector3d start_pt_, start_vel_, start_acc_, end_pt_, end_vel_; - int current_wp_; - - bool planSearchOpt(); // front-end and back-end method - - /* ---------- sub and pub ---------- */ - ros::NodeHandle node_; - - ros::Timer exec_timer_, safety_timer_; - ros::Timer vis_timer_, query_timer_; - - ros::Subscriber waypoint_sub_; - - ros::Publisher replan_pub_, bspline_pub_; - - void execFSMCallback(const ros::TimerEvent& e); - void safetyCallback(const ros::TimerEvent& e); - - void waypointCallback(const nav_msgs::PathConstPtr& msg); - -public: - PlanningFSM(/* args */) - { - } - ~PlanningFSM() - { - } - - void init(ros::NodeHandle& nh); -}; - -} // namespace dyn_planner - -#endif \ No newline at end of file diff --git a/dyn_planner/plan_manage/launch/rviz.launch b/dyn_planner/plan_manage/launch/rviz.launch deleted file mode 100644 index 38b2b0e74..000000000 --- a/dyn_planner/plan_manage/launch/rviz.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/dyn_planner/plan_manage/launch/simulation.launch b/dyn_planner/plan_manage/launch/simulation.launch deleted file mode 100644 index d6fc3f2bf..000000000 --- a/dyn_planner/plan_manage/launch/simulation.launch +++ /dev/null @@ -1,103 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/dyn_planner/plan_manage/launch/simulator.xml b/dyn_planner/plan_manage/launch/simulator.xml deleted file mode 100644 index 89cf289be..000000000 --- a/dyn_planner/plan_manage/launch/simulator.xml +++ /dev/null @@ -1,79 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/dyn_planner/plan_manage/launch/test_collision.launch b/dyn_planner/plan_manage/launch/test_collision.launch deleted file mode 100644 index ae89d3c7d..000000000 --- a/dyn_planner/plan_manage/launch/test_collision.launch +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/dyn_planner/plan_manage/msg/Bspline.msg b/dyn_planner/plan_manage/msg/Bspline.msg deleted file mode 100644 index 446054d86..000000000 --- a/dyn_planner/plan_manage/msg/Bspline.msg +++ /dev/null @@ -1,6 +0,0 @@ -int32 order -int64 traj_id -float64[] knots -geometry_msgs/Point[] pts -time start_time - diff --git a/dyn_planner/plan_manage/package.xml b/dyn_planner/plan_manage/package.xml deleted file mode 100644 index 7325dbe69..000000000 --- a/dyn_planner/plan_manage/package.xml +++ /dev/null @@ -1,77 +0,0 @@ - - - plan_manage - 0.0.0 - The dyn_planner package - - - - - bzhouai - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - std_msgs - plan_env - path_searching - bspline_opt - traj_utils - message_runtime - - roscpp - std_msgs - - roscpp - std_msgs - plan_env - path_searching - bspline_opt - traj_utils - message_runtime - - - - - - - - diff --git a/dyn_planner/plan_manage/script/polyfit_predict.py b/dyn_planner/plan_manage/script/polyfit_predict.py deleted file mode 100644 index 9929377ca..000000000 --- a/dyn_planner/plan_manage/script/polyfit_predict.py +++ /dev/null @@ -1,80 +0,0 @@ -import numpy as np -import sympy as sp - -def polyfit(): - p0 = sp.symbols('p0') - p1 = sp.symbols('p1') - p2 = sp.symbols('p2') - p3 = sp.symbols('p3') - p4 = sp.symbols('p4') - p5 = sp.symbols('p5') - t = sp.symbols('t') - - # ti = np.array(0., 0.2, 0.4, 0.6, 0.8, 1.0, 1.2, 1.4, 1.6, 1.8, 2.0]) - - # poly = p0 + p1 * t + p2 * t**2 + p3 * t**3 + p4 * t**4 + p5 * t**5 - # print poly - - # print ti[0] - - # poly = p0 + p1 * ti[2] + p2 * ti[2]**2 + p3 * ti[2]**3 + p4 * ti[ - # 2]**4 + p5 * ti[2]**5 - # print poly - - # calc the estimation error terms - ti = sp.symbols('ti') - qi = sp.symbols('qi') - - est_err = (p0 + p1 * ti + p2 * ti**2 + p3 * ti**3 + p4 * ti**4 + p5 * ti**5 - qi)**2 - est_err = sp.expand(est_err) - print "est err:" - print est_err - print - - # calc the regulator - pos = p0 + p1 * t + p2 * t**2 + p3 * t**3 + p4 * t**4 + p5 * t**5 - acc = sp.diff(pos, t, 2) - acc =sp.simplify(acc) - - acc2 = acc *acc - # reg = sp.integrate(sp.sin(t), (t, 0, 3.141)) - t1 = sp.symbols('t1') - t2 = sp.symbols('t2') - reg = sp.integrate(acc2, (t, t1, t2)) - reg = sp.simplify(reg) - print 'reg:' - print reg - print - - # sum of cost - ld = sp.symbols('ld') - - sum = est_err + ld * reg - print 'sum:' - print sum - - - print 'grad:' - grad = sp.simplify(sp.diff(sum, p0)) - print grad - print - grad = sp.simplify(sp.diff(sum, p1)) - print grad - print - grad = sp.simplify(sp.diff(sum, p2)) - print grad - print - grad = sp.simplify(sp.diff(sum, p3)) - print grad - print - grad = sp.simplify(sp.diff(sum, p4)) - print grad - print - grad = sp.simplify(sp.diff(sum, p5)) - print grad - print - - - -if __name__ == "__main__": - polyfit() diff --git a/dyn_planner/plan_manage/script/traj_opt.py b/dyn_planner/plan_manage/script/traj_opt.py deleted file mode 100644 index bcfea191f..000000000 --- a/dyn_planner/plan_manage/script/traj_opt.py +++ /dev/null @@ -1,78 +0,0 @@ -import numpy as np -import sympy as sp - - -def traj_opt(): - print - - # ---------------------- variable ---------------------- - - # control points - q = sp.symbols('q0:10') - print q - # print q[10] - - # distance - dt = sp.symbols('dt') - d = sp.symbols('d:10') - print d - # print d[10] - - # dynamic - vm = sp.symbols('vm') - am = sp.symbols('am') - ts = sp.symbols('ts') - - lambda1 = sp.symbols('lambda1') - lambda2 = sp.symbols('lambda2') - lambda3 = sp.symbols('lambda3') - lambda4 = sp.symbols('lambda4') - - sp.pprint(lambda1) - sp.pprint(lambda3) - - # ---------------------- objective ---------------------- - - # smoothness, jerk - print '------------------------smoothness------------------------' - Js = 0 - - # for n in range(len(q)): - for i in range(10): - if i < 10 - 4: - Js += (q[i + 3] - 3 * q[i + 2] + 3 * q[i + 1] - q[i])**2 - - Js = sp.simplify(Js) - sp.pprint(Js) - print - print Js - - # collision - print '------------------------collision------------------------' - Jc = 0 - - # for n in range(len(q)): - for i in range(10): - Jc += (d[i] - dt)**2 - - Jc = sp.simplify(Jc) - sp.pprint(Jc) - print - print Jc - - # feasibility - print '------------------------feasibility-----------------------' - - Jv = 0 - - # velocity - for i in range(10): - if i < 10 - 1: - Jv += 1 - - - - - -if __name__ == "__main__": - traj_opt() \ No newline at end of file diff --git a/dyn_planner/plan_manage/src/dyn_planner_manager.cpp b/dyn_planner/plan_manage/src/dyn_planner_manager.cpp deleted file mode 100644 index 0c0b54525..000000000 --- a/dyn_planner/plan_manage/src/dyn_planner_manager.cpp +++ /dev/null @@ -1,220 +0,0 @@ -#include -#include - -namespace dyn_planner -{ -DynPlannerManager::~DynPlannerManager() -{ - traj_id_ = 0; -} - -void DynPlannerManager::setParam(ros::NodeHandle& nh) -{ - nh.param("manager/time_sample", time_sample_, -1.0); - nh.param("manager/max_vel", max_vel_, -1.0); - nh.param("manager/dynamic", dynamic_, -1); - nh.param("manager/margin", margin_, -1.0); -} - -void DynPlannerManager::setPathFinder0(const Astar::Ptr& finder) -{ - path_finder0_ = finder; -} - -void DynPlannerManager::setPathFinder(const KinodynamicAstar::Ptr& finder) -{ - path_finder_ = finder; -} - -void DynPlannerManager::setOptimizer(const BsplineOptimizer::Ptr& optimizer) -{ - bspline_optimizer_ = optimizer; -} - -void DynPlannerManager::setEnvironment(const EDTEnvironment::Ptr& env) -{ - edt_env_ = env; -} - -bool DynPlannerManager::checkTrajCollision() -{ - /* check collision */ - for (double t = t_start_; t <= t_end_; t += 0.02) - { - Eigen::Vector3d pos = traj_pos_.evaluateDeBoor(t); - double dist = dynamic_ ? edt_env_->evaluateCoarseEDT(pos, time_start_ + t - t_start_) : - edt_env_->evaluateCoarseEDT(pos, -1.0); - - if (dist < margin_) - { - return false; - } - } - - return true; -} - -void DynPlannerManager::retrieveTrajectory() -{ - traj_vel_ = traj_pos_.getDerivative(); - traj_acc_ = traj_vel_.getDerivative(); - - traj_pos_.getTimeSpan(t_start_, t_end_); - pos_traj_start_ = traj_pos_.evaluateDeBoor(t_start_); - traj_duration_ = t_end_ - t_start_; - - traj_id_ += 1; -} - -void DynPlannerManager::getSolvingTime(double& ts, double& to, double& ta) -{ - ts = time_search_; - to = time_optimize_; - ta = time_adjust_; -} - -bool DynPlannerManager::generateTrajectory(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel, - Eigen::Vector3d start_acc, Eigen::Vector3d end_pt, Eigen::Vector3d end_vel) -{ - std::cout << "[planner]: -----------------------" << std::endl; - cout << "start: " << start_pt.transpose() << ", " << start_vel.transpose() << ", " << start_acc.transpose() - << "\ngoal:" << end_pt.transpose() << ", " << end_vel.transpose() << endl; - - if ((start_pt - end_pt).norm() < 0.2) - { - cout << "Close goal" << endl; - return false; - } - - time_traj_start_ = ros::Time::now(); - time_start_ = -1.0; - - double t_search = 0.0, t_sample = 0.0, t_axb = 0.0, t_opt = 0.0, t_adjust = 0.0; - - Eigen::Vector3d init_pos = start_pt; - Eigen::Vector3d init_vel = start_vel; - Eigen::Vector3d init_acc = start_acc; - - ros::Time t1, t2; - t1 = ros::Time::now(); - /* ---------- search kino path ---------- */ - path_finder_->reset(); - - int status = path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel, true, dynamic_, time_start_); - if (status == KinodynamicAstar::NO_PATH) - { - cout << "[planner]: init search fail!" << endl; - path_finder_->reset(); - status = path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel, false, dynamic_, time_start_); - if (status == KinodynamicAstar::NO_PATH) - { - cout << "[planner]: Can't find path." << endl; - return false; - } - else - { - cout << "[planner]: retry search success." << endl; - } - } - else - { - cout << "[planner]: init search success." << endl; - } - - t2 = ros::Time::now(); - t_search = (t2 - t1).toSec(); - - /* ---------- bspline parameterization ---------- */ - t1 = ros::Time::now(); - - int K; - double ts = time_sample_ / max_vel_; - Eigen::MatrixXd vel_acc; - - Eigen::MatrixXd samples = path_finder_->getSamples(ts, K); - // cout << "ts: " << ts << endl; - // cout << "sample:\n" << samples.transpose() << endl; - - t2 = ros::Time::now(); - t_sample = (t2 - t1).toSec(); - - t1 = ros::Time::now(); - - Eigen::MatrixXd control_pts; - NonUniformBspline::getControlPointEqu3(samples, ts, control_pts); - - NonUniformBspline init = NonUniformBspline(control_pts, 3, ts); - - t2 = ros::Time::now(); - t_axb = (t2 - t1).toSec(); - - /* ---------- optimize trajectory ---------- */ - t1 = ros::Time::now(); - - // cout << "ctrl pts:" << control_pts << endl; - - bspline_optimizer_->setControlPoints(control_pts); - bspline_optimizer_->setBSplineInterval(ts); - - if (status != KinodynamicAstar::REACH_END) - bspline_optimizer_->optimize(BsplineOptimizer::SOFT_CONSTRAINT, dynamic_, time_start_); - else - bspline_optimizer_->optimize(BsplineOptimizer::HARD_CONSTRAINT, dynamic_, time_start_); - - control_pts = bspline_optimizer_->getControlPoints(); - - t2 = ros::Time::now(); - t_opt = (t2 - t1).toSec(); - - /* ---------- time adjustment ---------- */ - - t1 = ros::Time::now(); - NonUniformBspline pos = NonUniformBspline(control_pts, 3, ts); - - double tm, tmp, to, tn; - pos.getTimeSpan(tm, tmp); - to = tmp - tm; - - bool feasible = pos.checkFeasibility(false); - - int iter_num = 0; - while (!feasible && ros::ok()) - { - ++iter_num; - - feasible = pos.reallocateTime(); - /* actually this not needed, converges within 10 iteration */ - if (iter_num >= 50) - break; - } - - // cout << "[Main]: iter num: " << iter_num << endl; - pos.getTimeSpan(tm, tmp); - tn = tmp - tm; - cout << "[planner]: Reallocate ratio: " << tn / to << endl; - - t2 = ros::Time::now(); - t_adjust = (t2 - t1).toSec(); - - pos.checkFeasibility(true); - // drawVelAndAccPro(pos); - - /* save result */ - traj_pos_ = pos; - - double t_total = t_search + t_sample + t_axb + t_opt + t_adjust; - - cout << "[planner]: time: " << t_total << ", search: " << t_search << ", optimize: " << t_sample + t_axb + t_opt - << ", adjust time:" << t_adjust << endl; - - time_search_ = t_search; - time_optimize_ = t_sample + t_axb + t_opt; - time_adjust_ = t_adjust; - - time_traj_start_ = ros::Time::now(); - time_start_ = -1.0; - - return true; -} - -} // namespace dyn_planner diff --git a/dyn_planner/plan_manage/src/dyn_planner_node.cpp b/dyn_planner/plan_manage/src/dyn_planner_node.cpp deleted file mode 100644 index ed7719632..000000000 --- a/dyn_planner/plan_manage/src/dyn_planner_node.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#include -#include - -// #include -// #include -// #include -#include - -#include -namespace backward -{ -backward::SignalHandling sh; -} - -using namespace dyn_planner; - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "dyn_planner_node"); - ros::NodeHandle node; - ros::NodeHandle nh("~"); - - PlanningFSM fsm; - fsm.init(nh); - - ros::spin(); - - return 0; -} diff --git a/dyn_planner/plan_manage/src/planning_fsm.cpp b/dyn_planner/plan_manage/src/planning_fsm.cpp deleted file mode 100644 index 2082beb5d..000000000 --- a/dyn_planner/plan_manage/src/planning_fsm.cpp +++ /dev/null @@ -1,384 +0,0 @@ - -#include - -namespace dyn_planner -{ -void PlanningFSM::init(ros::NodeHandle& nh) -{ - /* ---------- init global param---------- */ - nh.param("bspline/limit_vel", NonUniformBspline::limit_vel_, -1.0); - nh.param("bspline/limit_acc", NonUniformBspline::limit_acc_, -1.0); - nh.param("bspline/limit_ratio", NonUniformBspline::limit_ratio_, -1.0); - - /* ---------- fsm param ---------- */ - nh.param("fsm/flight_type", flight_type_, -1); - nh.param("fsm/thresh_replan", thresh_replan_, -1.0); - nh.param("fsm/thresh_no_replan", thresh_no_replan_, -1.0); - nh.param("fsm/wp_num", wp_num_, -1); - - for (int i = 0; i < wp_num_; i++) - { - nh.param("fsm/wp" + to_string(i) + "_x", waypoints_[i][0], -1.0); - nh.param("fsm/wp" + to_string(i) + "_y", waypoints_[i][1], -1.0); - nh.param("fsm/wp" + to_string(i) + "_z", waypoints_[i][2], -1.0); - } - - current_wp_ = 0; - exec_state_ = EXEC_STATE::INIT; - have_goal_ = false; - - /* ---------- init edt environment ---------- */ - sdf_map_.reset(new SDFMap); - sdf_map_->init(nh); - - edt_env_.reset(new EDTEnvironment); - edt_env_->setMap(sdf_map_); - - /* ---------- init path finder and optimizer ---------- */ - path_finder0_.reset(new Astar); - path_finder0_->setParam(nh); - path_finder0_->setEnvironment(edt_env_); - path_finder0_->init(); - - path_finder_.reset(new KinodynamicAstar); - path_finder_->setParam(nh); - path_finder_->setEnvironment(edt_env_); - path_finder_->init(); - - bspline_optimizer_.reset(new BsplineOptimizer); - bspline_optimizer_->setParam(nh); - bspline_optimizer_->setEnvironment(edt_env_); - - planner_manager_.reset(new DynPlannerManager); - planner_manager_->setParam(nh); - planner_manager_->setPathFinder0(path_finder0_); - planner_manager_->setPathFinder(path_finder_); - planner_manager_->setOptimizer(bspline_optimizer_); - planner_manager_->setEnvironment(edt_env_); - - visualization_.reset(new PlanningVisualization(nh)); - - /* ---------- callback ---------- */ - exec_timer_ = node_.createTimer(ros::Duration(0.01), &PlanningFSM::execFSMCallback, this); - - safety_timer_ = node_.createTimer(ros::Duration(0.1), &PlanningFSM::safetyCallback, this); - - waypoint_sub_ = node_.subscribe("/waypoint_generator/waypoints", 1, &PlanningFSM::waypointCallback, this); - - replan_pub_ = node_.advertise("planning/replan", 10); - bspline_pub_ = node_.advertise("planning/bspline", 10); -} - - -void PlanningFSM::waypointCallback(const nav_msgs::PathConstPtr& msg) -{ - if (msg->poses[0].pose.position.z < 0.0) - return; - - cout << "Triggered!" << endl; - trigger_ = true; - - if (flight_type_ == FLIGHT_TYPE::MANUAL_GOAL) - { - end_pt_ << msg->poses[0].pose.position.x, msg->poses[0].pose.position.y, 1.0; - } - else if (flight_type_ == FLIGHT_TYPE::PRESET_GOAL) - { - end_pt_(0) = waypoints_[current_wp_][0]; - end_pt_(1) = waypoints_[current_wp_][1]; - end_pt_(2) = waypoints_[current_wp_][2]; - current_wp_ = (current_wp_ + 1) % wp_num_; - } - - visualization_->drawGoal(end_pt_, 0.3, Eigen::Vector4d(1, 0, 0, 1.0)); - end_vel_.setZero(); - have_goal_ = true; - - if (exec_state_ == WAIT_GOAL) - changeExecState(GEN_NEW_TRAJ, "TRIG"); - else if (exec_state_ == EXEC_TRAJ) - changeExecState(REPLAN_TRAJ, "TRIG"); -} - -void PlanningFSM::changeExecState(EXEC_STATE new_state, string pos_call) -{ - string state_str[5] = { "INIT", "WAIT_GOAL", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ" }; - int pre_s = int(exec_state_); - exec_state_ = new_state; - cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << endl; -} - -void PlanningFSM::printExecState() -{ - string state_str[5] = { "INIT", "WAIT_GOAL", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ" }; - - cout << "[FSM]: state: " + state_str[int(exec_state_)] << endl; -} - -void PlanningFSM::execFSMCallback(const ros::TimerEvent& e) -{ - static int fsm_num = 0; - fsm_num++; - if (fsm_num == 100) - { - printExecState(); - if (!edt_env_->odomValid()) - cout << "no odom." << endl; - if (!edt_env_->mapValid()) - cout << "no map." << endl; - if (!trigger_) - cout << "wait for goal." << endl; - fsm_num = 0; - } - switch (exec_state_) - { - case INIT: - { - if (!edt_env_->odomValid()) - { - return; - } - if (!edt_env_->mapValid()) - { - return; - } - if (!trigger_) - { - return; - } - changeExecState(WAIT_GOAL, "FSM"); - break; - } - - case WAIT_GOAL: - { - if (!have_goal_) - return; - else - { - changeExecState(GEN_NEW_TRAJ, "FSM"); - } - break; - } - - case GEN_NEW_TRAJ: - { - nav_msgs::Odometry odom = edt_env_->getOdom(); - start_pt_(0) = odom.pose.pose.position.x; - start_pt_(1) = odom.pose.pose.position.y; - start_pt_(2) = odom.pose.pose.position.z; - - start_vel_(0) = odom.twist.twist.linear.x; - start_vel_(1) = odom.twist.twist.linear.y; - start_vel_(2) = odom.twist.twist.linear.z; - start_acc_.setZero(); - - bool success = planSearchOpt(); - if (success) - { - changeExecState(EXEC_TRAJ, "FSM"); - } - else - { - // have_goal_ = false; - // changeExecState(WAIT_GOAL, "FSM"); - changeExecState(GEN_NEW_TRAJ, "FSM"); - } - break; - } - - case EXEC_TRAJ: - { - /* determine if need to replan */ - ros::Time time_now = ros::Time::now(); - double t_cur = (time_now - planner_manager_->time_traj_start_).toSec(); - t_cur = min(planner_manager_->traj_duration_, t_cur); - Eigen::Vector3d pos = planner_manager_->traj_pos_.evaluateDeBoor(planner_manager_->t_start_ + t_cur); - - /* && (end_pt_ - pos).norm() < 0.5 */ - if (t_cur > planner_manager_->traj_duration_ - 1e-2) - { - have_goal_ = false; - changeExecState(WAIT_GOAL, "FSM"); - return; - } - else if ((end_pt_ - pos).norm() < thresh_no_replan_) - { - // cout << "near end" << endl; - return; - } - else if ((planner_manager_->pos_traj_start_ - pos).norm() < thresh_replan_) - { - // cout << "near start" << endl; - return; - } - else - { - changeExecState(REPLAN_TRAJ, "FSM"); - } - break; - } - - case REPLAN_TRAJ: - { - ros::Time time_now = ros::Time::now(); - double t_cur = (time_now - planner_manager_->time_traj_start_).toSec(); - start_pt_ = planner_manager_->traj_pos_.evaluateDeBoor(planner_manager_->t_start_ + t_cur); - start_vel_ = planner_manager_->traj_vel_.evaluateDeBoor(planner_manager_->t_start_ + t_cur); - start_acc_ = planner_manager_->traj_acc_.evaluateDeBoor(planner_manager_->t_start_ + t_cur); - // cout << "t_cur: " << t_cur << endl; - // cout << "start pt: " << start_pt_.transpose() << endl; - - /* inform server */ - std_msgs::Empty replan_msg; - replan_pub_.publish(replan_msg); - - bool success = planSearchOpt(); - if (success) - { - changeExecState(EXEC_TRAJ, "FSM"); - } - else - { - // have_goal_ = false; - // changeExecState(WAIT_GOAL, "FSM"); - changeExecState(GEN_NEW_TRAJ, "FSM"); - } - break; - } - } -} - -void PlanningFSM::safetyCallback(const ros::TimerEvent& e) -{ - /* ---------- check goal safety ---------- */ - if (have_goal_) - { - double dist = - planner_manager_->dynamic_ ? - edt_env_->evaluateCoarseEDT(end_pt_, planner_manager_->time_start_ + planner_manager_->traj_duration_) : - edt_env_->evaluateCoarseEDT(end_pt_, -1.0); - - if (dist <= planner_manager_->margin_) - { - /* try to find a max distance goal around */ - bool new_goal = false; - const double dr = 0.5, dtheta = 30, dz = 0.3; - double new_x, new_y, new_z, max_dist = -1.0; - Eigen::Vector3d goal; - for (double r = dr; r <= 5 * dr + 1e-3; r += dr) - { - for (double theta = -90; theta <= 270; theta += dtheta) - { - for (double nz = 1 * dz; nz >= -1 * dz; nz -= dz) - { - new_x = end_pt_(0) + r * cos(theta / 57.3); - new_y = end_pt_(1) + r * sin(theta / 57.3); - new_z = end_pt_(2) + dz; - Eigen::Vector3d new_pt(new_x, new_y, new_z); - dist = planner_manager_->dynamic_ ? - edt_env_->evaluateCoarseEDT(new_pt, - planner_manager_->time_start_ + planner_manager_->traj_duration_) : - edt_env_->evaluateCoarseEDT(new_pt, -1.0); - if (dist > max_dist) - { - /* reset end_pt_ */ - goal(0) = new_x; - goal(1) = new_y; - goal(2) = new_z; - max_dist = dist; - } - } - } - } - - if (max_dist > planner_manager_->margin_) - { - cout << "change goal, replan." << endl; - end_pt_ = goal; - have_goal_ = true; - end_vel_.setZero(); - - if (exec_state_ == EXEC_TRAJ) - { - changeExecState(REPLAN_TRAJ, "SAFETY"); - } - - visualization_->drawGoal(end_pt_, 0.3, Eigen::Vector4d(1, 0, 0, 1.0)); - } - else - { - // have_goal_ = false; - // cout << "Goal near collision, stop." << endl; - // changeExecState(WAIT_GOAL, "SAFETY"); - cout << "goal near collision, keep retry" << endl; - changeExecState(REPLAN_TRAJ, "FSM"); - - std_msgs::Empty emt; - replan_pub_.publish(emt); - } - } - } - - /* ---------- check trajectory ---------- */ - if (exec_state_ == EXEC_STATE::EXEC_TRAJ) - { - bool safe = planner_manager_->checkTrajCollision(); - - if (!safe) - { - // cout << "current traj in collision." << endl; - ROS_WARN("current traj in collision."); - changeExecState(REPLAN_TRAJ, "SAFETY"); - } - } -} - -bool PlanningFSM::planSearchOpt() -{ - bool plan_success = planner_manager_->generateTrajectory(start_pt_, start_vel_, start_acc_, end_pt_, end_vel_); - - if (plan_success) - { - planner_manager_->retrieveTrajectory(); - - /* publish traj */ - plan_manage::Bspline bspline; - bspline.order = 3; - bspline.start_time = planner_manager_->time_traj_start_; - bspline.traj_id = planner_manager_->traj_id_; - Eigen::MatrixXd ctrl_pts = planner_manager_->traj_pos_.getControlPoint(); - for (int i = 0; i < ctrl_pts.rows(); ++i) - { - Eigen::Vector3d pvt = ctrl_pts.row(i); - geometry_msgs::Point pt; - pt.x = pvt(0); - pt.y = pvt(1); - pt.z = pvt(2); - bspline.pts.push_back(pt); - } - Eigen::VectorXd knots = planner_manager_->traj_pos_.getKnot(); - for (int i = 0; i < knots.rows(); ++i) - { - bspline.knots.push_back(knots(i)); - } - bspline_pub_.publish(bspline); - - /* visulization */ - vector kino_path = path_finder_->getKinoTraj(0.02); - - visualization_->drawPath(kino_path, 0.1, Eigen::Vector4d(1, 0, 0, 1)); - - visualization_->drawBspline(planner_manager_->traj_pos_, 0.1, Eigen::Vector4d(1.0, 1.0, 0.0, 1), true, 0.12, - Eigen::Vector4d(0, 1, 0, 1)); - return true; - } - else - { - cout << "generate new traj fail." << endl; - return false; - } -} - -// PlanningFSM:: -} // namespace dyn_planner diff --git a/dyn_planner/plan_manage/src/traj_server.cpp b/dyn_planner/plan_manage/src/traj_server.cpp deleted file mode 100644 index 8499d49dc..000000000 --- a/dyn_planner/plan_manage/src/traj_server.cpp +++ /dev/null @@ -1,243 +0,0 @@ -#include -#include "plan_manage/Bspline.h" -#include "bspline_opt/non_uniform_bspline.h" -#include "nav_msgs/Odometry.h" -#include "quadrotor_msgs/PositionCommand.h" -#include "std_msgs/Empty.h" -#include "visualization_msgs/Marker.h" - -using namespace dyn_planner; - -ros::Publisher state_pub, pos_cmd_pub, traj_pub; - -nav_msgs::Odometry odom; - -quadrotor_msgs::PositionCommand cmd; -// double pos_gain[3] = {5.7, 5.7, 6.2}; -// double vel_gain[3] = {3.4, 3.4, 4.0}; -double pos_gain[3] = {5.7, 5.7, 6.2}; -double vel_gain[3] = {3.4, 3.4, 4.0}; - -bool receive_traj = false; -vector traj; -ros::Time time_traj_start; -int traj_id; -double traj_duration; -double t_cmd_start, t_cmd_end; - -vector traj_cmd, traj_real; - -Eigen::Vector3d hover_pt; - -void displayTrajWithColor(vector path, double resolution, - Eigen::Vector4d color, int id) { - visualization_msgs::Marker mk; - mk.header.frame_id = "world"; - mk.header.stamp = ros::Time::now(); - mk.type = visualization_msgs::Marker::SPHERE_LIST; - mk.action = visualization_msgs::Marker::DELETE; - mk.id = id; - - traj_pub.publish(mk); - - mk.action = visualization_msgs::Marker::ADD; - mk.pose.orientation.x = 0.0; - mk.pose.orientation.y = 0.0; - mk.pose.orientation.z = 0.0; - mk.pose.orientation.w = 1.0; - - mk.color.r = color(0); - mk.color.g = color(1); - mk.color.b = color(2); - mk.color.a = color(3); - - mk.scale.x = resolution; - mk.scale.y = resolution; - mk.scale.z = resolution; - - geometry_msgs::Point pt; - for (int i = 0; i < int(path.size()); i++) { - pt.x = path[i](0); - pt.y = path[i](1); - pt.z = path[i](2); - mk.points.push_back(pt); - } - traj_pub.publish(mk); - ros::Duration(0.001).sleep(); -} - -void drawState(Eigen::Vector3d pos, Eigen::Vector3d vec, int id, - Eigen::Vector4d color) { - visualization_msgs::Marker mk_state; - mk_state.header.frame_id = "world"; - mk_state.header.stamp = ros::Time::now(); - mk_state.id = id; - mk_state.type = visualization_msgs::Marker::ARROW; - mk_state.action = visualization_msgs::Marker::ADD; - mk_state.pose.orientation.w = 1.0; - mk_state.scale.x = 0.1; - mk_state.scale.y = 0.2; - mk_state.scale.z = 0.3; - geometry_msgs::Point pt; - pt.x = pos(0); - pt.y = pos(1); - pt.z = pos(2); - mk_state.points.push_back(pt); - pt.x = pos(0) + vec(0); - pt.y = pos(1) + vec(1); - pt.z = pos(2) + vec(2); - mk_state.points.push_back(pt); - mk_state.color.r = color(0); - mk_state.color.g = color(1); - mk_state.color.b = color(2); - mk_state.color.a = color(3); - state_pub.publish(mk_state); -} - -void bsplineCallback(plan_manage::BsplineConstPtr msg) { - Eigen::VectorXd knots(msg->knots.size()); - for (int i = 0; i < msg->knots.size(); ++i) { - knots(i) = msg->knots[i]; - } - - Eigen::MatrixXd ctrl_pts(msg->pts.size(), 3); - for (int i = 0; i < msg->pts.size(); ++i) { - Eigen::Vector3d pt; - pt(0) = msg->pts[i].x; - pt(1) = msg->pts[i].y; - pt(2) = msg->pts[i].z; - ctrl_pts.row(i) = pt.transpose(); - } - - NonUniformBspline bspline(ctrl_pts, msg->order, 0.1); - bspline.setKnot(knots); - - time_traj_start = msg->start_time; - traj_id = msg->traj_id; - - traj.clear(); - traj.push_back(bspline); - traj.push_back(traj[0].getDerivative()); - traj.push_back(traj[1].getDerivative()); - - traj[0].getTimeSpan(t_cmd_start, t_cmd_end); - traj_duration = t_cmd_end - t_cmd_start; - - receive_traj = true; -} - -void replanCallback(std_msgs::Empty msg) { - /* reset duration */ - const double time_out = 0.25; - ros::Time time_now = ros::Time::now(); - double t_stop = (time_now - time_traj_start).toSec() + time_out; - traj_duration = min(t_stop, traj_duration); - t_cmd_end = t_cmd_start + traj_duration; -} - -void odomCallbck(const nav_msgs::Odometry& msg) { - if (msg.child_frame_id == "X" || msg.child_frame_id == "O") return; - - odom = msg; - - traj_real.push_back(Eigen::Vector3d(odom.pose.pose.position.x, - odom.pose.pose.position.y, - odom.pose.pose.position.z)); - - if (traj_real.size() > 10000) - traj_real.erase(traj_real.begin(), traj_real.begin() + 1000); -} - -void visCallback(const ros::TimerEvent& e) { - displayTrajWithColor(traj_real, 0.03, Eigen::Vector4d(0.925, 0.054, 0.964, 1), - 1); - displayTrajWithColor(traj_cmd, 0.03, Eigen::Vector4d(1, 1, 0, 1), 2); -} - -void cmdCallback(const ros::TimerEvent& e) { - /* no publishing before receive traj */ - if (!receive_traj) return; - - ros::Time time_now = ros::Time::now(); - double t_cur = (time_now - time_traj_start).toSec(); - - Eigen::Vector3d pos, vel, acc; - - if (t_cur < traj_duration && t_cur >= 0.0) { - pos = traj[0].evaluateDeBoor(t_cmd_start + t_cur); - vel = traj[1].evaluateDeBoor(t_cmd_start + t_cur); - acc = traj[2].evaluateDeBoor(t_cmd_start + t_cur); - } else if (t_cur >= traj_duration) { - /* hover when finish traj */ - pos = traj[0].evaluateDeBoor(t_cmd_end); - vel.setZero(); - acc.setZero(); - } else { - cout << "[Traj server]: invalid time." << endl; - } - - cmd.header.stamp = time_now; - cmd.header.frame_id = "world"; - cmd.trajectory_flag = - quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_READY; - cmd.trajectory_id = traj_id; - - cmd.position.x = pos(0); - cmd.position.y = pos(1); - cmd.position.z = pos(2); - - cmd.velocity.x = vel(0); - cmd.velocity.y = vel(1); - cmd.velocity.z = vel(2); - - cmd.acceleration.x = acc(0); - cmd.acceleration.y = acc(1); - cmd.acceleration.z = acc(2); - - pos_cmd_pub.publish(cmd); - - drawState(pos, vel, 0, Eigen::Vector4d(0, 1, 0, 1)); - drawState(pos, acc, 1, Eigen::Vector4d(0, 0, 1, 1)); - - traj_cmd.push_back(pos); - if (pos.size() > 10000) - traj_cmd.erase(traj_cmd.begin(), traj_cmd.begin() + 1000); -} - -int main(int argc, char** argv) { - ros::init(argc, argv, "traj_server"); - ros::NodeHandle node; - - ros::Subscriber bspline_sub = - node.subscribe("planning/bspline", 10, bsplineCallback); - - ros::Subscriber replan_sub = - node.subscribe("planning/replan", 10, replanCallback); - - ros::Subscriber odom_sub = node.subscribe("/odom_world", 50, odomCallbck); - - ros::Timer cmd_timer = node.createTimer(ros::Duration(0.01), cmdCallback); - state_pub = node.advertise("planning/state", 10); - pos_cmd_pub = - node.advertise("/position_cmd", 50); - - ros::Timer vis_timer = node.createTimer(ros::Duration(0.5), visCallback); - traj_pub = node.advertise("planning/traj", 10); - - /* control parameter */ - cmd.kx[0] = pos_gain[0]; - cmd.kx[1] = pos_gain[1]; - cmd.kx[2] = pos_gain[2]; - - cmd.kv[0] = vel_gain[0]; - cmd.kv[1] = vel_gain[1]; - cmd.kv[2] = vel_gain[2]; - - ros::Duration(1.0).sleep(); - - cout << "[Traj server]: ready." << endl; - - ros::spin(); - - return 0; -} diff --git a/dyn_planner/traj_utils/CMakeLists.txt b/dyn_planner/traj_utils/CMakeLists.txt deleted file mode 100644 index b768f72c7..000000000 --- a/dyn_planner/traj_utils/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(traj_utils) - -find_package(catkin REQUIRED COMPONENTS - bspline_opt - path_searching - roscpp - std_msgs -) - -find_package(Eigen3 REQUIRED) -find_package(PCL 1.7 REQUIRED) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES traj_utils - CATKIN_DEPENDS path_searching bspline_opt -# DEPENDS system_lib -) - -include_directories( - SYSTEM - include - ${catkin_INCLUDE_DIRS} - ${Eigen3_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -link_directories(${PCL_LIBRARY_DIRS}) - -set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS} -O3 -Wall") - -add_library( traj_utils - src/planning_visualization.cpp - ) -target_link_libraries( traj_utils - ${catkin_LIBRARIES} - ) \ No newline at end of file diff --git a/dyn_planner/traj_utils/include/traj_utils/planning_visualization.h b/dyn_planner/traj_utils/include/traj_utils/planning_visualization.h deleted file mode 100644 index dd623f587..000000000 --- a/dyn_planner/traj_utils/include/traj_utils/planning_visualization.h +++ /dev/null @@ -1,53 +0,0 @@ -#ifndef _PLANNING_VISUALIZATION_H_ -#define _PLANNING_VISUALIZATION_H_ - -#include -#include -#include -#include -#include -#include -#include - -using std::vector; -namespace dyn_planner -{ -class PlanningVisualization -{ -private: - enum DRAW_ID - { - GOAL = 1, - PATH = 200, - BSPLINE = 300, - BSPLINE_CTRL_PT = 400, - PREDICTION = 500 - }; - - /* data */ - ros::NodeHandle node; - ros::Publisher traj_pub; - - void displaySphereList(vector list, double resolution, Eigen::Vector4d color, int id); - -public: - PlanningVisualization(/* args */) - { - } - ~PlanningVisualization() - { - } - - PlanningVisualization(ros::NodeHandle& nh); - - void drawPath(vector path, double resolution, Eigen::Vector4d color, int id = 0); - - void drawBspline(NonUniformBspline bspline, double size, Eigen::Vector4d color, bool show_ctrl_pts = false, - double size2 = 0.1, Eigen::Vector4d color2 = Eigen::Vector4d(1, 1, 0, 1), int id1 = 0, int id2 = 0); - - void drawGoal(Eigen::Vector3d goal, double resolution, Eigen::Vector4d color, int id = 0); - - typedef std::shared_ptr Ptr; -}; -} // namespace dyn_planner -#endif \ No newline at end of file diff --git a/dyn_planner/traj_utils/package.xml b/dyn_planner/traj_utils/package.xml deleted file mode 100644 index 8952bdfa9..000000000 --- a/dyn_planner/traj_utils/package.xml +++ /dev/null @@ -1,71 +0,0 @@ - - - traj_utils - 0.0.0 - The traj_utils package - - - - - bzhouai - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - bspline_opt - path_searching - roscpp - std_msgs - bspline_opt - path_searching - roscpp - std_msgs - bspline_opt - path_searching - roscpp - std_msgs - - - - - - - - diff --git a/dyn_planner/traj_utils/src/planning_visualization.cpp b/dyn_planner/traj_utils/src/planning_visualization.cpp deleted file mode 100644 index 9d91bd4a0..000000000 --- a/dyn_planner/traj_utils/src/planning_visualization.cpp +++ /dev/null @@ -1,79 +0,0 @@ -#include - -using std::cout; -using std::endl; -namespace dyn_planner -{ -PlanningVisualization::PlanningVisualization(ros::NodeHandle& nh) -{ - node = nh; - - traj_pub = node.advertise("/planning_vis/trajectory", 10); -} - -void PlanningVisualization::displaySphereList(vector list, double resolution, Eigen::Vector4d color, - int id) -{ - visualization_msgs::Marker mk; - mk.header.frame_id = "world"; - mk.header.stamp = ros::Time::now(); - mk.type = visualization_msgs::Marker::SPHERE_LIST; - mk.action = visualization_msgs::Marker::DELETE; - mk.id = id; - traj_pub.publish(mk); - - mk.action = visualization_msgs::Marker::ADD; - mk.pose.orientation.x = 0.0, mk.pose.orientation.y = 0.0, mk.pose.orientation.z = 0.0, mk.pose.orientation.w = 1.0; - mk.color.r = color(0), mk.color.g = color(1), mk.color.b = color(2), mk.color.a = color(3); - mk.scale.x = resolution, mk.scale.y = resolution, mk.scale.z = resolution; - geometry_msgs::Point pt; - for (int i = 0; i < int(list.size()); i++) - { - pt.x = list[i](0), pt.y = list[i](1), pt.z = list[i](2); - mk.points.push_back(pt); - } - traj_pub.publish(mk); -} - -void PlanningVisualization::drawBspline(NonUniformBspline bspline, double size, Eigen::Vector4d color, - bool show_ctrl_pts, double size2, Eigen::Vector4d color2, int id1, int id2) -{ - vector traj_pts; - double tm, tmp; - bspline.getTimeSpan(tm, tmp); - for (double t = tm; t <= tmp; t += 0.01) - { - Eigen::Vector3d pt = bspline.evaluateDeBoor(t); - traj_pts.push_back(pt); - } - displaySphereList(traj_pts, size, color, BSPLINE + id1 % 100); - - // draw the control point - if (!show_ctrl_pts) - return; - - Eigen::MatrixXd ctrl_pts = bspline.getControlPoint(); - - vector ctp; - for (int i = 0; i < int(ctrl_pts.rows()); ++i) - { - Eigen::Vector3d pt = ctrl_pts.row(i).transpose(); - ctp.push_back(pt); - } - displaySphereList(ctp, size2, color2, BSPLINE_CTRL_PT + id2 % 100); -} - -void PlanningVisualization::drawGoal(Eigen::Vector3d goal, double resolution, Eigen::Vector4d color, int id) -{ - vector goal_vec = { goal }; - - displaySphereList(goal_vec, resolution, color, GOAL + id % 100); -} - -void PlanningVisualization::drawPath(vector path, double resolution, Eigen::Vector4d color, int id) -{ - displaySphereList(path, resolution, color, PATH + id % 100); -} - -// PlanningVisualization:: -} // namespace dyn_planner \ No newline at end of file diff --git a/fast_planner b/fast_planner new file mode 160000 index 000000000..ff9615e5f --- /dev/null +++ b/fast_planner @@ -0,0 +1 @@ +Subproject commit ff9615e5f0932e56b1f9e415ba071472c16dd4bb diff --git a/files/bib.txt b/files/bib.txt new file mode 100644 index 000000000..0571e1881 --- /dev/null +++ b/files/bib.txt @@ -0,0 +1,10 @@ +@article{zhou2019robust, + title={Robust and efficient quadrotor trajectory generation for fast autonomous flight}, + author={Zhou, Boyu and Gao, Fei and Wang, Luqi and Liu, Chuhao and Shen, Shaojie}, + journal={IEEE Robotics and Automation Letters}, + volume={4}, + number={4}, + pages={3529--3536}, + year={2019}, + publisher={IEEE} +} \ No newline at end of file diff --git a/img/exp1.gif b/files/exp1.gif similarity index 100% rename from img/exp1.gif rename to files/exp1.gif diff --git a/img/exp2.gif b/files/exp2.gif similarity index 100% rename from img/exp2.gif rename to files/exp2.gif diff --git a/img/exp3.gif b/files/exp3.gif similarity index 100% rename from img/exp3.gif rename to files/exp3.gif diff --git a/img/title.png b/files/title.png similarity index 100% rename from img/title.png rename to files/title.png diff --git a/uav_simulator/README.md b/uav_simulator/README.md deleted file mode 100644 index da4dc4ff5..000000000 --- a/uav_simulator/README.md +++ /dev/null @@ -1,2 +0,0 @@ -# simulator and related utils - diff --git a/uav_simulator/odom_visualization/CMakeLists.txt b/uav_simulator/Utils/odom_visualization/CMakeLists.txt similarity index 100% rename from uav_simulator/odom_visualization/CMakeLists.txt rename to uav_simulator/Utils/odom_visualization/CMakeLists.txt diff --git a/uav_simulator/odom_visualization/Makefile b/uav_simulator/Utils/odom_visualization/Makefile similarity index 100% rename from uav_simulator/odom_visualization/Makefile rename to uav_simulator/Utils/odom_visualization/Makefile diff --git a/uav_simulator/odom_visualization/bin/odom_visualization b/uav_simulator/Utils/odom_visualization/bin/odom_visualization similarity index 100% rename from uav_simulator/odom_visualization/bin/odom_visualization rename to uav_simulator/Utils/odom_visualization/bin/odom_visualization diff --git a/uav_simulator/odom_visualization/bin/odom_visualization_vicon45 b/uav_simulator/Utils/odom_visualization/bin/odom_visualization_vicon45 similarity index 100% rename from uav_simulator/odom_visualization/bin/odom_visualization_vicon45 rename to uav_simulator/Utils/odom_visualization/bin/odom_visualization_vicon45 diff --git a/uav_simulator/odom_visualization/build/CATKIN_IGNORE b/uav_simulator/Utils/odom_visualization/build/CATKIN_IGNORE similarity index 100% rename from uav_simulator/odom_visualization/build/CATKIN_IGNORE rename to uav_simulator/Utils/odom_visualization/build/CATKIN_IGNORE diff --git a/uav_simulator/odom_visualization/build/CMakeCache.txt b/uav_simulator/Utils/odom_visualization/build/CMakeCache.txt similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeCache.txt rename to uav_simulator/Utils/odom_visualization/build/CMakeCache.txt diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/CMakeDirectoryInformation.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/CMakeDirectoryInformation.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/CMakeDirectoryInformation.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/CMakeDirectoryInformation.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/CMakeError.log b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/CMakeError.log similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/CMakeError.log rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/CMakeError.log diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/CMakeOutput.log b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/CMakeOutput.log similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/CMakeOutput.log rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/CMakeOutput.log diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/CMakeRuleHashes.txt b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/CMakeRuleHashes.txt similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/CMakeRuleHashes.txt rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/CMakeRuleHashes.txt diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/Makefile.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/Makefile.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/Makefile.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/Makefile.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/Makefile2 b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/Makefile2 similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/Makefile2 rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/Makefile2 diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_cpp.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_genmsg_lisp.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_cpp.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/ROSBUILD_gensrv_lisp.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/TargetDirectories.txt b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/TargetDirectories.txt similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/TargetDirectories.txt rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/TargetDirectories.txt diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/_catkin_empty_exported_target.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/clean_test_results.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/clean_test_results.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/clean_test_results.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/clean_test_results.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/clean_test_results.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/clean_test_results.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/clean_test_results.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/clean_test_results.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/cmake.check_cache b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/cmake.check_cache similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/cmake.check_cache rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/cmake.check_cache diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/doxygen.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/doxygen.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/doxygen.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/doxygen.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/doxygen.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/doxygen.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/doxygen.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/doxygen.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/doxygen.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/doxygen.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/doxygen.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/doxygen.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/doxygen.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/doxygen.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/doxygen.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/doxygen.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/CXX.includecache b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/CXX.includecache similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/CXX.includecache rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/CXX.includecache diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/depend.internal b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/depend.internal similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/depend.internal rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/depend.internal diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/depend.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/depend.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/depend.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/depend.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/flags.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/flags.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/flags.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/flags.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/link.txt b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/link.txt similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/link.txt rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/link.txt diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/src/odom_visualization.cpp.o b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/src/odom_visualization.cpp.o similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/odom_visualization.dir/src/odom_visualization.cpp.o rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/odom_visualization.dir/src/odom_visualization.cpp.o diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/progress.marks b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/progress.marks similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/progress.marks rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/progress.marks diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_clean-test-results.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/depend.internal b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/depend.internal similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/depend.internal rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/depend.internal diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/depend.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/depend.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/depend.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/depend.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_precompile.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rosbuild_premsgsrvgen.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/depend.internal b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/depend.internal similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/depend.internal rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/depend.internal diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/depend.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/depend.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/depend.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/depend.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_genmsg_libexe.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/rospack_gensrv.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/run_tests.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/run_tests.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/run_tests.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/run_tests.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/run_tests.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/run_tests.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/run_tests.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/run_tests.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/run_tests.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/run_tests.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/run_tests.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/run_tests.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/run_tests.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/run_tests.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/run_tests.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/run_tests.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/test-future.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-future.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/test-future.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-future.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/test-future.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-future.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/test-future.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-future.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/test-future.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-future.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/test-future.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-future.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/test-future.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-future.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/test-future.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-future.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/test-results-run.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-results-run.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/test-results-run.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-results-run.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/test-results-run.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-results-run.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/test-results-run.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-results-run.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/test-results-run.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-results-run.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/test-results-run.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-results-run.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/test-results-run.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-results-run.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/test-results-run.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-results-run.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/test-results.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-results.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/test-results.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-results.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/test-results.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-results.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/test-results.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-results.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/test-results.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-results.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/test-results.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-results.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/test-results.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-results.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/test-results.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/test-results.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/test.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/test.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/test.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/test.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/test.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/test.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/test.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/test.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/test.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/test.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/test.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/test.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/test.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/test.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/test.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/test.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/tests.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/tests.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/tests.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/tests.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/tests.dir/build.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/tests.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/tests.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/tests.dir/build.make diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/tests.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/tests.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/tests.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/tests.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/CMakeFiles/tests.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/CMakeFiles/tests.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/CMakeFiles/tests.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/CMakeFiles/tests.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/Makefile b/uav_simulator/Utils/odom_visualization/build/Makefile similarity index 100% rename from uav_simulator/odom_visualization/build/Makefile rename to uav_simulator/Utils/odom_visualization/build/Makefile diff --git a/uav_simulator/odom_visualization/build/catkin/catkin_generated/version/package.cmake b/uav_simulator/Utils/odom_visualization/build/catkin/catkin_generated/version/package.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/catkin/catkin_generated/version/package.cmake rename to uav_simulator/Utils/odom_visualization/build/catkin/catkin_generated/version/package.cmake diff --git a/uav_simulator/odom_visualization/build/catkin_generated/env_cached.sh b/uav_simulator/Utils/odom_visualization/build/catkin_generated/env_cached.sh similarity index 100% rename from uav_simulator/odom_visualization/build/catkin_generated/env_cached.sh rename to uav_simulator/Utils/odom_visualization/build/catkin_generated/env_cached.sh diff --git a/uav_simulator/odom_visualization/build/catkin_generated/generate_cached_setup.py b/uav_simulator/Utils/odom_visualization/build/catkin_generated/generate_cached_setup.py similarity index 100% rename from uav_simulator/odom_visualization/build/catkin_generated/generate_cached_setup.py rename to uav_simulator/Utils/odom_visualization/build/catkin_generated/generate_cached_setup.py diff --git a/uav_simulator/odom_visualization/build/catkin_generated/installspace/.rosinstall b/uav_simulator/Utils/odom_visualization/build/catkin_generated/installspace/.rosinstall similarity index 100% rename from uav_simulator/odom_visualization/build/catkin_generated/installspace/.rosinstall rename to uav_simulator/Utils/odom_visualization/build/catkin_generated/installspace/.rosinstall diff --git a/uav_simulator/odom_visualization/build/catkin_generated/installspace/_setup_util.py b/uav_simulator/Utils/odom_visualization/build/catkin_generated/installspace/_setup_util.py similarity index 100% rename from uav_simulator/odom_visualization/build/catkin_generated/installspace/_setup_util.py rename to uav_simulator/Utils/odom_visualization/build/catkin_generated/installspace/_setup_util.py diff --git a/uav_simulator/odom_visualization/build/catkin_generated/installspace/env.sh b/uav_simulator/Utils/odom_visualization/build/catkin_generated/installspace/env.sh similarity index 100% rename from uav_simulator/odom_visualization/build/catkin_generated/installspace/env.sh rename to uav_simulator/Utils/odom_visualization/build/catkin_generated/installspace/env.sh diff --git a/uav_simulator/odom_visualization/build/catkin_generated/installspace/setup.bash b/uav_simulator/Utils/odom_visualization/build/catkin_generated/installspace/setup.bash similarity index 100% rename from uav_simulator/odom_visualization/build/catkin_generated/installspace/setup.bash rename to uav_simulator/Utils/odom_visualization/build/catkin_generated/installspace/setup.bash diff --git a/uav_simulator/odom_visualization/build/catkin_generated/installspace/setup.sh b/uav_simulator/Utils/odom_visualization/build/catkin_generated/installspace/setup.sh similarity index 100% rename from uav_simulator/odom_visualization/build/catkin_generated/installspace/setup.sh rename to uav_simulator/Utils/odom_visualization/build/catkin_generated/installspace/setup.sh diff --git a/uav_simulator/odom_visualization/build/catkin_generated/installspace/setup.zsh b/uav_simulator/Utils/odom_visualization/build/catkin_generated/installspace/setup.zsh similarity index 100% rename from uav_simulator/odom_visualization/build/catkin_generated/installspace/setup.zsh rename to uav_simulator/Utils/odom_visualization/build/catkin_generated/installspace/setup.zsh diff --git a/uav_simulator/odom_visualization/build/catkin_generated/ordered_paths.cmake b/uav_simulator/Utils/odom_visualization/build/catkin_generated/ordered_paths.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/catkin_generated/ordered_paths.cmake rename to uav_simulator/Utils/odom_visualization/build/catkin_generated/ordered_paths.cmake diff --git a/uav_simulator/odom_visualization/build/catkin_generated/setup_cached.sh b/uav_simulator/Utils/odom_visualization/build/catkin_generated/setup_cached.sh similarity index 100% rename from uav_simulator/odom_visualization/build/catkin_generated/setup_cached.sh rename to uav_simulator/Utils/odom_visualization/build/catkin_generated/setup_cached.sh diff --git a/uav_simulator/odom_visualization/build/catkin_generated/stamps/odom_visualization/interrogate_setup_dot_py.py.stamp b/uav_simulator/Utils/odom_visualization/build/catkin_generated/stamps/odom_visualization/interrogate_setup_dot_py.py.stamp similarity index 100% rename from uav_simulator/odom_visualization/build/catkin_generated/stamps/odom_visualization/interrogate_setup_dot_py.py.stamp rename to uav_simulator/Utils/odom_visualization/build/catkin_generated/stamps/odom_visualization/interrogate_setup_dot_py.py.stamp diff --git a/uav_simulator/odom_visualization/build/catkin_generated/stamps/odom_visualization/package.xml.stamp b/uav_simulator/Utils/odom_visualization/build/catkin_generated/stamps/odom_visualization/package.xml.stamp similarity index 100% rename from uav_simulator/odom_visualization/build/catkin_generated/stamps/odom_visualization/package.xml.stamp rename to uav_simulator/Utils/odom_visualization/build/catkin_generated/stamps/odom_visualization/package.xml.stamp diff --git a/uav_simulator/odom_visualization/build/cmake_install.cmake b/uav_simulator/Utils/odom_visualization/build/cmake_install.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/cmake_install.cmake rename to uav_simulator/Utils/odom_visualization/build/cmake_install.cmake diff --git a/uav_simulator/odom_visualization/build/devel/.catkin b/uav_simulator/Utils/odom_visualization/build/devel/.catkin similarity index 100% rename from uav_simulator/odom_visualization/build/devel/.catkin rename to uav_simulator/Utils/odom_visualization/build/devel/.catkin diff --git a/uav_simulator/odom_visualization/build/devel/.rosinstall b/uav_simulator/Utils/odom_visualization/build/devel/.rosinstall similarity index 100% rename from uav_simulator/odom_visualization/build/devel/.rosinstall rename to uav_simulator/Utils/odom_visualization/build/devel/.rosinstall diff --git a/uav_simulator/odom_visualization/build/devel/_setup_util.py b/uav_simulator/Utils/odom_visualization/build/devel/_setup_util.py similarity index 100% rename from uav_simulator/odom_visualization/build/devel/_setup_util.py rename to uav_simulator/Utils/odom_visualization/build/devel/_setup_util.py diff --git a/uav_simulator/odom_visualization/build/devel/env.sh b/uav_simulator/Utils/odom_visualization/build/devel/env.sh similarity index 100% rename from uav_simulator/odom_visualization/build/devel/env.sh rename to uav_simulator/Utils/odom_visualization/build/devel/env.sh diff --git a/uav_simulator/odom_visualization/build/devel/etc/catkin/profile.d/05.catkin-test-results.sh b/uav_simulator/Utils/odom_visualization/build/devel/etc/catkin/profile.d/05.catkin-test-results.sh similarity index 100% rename from uav_simulator/odom_visualization/build/devel/etc/catkin/profile.d/05.catkin-test-results.sh rename to uav_simulator/Utils/odom_visualization/build/devel/etc/catkin/profile.d/05.catkin-test-results.sh diff --git a/uav_simulator/odom_visualization/build/devel/etc/catkin/profile.d/05.catkin_make.bash b/uav_simulator/Utils/odom_visualization/build/devel/etc/catkin/profile.d/05.catkin_make.bash similarity index 100% rename from uav_simulator/odom_visualization/build/devel/etc/catkin/profile.d/05.catkin_make.bash rename to uav_simulator/Utils/odom_visualization/build/devel/etc/catkin/profile.d/05.catkin_make.bash diff --git a/uav_simulator/odom_visualization/build/devel/etc/catkin/profile.d/05.catkin_make_isolated.bash b/uav_simulator/Utils/odom_visualization/build/devel/etc/catkin/profile.d/05.catkin_make_isolated.bash similarity index 100% rename from uav_simulator/odom_visualization/build/devel/etc/catkin/profile.d/05.catkin_make_isolated.bash rename to uav_simulator/Utils/odom_visualization/build/devel/etc/catkin/profile.d/05.catkin_make_isolated.bash diff --git a/uav_simulator/odom_visualization/build/devel/setup.bash b/uav_simulator/Utils/odom_visualization/build/devel/setup.bash similarity index 100% rename from uav_simulator/odom_visualization/build/devel/setup.bash rename to uav_simulator/Utils/odom_visualization/build/devel/setup.bash diff --git a/uav_simulator/odom_visualization/build/devel/setup.sh b/uav_simulator/Utils/odom_visualization/build/devel/setup.sh similarity index 100% rename from uav_simulator/odom_visualization/build/devel/setup.sh rename to uav_simulator/Utils/odom_visualization/build/devel/setup.sh diff --git a/uav_simulator/odom_visualization/build/devel/setup.zsh b/uav_simulator/Utils/odom_visualization/build/devel/setup.zsh similarity index 100% rename from uav_simulator/odom_visualization/build/devel/setup.zsh rename to uav_simulator/Utils/odom_visualization/build/devel/setup.zsh diff --git a/uav_simulator/odom_visualization/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake b/uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake rename to uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake diff --git a/uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest.dir/build.make b/uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest.dir/build.make diff --git a/uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest.dir/depend.make b/uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest.dir/depend.make similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest.dir/depend.make rename to uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest.dir/depend.make diff --git a/uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest.dir/flags.make b/uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest.dir/flags.make similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest.dir/flags.make rename to uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest.dir/flags.make diff --git a/uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest.dir/link.txt b/uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest.dir/link.txt similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest.dir/link.txt rename to uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest.dir/link.txt diff --git a/uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake b/uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake rename to uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake diff --git a/uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/build.make b/uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/build.make similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/build.make rename to uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/build.make diff --git a/uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake b/uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake rename to uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake diff --git a/uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/depend.make b/uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/depend.make similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/depend.make rename to uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/depend.make diff --git a/uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/flags.make b/uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/flags.make similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/flags.make rename to uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/flags.make diff --git a/uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/link.txt b/uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/link.txt similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/link.txt rename to uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/link.txt diff --git a/uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/progress.make b/uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/progress.make similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/progress.make rename to uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/gtest_main.dir/progress.make diff --git a/uav_simulator/odom_visualization/build/gtest/CMakeFiles/progress.marks b/uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/progress.marks similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/CMakeFiles/progress.marks rename to uav_simulator/Utils/odom_visualization/build/gtest/CMakeFiles/progress.marks diff --git a/uav_simulator/odom_visualization/build/gtest/Makefile b/uav_simulator/Utils/odom_visualization/build/gtest/Makefile similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/Makefile rename to uav_simulator/Utils/odom_visualization/build/gtest/Makefile diff --git a/uav_simulator/odom_visualization/build/gtest/cmake_install.cmake b/uav_simulator/Utils/odom_visualization/build/gtest/cmake_install.cmake similarity index 100% rename from uav_simulator/odom_visualization/build/gtest/cmake_install.cmake rename to uav_simulator/Utils/odom_visualization/build/gtest/cmake_install.cmake diff --git a/uav_simulator/odom_visualization/mainpage.dox b/uav_simulator/Utils/odom_visualization/mainpage.dox similarity index 100% rename from uav_simulator/odom_visualization/mainpage.dox rename to uav_simulator/Utils/odom_visualization/mainpage.dox diff --git a/uav_simulator/odom_visualization/meshes/hummingbird.mesh b/uav_simulator/Utils/odom_visualization/meshes/hummingbird.mesh similarity index 100% rename from uav_simulator/odom_visualization/meshes/hummingbird.mesh rename to uav_simulator/Utils/odom_visualization/meshes/hummingbird.mesh diff --git a/uav_simulator/odom_visualization/package.xml b/uav_simulator/Utils/odom_visualization/package.xml similarity index 100% rename from uav_simulator/odom_visualization/package.xml rename to uav_simulator/Utils/odom_visualization/package.xml diff --git a/uav_simulator/odom_visualization/src/odom_visualization.cpp b/uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp similarity index 100% rename from uav_simulator/odom_visualization/src/odom_visualization.cpp rename to uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp diff --git a/uav_simulator/odom_visualization/src/odom_visualization.cpp~ b/uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp~ similarity index 100% rename from uav_simulator/odom_visualization/src/odom_visualization.cpp~ rename to uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp~ diff --git a/uav_simulator/quadrotor_msgs/CMakeLists.txt b/uav_simulator/Utils/quadrotor_msgs/CMakeLists.txt similarity index 100% rename from uav_simulator/quadrotor_msgs/CMakeLists.txt rename to uav_simulator/Utils/quadrotor_msgs/CMakeLists.txt diff --git a/uav_simulator/quadrotor_msgs/cmake/FindEigen3.cmake b/uav_simulator/Utils/quadrotor_msgs/cmake/FindEigen3.cmake similarity index 100% rename from uav_simulator/quadrotor_msgs/cmake/FindEigen3.cmake rename to uav_simulator/Utils/quadrotor_msgs/cmake/FindEigen3.cmake diff --git a/uav_simulator/quadrotor_msgs/include/quadrotor_msgs/comm_types.h b/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/comm_types.h similarity index 100% rename from uav_simulator/quadrotor_msgs/include/quadrotor_msgs/comm_types.h rename to uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/comm_types.h diff --git a/uav_simulator/quadrotor_msgs/include/quadrotor_msgs/decode_msgs.h b/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/decode_msgs.h similarity index 100% rename from uav_simulator/quadrotor_msgs/include/quadrotor_msgs/decode_msgs.h rename to uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/decode_msgs.h diff --git a/uav_simulator/quadrotor_msgs/include/quadrotor_msgs/encode_msgs.h b/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/encode_msgs.h similarity index 100% rename from uav_simulator/quadrotor_msgs/include/quadrotor_msgs/encode_msgs.h rename to uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/encode_msgs.h diff --git a/uav_simulator/quadrotor_msgs/msg/AuxCommand.msg b/uav_simulator/Utils/quadrotor_msgs/msg/AuxCommand.msg similarity index 100% rename from uav_simulator/quadrotor_msgs/msg/AuxCommand.msg rename to uav_simulator/Utils/quadrotor_msgs/msg/AuxCommand.msg diff --git a/uav_simulator/quadrotor_msgs/msg/Corrections.msg b/uav_simulator/Utils/quadrotor_msgs/msg/Corrections.msg similarity index 100% rename from uav_simulator/quadrotor_msgs/msg/Corrections.msg rename to uav_simulator/Utils/quadrotor_msgs/msg/Corrections.msg diff --git a/uav_simulator/quadrotor_msgs/msg/Gains.msg b/uav_simulator/Utils/quadrotor_msgs/msg/Gains.msg similarity index 100% rename from uav_simulator/quadrotor_msgs/msg/Gains.msg rename to uav_simulator/Utils/quadrotor_msgs/msg/Gains.msg diff --git a/uav_simulator/quadrotor_msgs/msg/LQRTrajectory.msg b/uav_simulator/Utils/quadrotor_msgs/msg/LQRTrajectory.msg similarity index 100% rename from uav_simulator/quadrotor_msgs/msg/LQRTrajectory.msg rename to uav_simulator/Utils/quadrotor_msgs/msg/LQRTrajectory.msg diff --git a/uav_simulator/quadrotor_msgs/msg/Odometry.msg b/uav_simulator/Utils/quadrotor_msgs/msg/Odometry.msg similarity index 100% rename from uav_simulator/quadrotor_msgs/msg/Odometry.msg rename to uav_simulator/Utils/quadrotor_msgs/msg/Odometry.msg diff --git a/uav_simulator/quadrotor_msgs/msg/OutputData.msg b/uav_simulator/Utils/quadrotor_msgs/msg/OutputData.msg similarity index 100% rename from uav_simulator/quadrotor_msgs/msg/OutputData.msg rename to uav_simulator/Utils/quadrotor_msgs/msg/OutputData.msg diff --git a/uav_simulator/quadrotor_msgs/msg/PPROutputData.msg b/uav_simulator/Utils/quadrotor_msgs/msg/PPROutputData.msg similarity index 100% rename from uav_simulator/quadrotor_msgs/msg/PPROutputData.msg rename to uav_simulator/Utils/quadrotor_msgs/msg/PPROutputData.msg diff --git a/uav_simulator/quadrotor_msgs/msg/PolynomialTrajectory.msg b/uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg similarity index 100% rename from uav_simulator/quadrotor_msgs/msg/PolynomialTrajectory.msg rename to uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg diff --git a/uav_simulator/quadrotor_msgs/msg/PolynomialTrajectory.msg~ b/uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg~ similarity index 100% rename from uav_simulator/quadrotor_msgs/msg/PolynomialTrajectory.msg~ rename to uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg~ diff --git a/uav_simulator/quadrotor_msgs/msg/PositionCommand.msg b/uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg similarity index 100% rename from uav_simulator/quadrotor_msgs/msg/PositionCommand.msg rename to uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg diff --git a/uav_simulator/quadrotor_msgs/msg/PositionCommand.msg~ b/uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg~ similarity index 100% rename from uav_simulator/quadrotor_msgs/msg/PositionCommand.msg~ rename to uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg~ diff --git a/uav_simulator/quadrotor_msgs/msg/SO3Command.msg b/uav_simulator/Utils/quadrotor_msgs/msg/SO3Command.msg similarity index 100% rename from uav_simulator/quadrotor_msgs/msg/SO3Command.msg rename to uav_simulator/Utils/quadrotor_msgs/msg/SO3Command.msg diff --git a/uav_simulator/quadrotor_msgs/msg/Serial.msg b/uav_simulator/Utils/quadrotor_msgs/msg/Serial.msg similarity index 100% rename from uav_simulator/quadrotor_msgs/msg/Serial.msg rename to uav_simulator/Utils/quadrotor_msgs/msg/Serial.msg diff --git a/uav_simulator/quadrotor_msgs/msg/StatusData.msg b/uav_simulator/Utils/quadrotor_msgs/msg/StatusData.msg similarity index 100% rename from uav_simulator/quadrotor_msgs/msg/StatusData.msg rename to uav_simulator/Utils/quadrotor_msgs/msg/StatusData.msg diff --git a/uav_simulator/quadrotor_msgs/msg/TRPYCommand.msg b/uav_simulator/Utils/quadrotor_msgs/msg/TRPYCommand.msg similarity index 100% rename from uav_simulator/quadrotor_msgs/msg/TRPYCommand.msg rename to uav_simulator/Utils/quadrotor_msgs/msg/TRPYCommand.msg diff --git a/uav_simulator/quadrotor_msgs/package.xml b/uav_simulator/Utils/quadrotor_msgs/package.xml similarity index 100% rename from uav_simulator/quadrotor_msgs/package.xml rename to uav_simulator/Utils/quadrotor_msgs/package.xml diff --git a/uav_simulator/quadrotor_msgs/src/decode_msgs.cpp b/uav_simulator/Utils/quadrotor_msgs/src/decode_msgs.cpp similarity index 100% rename from uav_simulator/quadrotor_msgs/src/decode_msgs.cpp rename to uav_simulator/Utils/quadrotor_msgs/src/decode_msgs.cpp diff --git a/uav_simulator/quadrotor_msgs/src/encode_msgs.cpp b/uav_simulator/Utils/quadrotor_msgs/src/encode_msgs.cpp similarity index 100% rename from uav_simulator/quadrotor_msgs/src/encode_msgs.cpp rename to uav_simulator/Utils/quadrotor_msgs/src/encode_msgs.cpp diff --git a/uav_simulator/quadrotor_msgs/src/quadrotor_msgs/__init__.py b/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/__init__.py similarity index 100% rename from uav_simulator/quadrotor_msgs/src/quadrotor_msgs/__init__.py rename to uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/__init__.py diff --git a/uav_simulator/quadrotor_msgs/src/quadrotor_msgs/__init__.pyc b/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/__init__.pyc similarity index 100% rename from uav_simulator/quadrotor_msgs/src/quadrotor_msgs/__init__.pyc rename to uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/__init__.pyc diff --git a/uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_AuxCommand.py b/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_AuxCommand.py similarity index 100% rename from uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_AuxCommand.py rename to uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_AuxCommand.py diff --git a/uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_Corrections.py b/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Corrections.py similarity index 100% rename from uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_Corrections.py rename to uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Corrections.py diff --git a/uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_Gains.py b/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Gains.py similarity index 100% rename from uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_Gains.py rename to uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Gains.py diff --git a/uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_OutputData.py b/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_OutputData.py similarity index 100% rename from uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_OutputData.py rename to uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_OutputData.py diff --git a/uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_PPROutputData.py b/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_PPROutputData.py similarity index 100% rename from uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_PPROutputData.py rename to uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_PPROutputData.py diff --git a/uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_PositionCommand.py b/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_PositionCommand.py similarity index 100% rename from uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_PositionCommand.py rename to uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_PositionCommand.py diff --git a/uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_SO3Command.py b/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_SO3Command.py similarity index 100% rename from uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_SO3Command.py rename to uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_SO3Command.py diff --git a/uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_Serial.py b/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Serial.py similarity index 100% rename from uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_Serial.py rename to uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Serial.py diff --git a/uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_StatusData.py b/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_StatusData.py similarity index 100% rename from uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_StatusData.py rename to uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_StatusData.py diff --git a/uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_TRPYCommand.py b/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_TRPYCommand.py similarity index 100% rename from uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/_TRPYCommand.py rename to uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_TRPYCommand.py diff --git a/uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/__init__.py b/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/__init__.py similarity index 100% rename from uav_simulator/quadrotor_msgs/src/quadrotor_msgs/msg/__init__.py rename to uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/__init__.py diff --git a/uav_simulator/Utils/rviz_plugins/CMakeLists.txt b/uav_simulator/Utils/rviz_plugins/CMakeLists.txt index 5d464b964..27323201c 100644 --- a/uav_simulator/Utils/rviz_plugins/CMakeLists.txt +++ b/uav_simulator/Utils/rviz_plugins/CMakeLists.txt @@ -79,7 +79,6 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${QT_LIBRARIES} ) - install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/uav_simulator/Utils/rviz_plugins/src/gamelikeinput.cpp b/uav_simulator/Utils/rviz_plugins/src/gamelikeinput.cpp index 3f98500f1..b30730a01 100644 --- a/uav_simulator/Utils/rviz_plugins/src/gamelikeinput.cpp +++ b/uav_simulator/Utils/rviz_plugins/src/gamelikeinput.cpp @@ -11,7 +11,7 @@ #include "rviz/viewport_mouse_event.h" #undef private -#include "gamelikeinput.hpp" +// #include "gamelikeinput.hpp" #include "rviz/default_plugin/tools/move_tool.h" #include "rviz/display_context.h" @@ -37,7 +37,7 @@ #include "rviz/properties/string_property.h" #include "nav_msgs/Path.h" -#include "quadrotor_msgs/SwarmCommand.h" +// #include "quadrotor_msgs/SwarmCommand.h" #include "std_msgs/Int32MultiArray.h" //! @todo rewrite this grabage code diff --git a/uav_simulator/Utils/waypoint_generator/src/waypoint_generator.cpp b/uav_simulator/Utils/waypoint_generator/src/waypoint_generator.cpp index 7f6d26e44..1e1d4c472 100755 --- a/uav_simulator/Utils/waypoint_generator/src/waypoint_generator.cpp +++ b/uav_simulator/Utils/waypoint_generator/src/waypoint_generator.cpp @@ -170,7 +170,7 @@ void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) { } else if (waypoint_type == string("series")) { load_waypoints(n, trigged_time); } else if (waypoint_type == string("manual-lonely-waypoint")) { - if (msg->pose.position.z > 0) { + if (msg->pose.position.z > -0.1) { // if height > 0, it's a valid goal; geometry_msgs::PoseStamped pt = *msg; waypoints.poses.clear(); diff --git a/uav_simulator/local_sensing/.vscode/settings.json b/uav_simulator/local_sensing/.vscode/settings.json new file mode 100644 index 000000000..9ef3eef07 --- /dev/null +++ b/uav_simulator/local_sensing/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "files.associations": { + "*.cuh": "cpp" + } +} \ No newline at end of file diff --git a/uav_simulator/local_sensing/CMakeLists.txt b/uav_simulator/local_sensing/CMakeLists.txt new file mode 100644 index 000000000..a8685468a --- /dev/null +++ b/uav_simulator/local_sensing/CMakeLists.txt @@ -0,0 +1,89 @@ +PROJECT(local_sensing_node) +CMAKE_MINIMUM_REQUIRED(VERSION 2.8.3) +SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + +set(ENABLE_CUDA false) +# set(ENABLE_CUDA true) + +if(ENABLE_CUDA) + find_package(CUDA REQUIRED) + SET(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS};-O3 -use_fast_math) + set(CUDA_NVCC_FLAGS +# -gencode arch=compute_20,code=sm_20; +# -gencode arch=compute_20,code=sm_21; +# -gencode arch=compute_30,code=sm_30; +# -gencode arch=compute_35,code=sm_35; +# -gencode arch=compute_50,code=sm_50; +# -gencode arch=compute_52,code=sm_52; +# -gencode arch=compute_60,code=sm_60; + -gencode arch=compute_61,code=sm_61; + ) + + SET(CUDA_PROPAGATE_HOST_FLAGS OFF) + + find_package(OpenCV REQUIRED) + find_package(Eigen3 REQUIRED) + find_package(Boost REQUIRED COMPONENTS system filesystem) + + find_package(catkin REQUIRED COMPONENTS + roscpp roslib cmake_modules cv_bridge image_transport pcl_ros sensor_msgs geometry_msgs nav_msgs quadrotor_msgs dynamic_reconfigure) + generate_dynamic_reconfigure_options( + cfg/local_sensing_node.cfg + ) + catkin_package( + DEPENDS OpenCV Eigen Boost + CATKIN_DEPENDS roscpp roslib image_transport pcl_ros + #INCLUDE_DIRS include + LIBRARIES depth_render_cuda + ) + + include_directories( + SYSTEM + #include + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${Eigen_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ) + + CUDA_ADD_LIBRARY( depth_render_cuda + src/depth_render.cu + ) + + add_executable( + pcl_render_node + src/pcl_render_node.cpp + ) + target_link_libraries( pcl_render_node + depth_render_cuda + ${OpenCV_LIBS} + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ) +else(ENABLE_CUDA) + find_package(Eigen3 REQUIRED) + find_package(catkin REQUIRED COMPONENTS + roscpp roslib cmake_modules pcl_ros sensor_msgs geometry_msgs nav_msgs quadrotor_msgs) + + catkin_package( + DEPENDS Eigen + CATKIN_DEPENDS roscpp roslib pcl_ros + ) + + include_directories( + SYSTEM + ${catkin_INCLUDE_DIRS} + ${Eigen_INCLUDE_DIRS} + ) + + add_executable( + pcl_render_node + src/pointcloud_render_node.cpp + ) + + target_link_libraries( pcl_render_node + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} + ) +endif(ENABLE_CUDA) diff --git a/uav_simulator/local_sensing/CMakeModules/FindCUDA.cmake b/uav_simulator/local_sensing/CMakeModules/FindCUDA.cmake new file mode 100644 index 000000000..8afb632dc --- /dev/null +++ b/uav_simulator/local_sensing/CMakeModules/FindCUDA.cmake @@ -0,0 +1,1311 @@ +# - Tools for building CUDA C files: libraries and build dependencies. +# This script locates the NVIDIA CUDA C tools. It should work on linux, windows, +# and mac and should be reasonably up to date with CUDA C releases. +# +# This script makes use of the standard find_package arguments of , +# REQUIRED and QUIET. CUDA_FOUND will report if an acceptable version of CUDA +# was found. +# +# The script will prompt the user to specify CUDA_TOOLKIT_ROOT_DIR if the prefix +# cannot be determined by the location of nvcc in the system path and REQUIRED +# is specified to find_package(). To use a different installed version of the +# toolkit set the environment variable CUDA_BIN_PATH before running cmake +# (e.g. CUDA_BIN_PATH=/usr/local/cuda1.0 instead of the default /usr/local/cuda) +# or set CUDA_TOOLKIT_ROOT_DIR after configuring. If you change the value of +# CUDA_TOOLKIT_ROOT_DIR, various components that depend on the path will be +# relocated. +# +# It might be necessary to set CUDA_TOOLKIT_ROOT_DIR manually on certain +# platforms, or to use a cuda runtime not installed in the default location. In +# newer versions of the toolkit the cuda library is included with the graphics +# driver- be sure that the driver version matches what is needed by the cuda +# runtime version. +# +# The following variables affect the behavior of the macros in the script (in +# alphebetical order). Note that any of these flags can be changed multiple +# times in the same directory before calling CUDA_ADD_EXECUTABLE, +# CUDA_ADD_LIBRARY, CUDA_COMPILE, CUDA_COMPILE_PTX or CUDA_WRAP_SRCS. +# +# CUDA_64_BIT_DEVICE_CODE (Default matches host bit size) +# -- Set to ON to compile for 64 bit device code, OFF for 32 bit device code. +# Note that making this different from the host code when generating object +# or C files from CUDA code just won't work, because size_t gets defined by +# nvcc in the generated source. If you compile to PTX and then load the +# file yourself, you can mix bit sizes between device and host. +# +# CUDA_ATTACH_VS_BUILD_RULE_TO_CUDA_FILE (Default ON) +# -- Set to ON if you want the custom build rule to be attached to the source +# file in Visual Studio. Turn OFF if you add the same cuda file to multiple +# targets. +# +# This allows the user to build the target from the CUDA file; however, bad +# things can happen if the CUDA source file is added to multiple targets. +# When performing parallel builds it is possible for the custom build +# command to be run more than once and in parallel causing cryptic build +# errors. VS runs the rules for every source file in the target, and a +# source can have only one rule no matter how many projects it is added to. +# When the rule is run from multiple targets race conditions can occur on +# the generated file. Eventually everything will get built, but if the user +# is unaware of this behavior, there may be confusion. It would be nice if +# this script could detect the reuse of source files across multiple targets +# and turn the option off for the user, but no good solution could be found. +# +# CUDA_BUILD_CUBIN (Default OFF) +# -- Set to ON to enable and extra compilation pass with the -cubin option in +# Device mode. The output is parsed and register, shared memory usage is +# printed during build. +# +# CUDA_BUILD_EMULATION (Default OFF for device mode) +# -- Set to ON for Emulation mode. -D_DEVICEEMU is defined for CUDA C files +# when CUDA_BUILD_EMULATION is TRUE. +# +# CUDA_GENERATED_OUTPUT_DIR (Default CMAKE_CURRENT_BINARY_DIR) +# -- Set to the path you wish to have the generated files placed. If it is +# blank output files will be placed in CMAKE_CURRENT_BINARY_DIR. +# Intermediate files will always be placed in +# CMAKE_CURRENT_BINARY_DIR/CMakeFiles. +# +# CUDA_HOST_COMPILATION_CPP (Default ON) +# -- Set to OFF for C compilation of host code. +# +# CUDA_NVCC_FLAGS +# CUDA_NVCC_FLAGS_ +# -- Additional NVCC command line arguments. NOTE: multiple arguments must be +# semi-colon delimited (e.g. --compiler-options;-Wall) +# +# CUDA_PROPAGATE_HOST_FLAGS (Default ON) +# -- Set to ON to propagate CMAKE_{C,CXX}_FLAGS and their configuration +# dependent counterparts (e.g. CMAKE_C_FLAGS_DEBUG) automatically to the +# host compiler through nvcc's -Xcompiler flag. This helps make the +# generated host code match the rest of the system better. Sometimes +# certain flags give nvcc problems, and this will help you turn the flag +# propagation off. This does not affect the flags supplied directly to nvcc +# via CUDA_NVCC_FLAGS or through the OPTION flags specified through +# CUDA_ADD_LIBRARY, CUDA_ADD_EXECUTABLE, or CUDA_WRAP_SRCS. Flags used for +# shared library compilation are not affected by this flag. +# +# CUDA_VERBOSE_BUILD (Default OFF) +# -- Set to ON to see all the commands used when building the CUDA file. When +# using a Makefile generator the value defaults to VERBOSE (run make +# VERBOSE=1 to see output), although setting CUDA_VERBOSE_BUILD to ON will +# always print the output. +# +# The script creates the following macros (in alphebetical order): +# +# CUDA_ADD_CUFFT_TO_TARGET( cuda_target ) +# -- Adds the cufft library to the target (can be any target). Handles whether +# you are in emulation mode or not. +# +# CUDA_ADD_CUBLAS_TO_TARGET( cuda_target ) +# -- Adds the cublas library to the target (can be any target). Handles +# whether you are in emulation mode or not. +# +# CUDA_ADD_EXECUTABLE( cuda_target file0 file1 ... +# [WIN32] [MACOSX_BUNDLE] [EXCLUDE_FROM_ALL] [OPTIONS ...] ) +# -- Creates an executable "cuda_target" which is made up of the files +# specified. All of the non CUDA C files are compiled using the standard +# build rules specified by CMAKE and the cuda files are compiled to object +# files using nvcc and the host compiler. In addition CUDA_INCLUDE_DIRS is +# added automatically to include_directories(). Some standard CMake target +# calls can be used on the target after calling this macro +# (e.g. set_target_properties and target_link_libraries), but setting +# properties that adjust compilation flags will not affect code compiled by +# nvcc. Such flags should be modified before calling CUDA_ADD_EXECUTABLE, +# CUDA_ADD_LIBRARY or CUDA_WRAP_SRCS. +# +# CUDA_ADD_LIBRARY( cuda_target file0 file1 ... +# [STATIC | SHARED | MODULE] [EXCLUDE_FROM_ALL] [OPTIONS ...] ) +# -- Same as CUDA_ADD_EXECUTABLE except that a library is created. +# +# CUDA_BUILD_CLEAN_TARGET() +# -- Creates a convience target that deletes all the dependency files +# generated. You should make clean after running this target to ensure the +# dependency files get regenerated. +# +# CUDA_COMPILE( generated_files file0 file1 ... [STATIC | SHARED | MODULE] +# [OPTIONS ...] ) +# -- Returns a list of generated files from the input source files to be used +# with ADD_LIBRARY or ADD_EXECUTABLE. +# +# CUDA_COMPILE_PTX( generated_files file0 file1 ... [OPTIONS ...] ) +# -- Returns a list of PTX files generated from the input source files. +# +# CUDA_INCLUDE_DIRECTORIES( path0 path1 ... ) +# -- Sets the directories that should be passed to nvcc +# (e.g. nvcc -Ipath0 -Ipath1 ... ). These paths usually contain other .cu +# files. +# +# CUDA_WRAP_SRCS ( cuda_target format generated_files file0 file1 ... +# [STATIC | SHARED | MODULE] [OPTIONS ...] ) +# -- This is where all the magic happens. CUDA_ADD_EXECUTABLE, +# CUDA_ADD_LIBRARY, CUDA_COMPILE, and CUDA_COMPILE_PTX all call this +# function under the hood. +# +# Given the list of files (file0 file1 ... fileN) this macro generates +# custom commands that generate either PTX or linkable objects (use "PTX" or +# "OBJ" for the format argument to switch). Files that don't end with .cu +# or have the HEADER_FILE_ONLY property are ignored. +# +# The arguments passed in after OPTIONS are extra command line options to +# give to nvcc. You can also specify per configuration options by +# specifying the name of the configuration followed by the options. General +# options must preceed configuration specific options. Not all +# configurations need to be specified, only the ones provided will be used. +# +# OPTIONS -DFLAG=2 "-DFLAG_OTHER=space in flag" +# DEBUG -g +# RELEASE --use_fast_math +# RELWITHDEBINFO --use_fast_math;-g +# MINSIZEREL --use_fast_math +# +# For certain configurations (namely VS generating object files with +# CUDA_ATTACH_VS_BUILD_RULE_TO_CUDA_FILE set to ON), no generated file will +# be produced for the given cuda file. This is because when you add the +# cuda file to Visual Studio it knows that this file produces an object file +# and will link in the resulting object file automatically. +# +# This script will also generate a separate cmake script that is used at +# build time to invoke nvcc. This is for several reasons. +# +# 1. nvcc can return negative numbers as return values which confuses +# Visual Studio into thinking that the command succeeded. The script now +# checks the error codes and produces errors when there was a problem. +# +# 2. nvcc has been known to not delete incomplete results when it +# encounters problems. This confuses build systems into thinking the +# target was generated when in fact an unusable file exists. The script +# now deletes the output files if there was an error. +# +# 3. By putting all the options that affect the build into a file and then +# make the build rule dependent on the file, the output files will be +# regenerated when the options change. +# +# This script also looks at optional arguments STATIC, SHARED, or MODULE to +# determine when to target the object compilation for a shared library. +# BUILD_SHARED_LIBS is ignored in CUDA_WRAP_SRCS, but it is respected in +# CUDA_ADD_LIBRARY. On some systems special flags are added for building +# objects intended for shared libraries. A preprocessor macro, +# _EXPORTS is defined when a shared library compilation is +# detected. +# +# Flags passed into add_definitions with -D or /D are passed along to nvcc. +# +# The script defines the following variables: +# +# CUDA_VERSION_MAJOR -- The major version of cuda as reported by nvcc. +# CUDA_VERSION_MINOR -- The minor version. +# CUDA_VERSION +# CUDA_VERSION_STRING -- CUDA_VERSION_MAJOR.CUDA_VERSION_MINOR +# +# CUDA_TOOLKIT_ROOT_DIR -- Path to the CUDA Toolkit (defined if not set). +# CUDA_SDK_ROOT_DIR -- Path to the CUDA SDK. Use this to find files in the +# SDK. This script will not directly support finding +# specific libraries or headers, as that isn't +# supported by NVIDIA. If you want to change +# libraries when the path changes see the +# FindCUDA.cmake script for an example of how to clear +# these variables. There are also examples of how to +# use the CUDA_SDK_ROOT_DIR to locate headers or +# libraries, if you so choose (at your own risk). +# CUDA_INCLUDE_DIRS -- Include directory for cuda headers. Added automatically +# for CUDA_ADD_EXECUTABLE and CUDA_ADD_LIBRARY. +# CUDA_LIBRARIES -- Cuda RT library. +# CUDA_CUFFT_LIBRARIES -- Device or emulation library for the Cuda FFT +# implementation (alternative to: +# CUDA_ADD_CUFFT_TO_TARGET macro) +# CUDA_CUBLAS_LIBRARIES -- Device or emulation library for the Cuda BLAS +# implementation (alterative to: +# CUDA_ADD_CUBLAS_TO_TARGET macro). +# +# +# James Bigler, NVIDIA Corp (nvidia.com - jbigler) +# Abe Stephens, SCI Institute -- http://www.sci.utah.edu/~abe/FindCuda.html +# +# Copyright (c) 2008 - 2009 NVIDIA Corporation. All rights reserved. +# +# Copyright (c) 2007-2009 +# Scientific Computing and Imaging Institute, University of Utah +# +# This code is licensed under the MIT License. See the FindCUDA.cmake script +# for the text of the license. + +# The MIT License +# +# License for the specific language governing rights and limitations under +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the "Software"), +# to deal in the Software without restriction, including without limitation +# the rights to use, copy, modify, merge, publish, distribute, sublicense, +# and/or sell copies of the Software, and to permit persons to whom the +# Software is furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +# DEALINGS IN THE SOFTWARE. +# +############################################################################### + +# FindCUDA.cmake + +# We need to have at least this version to support the VERSION_LESS argument to 'if' (2.6.2) and unset (2.6.3) +cmake_policy(PUSH) +cmake_minimum_required(VERSION 2.6.3) +cmake_policy(POP) + +# This macro helps us find the location of helper files we will need the full path to +macro(CUDA_FIND_HELPER_FILE _name _extension) + set(_full_name "${_name}.${_extension}") + # CMAKE_CURRENT_LIST_FILE contains the full path to the file currently being + # processed. Using this variable, we can pull out the current path, and + # provide a way to get access to the other files we need local to here. + get_filename_component(CMAKE_CURRENT_LIST_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) + find_file(CUDA_${_name} ${_full_name} PATHS ${CMAKE_CURRENT_LIST_DIR}/FindCUDA NO_DEFAULT_PATH) + if(NOT CUDA_${_name}) + set(error_message "${_full_name} not found in CMAKE_MODULE_PATH") + if(CUDA_FIND_REQUIRED) + message(FATAL_ERROR "${error_message}") + else(CUDA_FIND_REQUIRED) + if(NOT CUDA_FIND_QUIETLY) + message(STATUS "${error_message}") + endif(NOT CUDA_FIND_QUIETLY) + endif(CUDA_FIND_REQUIRED) + endif(NOT CUDA_${_name}) + # Set this variable as internal, so the user isn't bugged with it. + set(CUDA_${_name} ${CUDA_${_name}} CACHE INTERNAL "Location of ${_full_name}" FORCE) +endmacro(CUDA_FIND_HELPER_FILE) + +##################################################################### +## CUDA_INCLUDE_NVCC_DEPENDENCIES +## + +# So we want to try and include the dependency file if it exists. If +# it doesn't exist then we need to create an empty one, so we can +# include it. + +# If it does exist, then we need to check to see if all the files it +# depends on exist. If they don't then we should clear the dependency +# file and regenerate it later. This covers the case where a header +# file has disappeared or moved. + +macro(CUDA_INCLUDE_NVCC_DEPENDENCIES dependency_file) + set(CUDA_NVCC_DEPEND) + set(CUDA_NVCC_DEPEND_REGENERATE FALSE) + + + # Include the dependency file. Create it first if it doesn't exist . The + # INCLUDE puts a dependency that will force CMake to rerun and bring in the + # new info when it changes. DO NOT REMOVE THIS (as I did and spent a few + # hours figuring out why it didn't work. + if(NOT EXISTS ${dependency_file}) + file(WRITE ${dependency_file} "#FindCUDA.cmake generated file. Do not edit.\n") + endif() + # Always include this file to force CMake to run again next + # invocation and rebuild the dependencies. + #message("including dependency_file = ${dependency_file}") + include(${dependency_file}) + + # Now we need to verify the existence of all the included files + # here. If they aren't there we need to just blank this variable and + # make the file regenerate again. +# if(DEFINED CUDA_NVCC_DEPEND) +# message("CUDA_NVCC_DEPEND set") +# else() +# message("CUDA_NVCC_DEPEND NOT set") +# endif() + if(CUDA_NVCC_DEPEND) + #message("CUDA_NVCC_DEPEND true") + foreach(f ${CUDA_NVCC_DEPEND}) + #message("searching for ${f}") + if(NOT EXISTS ${f}) + #message("file ${f} not found") + set(CUDA_NVCC_DEPEND_REGENERATE TRUE) + endif() + endforeach(f) + else(CUDA_NVCC_DEPEND) + #message("CUDA_NVCC_DEPEND false") + # No dependencies, so regenerate the file. + set(CUDA_NVCC_DEPEND_REGENERATE TRUE) + endif(CUDA_NVCC_DEPEND) + + #message("CUDA_NVCC_DEPEND_REGENERATE = ${CUDA_NVCC_DEPEND_REGENERATE}") + # No incoming dependencies, so we need to generate them. Make the + # output depend on the dependency file itself, which should cause the + # rule to re-run. + if(CUDA_NVCC_DEPEND_REGENERATE) + file(WRITE ${dependency_file} "#FindCUDA.cmake generated file. Do not edit.\n") + endif(CUDA_NVCC_DEPEND_REGENERATE) + +endmacro(CUDA_INCLUDE_NVCC_DEPENDENCIES) + +############################################################################### +############################################################################### +# Setup variables' defaults +############################################################################### +############################################################################### + +# Allow the user to specify if the device code is supposed to be 32 or 64 bit. +if(CMAKE_SIZEOF_VOID_P EQUAL 8) + set(CUDA_64_BIT_DEVICE_CODE_DEFAULT ON) +else() + set(CUDA_64_BIT_DEVICE_CODE_DEFAULT OFF) +endif() +option(CUDA_64_BIT_DEVICE_CODE "Compile device code in 64 bit mode" ${CUDA_64_BIT_DEVICE_CODE_DEFAULT}) + +# Attach the build rule to the source file in VS. This option +option(CUDA_ATTACH_VS_BUILD_RULE_TO_CUDA_FILE "Attach the build rule to the CUDA source file. Enable only when the CUDA source file is added to at most one target." ON) + +# Prints out extra information about the cuda file during compilation +option(CUDA_BUILD_CUBIN "Generate and parse .cubin files in Device mode." OFF) + +# Set whether we are using emulation or device mode. +option(CUDA_BUILD_EMULATION "Build in Emulation mode" OFF) + +# Where to put the generated output. +set(CUDA_GENERATED_OUTPUT_DIR "" CACHE PATH "Directory to put all the output files. If blank it will default to the CMAKE_CURRENT_BINARY_DIR") + +# Parse HOST_COMPILATION mode. +option(CUDA_HOST_COMPILATION_CPP "Generated file extension" ON) + +# Extra user settable flags +set(CUDA_NVCC_FLAGS "" CACHE STRING "Semi-colon delimit multiple arguments.") + +# Propagate the host flags to the host compiler via -Xcompiler +option(CUDA_PROPAGATE_HOST_FLAGS "Propage C/CXX_FLAGS and friends to the host compiler via -Xcompile" ON) + +# Specifies whether the commands used when compiling the .cu file will be printed out. +option(CUDA_VERBOSE_BUILD "Print out the commands run while compiling the CUDA source file. With the Makefile generator this defaults to VERBOSE variable specified on the command line, but can be forced on with this option." OFF) + +mark_as_advanced( + CUDA_64_BIT_DEVICE_CODE + CUDA_ATTACH_VS_BUILD_RULE_TO_CUDA_FILE + CUDA_GENERATED_OUTPUT_DIR + CUDA_HOST_COMPILATION_CPP + CUDA_NVCC_FLAGS + CUDA_PROPAGATE_HOST_FLAGS + ) + +# Makefile and similar generators don't define CMAKE_CONFIGURATION_TYPES, so we +# need to add another entry for the CMAKE_BUILD_TYPE. We also need to add the +# standerd set of 4 build types (Debug, MinSizeRel, Release, and RelWithDebInfo) +# for completeness. We need run this loop in order to accomodate the addition +# of extra configuration types. Duplicate entries will be removed by +# REMOVE_DUPLICATES. +set(CUDA_configuration_types ${CMAKE_CONFIGURATION_TYPES} ${CMAKE_BUILD_TYPE} Debug MinSizeRel Release RelWithDebInfo) +list(REMOVE_DUPLICATES CUDA_configuration_types) +foreach(config ${CUDA_configuration_types}) + string(TOUPPER ${config} config_upper) + set(CUDA_NVCC_FLAGS_${config_upper} "" CACHE STRING "Semi-colon delimit multiple arguments.") + mark_as_advanced(CUDA_NVCC_FLAGS_${config_upper}) +endforeach() + +############################################################################### +############################################################################### +# Locate CUDA, Set Build Type, etc. +############################################################################### +############################################################################### + +# Check to see if the CUDA_TOOLKIT_ROOT_DIR and CUDA_SDK_ROOT_DIR have changed, +# if they have then clear the cache variables, so that will be detected again. +if(NOT "${CUDA_TOOLKIT_ROOT_DIR}" STREQUAL "${CUDA_TOOLKIT_ROOT_DIR_INTERNAL}") + unset(CUDA_NVCC_EXECUTABLE CACHE) + unset(CUDA_VERSION CACHE) + unset(CUDA_TOOLKIT_INCLUDE CACHE) + unset(CUDA_CUDART_LIBRARY CACHE) + if(CUDA_VERSION VERSION_EQUAL "3.0") + # This only existed in the 3.0 version of the CUDA toolkit + unset(CUDA_CUDARTEMU_LIBRARY CACHE) + endif() + unset(CUDA_CUDA_LIBRARY CACHE) + unset(CUDA_cublas_LIBRARY CACHE) + unset(CUDA_cublasemu_LIBRARY CACHE) + unset(CUDA_cufft_LIBRARY CACHE) + unset(CUDA_cufftemu_LIBRARY CACHE) +endif() + +if(NOT "${CUDA_SDK_ROOT_DIR}" STREQUAL "${CUDA_SDK_ROOT_DIR_INTERNAL}") + # No specific variables to catch. Use this kind of code before calling + # find_package(CUDA) to clean up any variables that may depend on this path. + + # unset(MY_SPECIAL_CUDA_SDK_INCLUDE_DIR CACHE) + # unset(MY_SPECIAL_CUDA_SDK_LIBRARY CACHE) +endif() + +# Search for the cuda distribution. +if(NOT CUDA_TOOLKIT_ROOT_DIR) + + # Search in the CUDA_BIN_PATH first. + find_path(CUDA_TOOLKIT_ROOT_DIR + NAMES nvcc nvcc.exe + PATHS ENV CUDA_BIN_PATH + DOC "Toolkit location." + NO_DEFAULT_PATH + ) + # Now search default paths + find_path(CUDA_TOOLKIT_ROOT_DIR + NAMES nvcc nvcc.exe + PATHS /usr/local/bin + /usr/local/cuda/bin + DOC "Toolkit location." + ) + + if (CUDA_TOOLKIT_ROOT_DIR) + string(REGEX REPLACE "[/\\\\]?bin[64]*[/\\\\]?$" "" CUDA_TOOLKIT_ROOT_DIR ${CUDA_TOOLKIT_ROOT_DIR}) + # We need to force this back into the cache. + set(CUDA_TOOLKIT_ROOT_DIR ${CUDA_TOOLKIT_ROOT_DIR} CACHE PATH "Toolkit location." FORCE) + endif(CUDA_TOOLKIT_ROOT_DIR) + if (NOT EXISTS ${CUDA_TOOLKIT_ROOT_DIR}) + if(CUDA_FIND_REQUIRED) + message(FATAL_ERROR "Specify CUDA_TOOLKIT_ROOT_DIR") + elseif(NOT CUDA_FIND_QUIETLY) + message("CUDA_TOOLKIT_ROOT_DIR not found or specified") + endif() + endif (NOT EXISTS ${CUDA_TOOLKIT_ROOT_DIR}) +endif (NOT CUDA_TOOLKIT_ROOT_DIR) + +# CUDA_NVCC_EXECUTABLE +find_program(CUDA_NVCC_EXECUTABLE + NAMES nvcc + PATHS "${CUDA_TOOLKIT_ROOT_DIR}/bin" + "${CUDA_TOOLKIT_ROOT_DIR}/bin64" + ENV CUDA_BIN_PATH + NO_DEFAULT_PATH + ) +# Search default search paths, after we search our own set of paths. +find_program(CUDA_NVCC_EXECUTABLE nvcc) +mark_as_advanced(CUDA_NVCC_EXECUTABLE) + +if(CUDA_NVCC_EXECUTABLE AND NOT CUDA_VERSION) + # Compute the version. + execute_process (COMMAND ${CUDA_NVCC_EXECUTABLE} "--version" OUTPUT_VARIABLE NVCC_OUT) + string(REGEX REPLACE ".*release ([0-9]+)\\.([0-9]+).*" "\\1" CUDA_VERSION_MAJOR ${NVCC_OUT}) + string(REGEX REPLACE ".*release ([0-9]+)\\.([0-9]+).*" "\\2" CUDA_VERSION_MINOR ${NVCC_OUT}) + set(CUDA_VERSION "${CUDA_VERSION_MAJOR}.${CUDA_VERSION_MINOR}" CACHE STRING "Version of CUDA as computed from nvcc.") + mark_as_advanced(CUDA_VERSION) +else() + # Need to set these based off of the cached value + string(REGEX REPLACE "([0-9]+)\\.([0-9]+).*" "\\1" CUDA_VERSION_MAJOR "${CUDA_VERSION}") + string(REGEX REPLACE "([0-9]+)\\.([0-9]+).*" "\\2" CUDA_VERSION_MINOR "${CUDA_VERSION}") +endif() + +# Always set this convenience variable +set(CUDA_VERSION_STRING "${CUDA_VERSION}") + +# Here we need to determine if the version we found is acceptable. We will +# assume that is unless CUDA_FIND_VERSION_EXACT or CUDA_FIND_VERSION is +# specified. The presence of either of these options checks the version +# string and signals if the version is acceptable or not. +set(_cuda_version_acceptable TRUE) +# +if(CUDA_FIND_VERSION_EXACT AND NOT CUDA_VERSION VERSION_EQUAL CUDA_FIND_VERSION) + set(_cuda_version_acceptable FALSE) +endif() +# +if(CUDA_FIND_VERSION AND CUDA_VERSION VERSION_LESS CUDA_FIND_VERSION) + set(_cuda_version_acceptable FALSE) +endif() +# +if(NOT _cuda_version_acceptable) + set(_cuda_error_message "Requested CUDA version ${CUDA_FIND_VERSION}, but found unacceptable version ${CUDA_VERSION}") + if(CUDA_FIND_REQUIRED) + message("${_cuda_error_message}") + elseif(NOT CUDA_FIND_QUIETLY) + message("${_cuda_error_message}") + endif() +endif() + +# CUDA_TOOLKIT_INCLUDE +find_path(CUDA_TOOLKIT_INCLUDE + device_functions.h # Header included in toolkit + PATHS "${CUDA_TOOLKIT_ROOT_DIR}/include" + ENV CUDA_INC_PATH + NO_DEFAULT_PATH + ) +# Search default search paths, after we search our own set of paths. +find_path(CUDA_TOOLKIT_INCLUDE device_functions.h) +mark_as_advanced(CUDA_TOOLKIT_INCLUDE) + +# Set the user list of include dir to nothing to initialize it. +set (CUDA_NVCC_INCLUDE_ARGS_USER "") +set (CUDA_INCLUDE_DIRS ${CUDA_TOOLKIT_INCLUDE}) + +macro(FIND_LIBRARY_LOCAL_FIRST _var _names _doc) + if(CMAKE_SIZEOF_VOID_P EQUAL 8) + # CUDA 3.2+ on Windows moved the library directoryies, so we need the new + # and old paths. + set(_cuda_64bit_lib_dir + "${CUDA_TOOLKIT_ROOT_DIR}/lib/x64" + "${CUDA_TOOLKIT_ROOT_DIR}/lib64" + ) + endif() + # CUDA 3.2+ on Windows moved the library directories, so we need to new + # (lib/Win32) and the old path (lib). + find_library(${_var} + NAMES ${_names} + PATHS ${_cuda_64bit_lib_dir} + "${CUDA_TOOLKIT_ROOT_DIR}/lib/Win32" + "${CUDA_TOOLKIT_ROOT_DIR}/lib" + ENV CUDA_LIB_PATH + DOC ${_doc} + NO_DEFAULT_PATH + ) + # Search default search paths, after we search our own set of paths. + find_library(${_var} NAMES ${_names} DOC ${_doc}) +endmacro() + +# CUDA_LIBRARIES +find_library_local_first(CUDA_CUDART_LIBRARY cudart "\"cudart\" library") +if(CUDA_VERSION VERSION_EQUAL "3.0") + # The cudartemu library only existed for the 3.0 version of CUDA. + find_library_local_first(CUDA_CUDARTEMU_LIBRARY cudartemu "\"cudartemu\" library") + mark_as_advanced( + CUDA_CUDARTEMU_LIBRARY + ) +endif() +# If we are using emulation mode and we found the cudartemu library then use +# that one instead of cudart. +if(CUDA_BUILD_EMULATION AND CUDA_CUDARTEMU_LIBRARY) + set(CUDA_LIBRARIES ${CUDA_CUDARTEMU_LIBRARY}) +else() +set(CUDA_LIBRARIES ${CUDA_CUDART_LIBRARY}) +endif() +if(APPLE) + # We need to add the path to cudart to the linker using rpath, since the + # library name for the cuda libraries is prepended with @rpath. + if(CUDA_BUILD_EMULATION AND CUDA_CUDARTEMU_LIBRARY) + get_filename_component(_cuda_path_to_cudart "${CUDA_CUDARTEMU_LIBRARY}" PATH) + else() + get_filename_component(_cuda_path_to_cudart "${CUDA_CUDART_LIBRARY}" PATH) + endif() + if(_cuda_path_to_cudart) + list(APPEND CUDA_LIBRARIES -Wl,-rpath "-Wl,${_cuda_path_to_cudart}") + endif() +endif() + +# 1.1 toolkit on linux doesn't appear to have a separate library on +# some platforms. +find_library_local_first(CUDA_CUDA_LIBRARY cuda "\"cuda\" library (older versions only).") + +# Add cuda library to the link line only if it is found. +if (CUDA_CUDA_LIBRARY) + set(CUDA_LIBRARIES ${CUDA_LIBRARIES} ${CUDA_CUDA_LIBRARY}) +endif(CUDA_CUDA_LIBRARY) + +mark_as_advanced( + CUDA_CUDA_LIBRARY + CUDA_CUDART_LIBRARY + ) + +####################### +# Look for some of the toolkit helper libraries +macro(FIND_CUDA_HELPER_LIBS _name) + find_library_local_first(CUDA_${_name}_LIBRARY ${_name} "\"${_name}\" library") + mark_as_advanced(CUDA_${_name}_LIBRARY) +endmacro(FIND_CUDA_HELPER_LIBS) + +####################### +# Disable emulation for v3.1 onward +if(CUDA_VERSION VERSION_GREATER "3.0") + if(CUDA_BUILD_EMULATION) + message(FATAL_ERROR "CUDA_BUILD_EMULATION is not supported in version 3.1 and onwards. You must disable it to proceed. You have version ${CUDA_VERSION}.") + endif() +endif() + +# Search for cufft and cublas libraries. +if(CUDA_VERSION VERSION_LESS "3.1") + # Emulation libraries aren't available in version 3.1 onward. +find_cuda_helper_libs(cufftemu) +find_cuda_helper_libs(cublasemu) +endif() +find_cuda_helper_libs(cufft) +find_cuda_helper_libs(cublas) + +if (CUDA_BUILD_EMULATION) + set(CUDA_CUFFT_LIBRARIES ${CUDA_cufftemu_LIBRARY}) + set(CUDA_CUBLAS_LIBRARIES ${CUDA_cublasemu_LIBRARY}) +else() + set(CUDA_CUFFT_LIBRARIES ${CUDA_cufft_LIBRARY}) + set(CUDA_CUBLAS_LIBRARIES ${CUDA_cublas_LIBRARY}) +endif() + +######################## +# Look for the SDK stuff. As of CUDA 3.0 NVSDKCUDA_ROOT has been replaced with +# NVSDKCOMPUTE_ROOT with the old CUDA C contents moved into the C subdirectory +find_path(CUDA_SDK_ROOT_DIR common/inc/cutil.h + "$ENV{HOME}/NVIDIA_GPU_Computing_SDK/C" + "$ENV{NVSDKCOMPUTE_ROOT}/C" + "$ENV{NVSDKCUDA_ROOT}" + "[HKEY_LOCAL_MACHINE\\SOFTWARE\\NVIDIA Corporation\\Installed Products\\NVIDIA SDK 10\\Compute;InstallDir]" + "/Developer/GPU\ Computing/C" + ) + +# Keep the CUDA_SDK_ROOT_DIR first in order to be able to override the +# environment variables. +set(CUDA_SDK_SEARCH_PATH + "${CUDA_SDK_ROOT_DIR}" + "${CUDA_TOOLKIT_ROOT_DIR}/local/NVSDK0.2" + "${CUDA_TOOLKIT_ROOT_DIR}/NVSDK0.2" + "${CUDA_TOOLKIT_ROOT_DIR}/NV_CUDA_SDK" + "$ENV{HOME}/NVIDIA_CUDA_SDK" + "$ENV{HOME}/NVIDIA_CUDA_SDK_MACOSX" + "$ENV{HOME}/NVIDIA_GPU_Computing_SDK/C" + "/Developer/CUDA" + ) + +# Example of how to find an include file from the CUDA_SDK_ROOT_DIR + +# find_path(CUDA_CUT_INCLUDE_DIR +# cutil.h +# PATHS ${CUDA_SDK_SEARCH_PATH} +# PATH_SUFFIXES "common/inc" +# DOC "Location of cutil.h" +# NO_DEFAULT_PATH +# ) +# # Now search system paths +# find_path(CUDA_CUT_INCLUDE_DIR cutil.h DOC "Location of cutil.h") + +# mark_as_advanced(CUDA_CUT_INCLUDE_DIR) + + +# Example of how to find a library in the CUDA_SDK_ROOT_DIR + +# # cutil library is called cutil64 for 64 bit builds on windows. We don't want +# # to get these confused, so we are setting the name based on the word size of +# # the build. + +# if(CMAKE_SIZEOF_VOID_P EQUAL 8) +# set(cuda_cutil_name cutil64) +# else(CMAKE_SIZEOF_VOID_P EQUAL 8) +# set(cuda_cutil_name cutil32) +# endif(CMAKE_SIZEOF_VOID_P EQUAL 8) + +# find_library(CUDA_CUT_LIBRARY +# NAMES cutil ${cuda_cutil_name} +# PATHS ${CUDA_SDK_SEARCH_PATH} +# # The new version of the sdk shows up in common/lib, but the old one is in lib +# PATH_SUFFIXES "common/lib" "lib" +# DOC "Location of cutil library" +# NO_DEFAULT_PATH +# ) +# # Now search system paths +# find_library(CUDA_CUT_LIBRARY NAMES cutil ${cuda_cutil_name} DOC "Location of cutil library") +# mark_as_advanced(CUDA_CUT_LIBRARY) +# set(CUDA_CUT_LIBRARIES ${CUDA_CUT_LIBRARY}) + + + +############################# +# Check for required components +set(CUDA_FOUND TRUE) + +set(CUDA_TOOLKIT_ROOT_DIR_INTERNAL "${CUDA_TOOLKIT_ROOT_DIR}" CACHE INTERNAL + "This is the value of the last time CUDA_TOOLKIT_ROOT_DIR was set successfully." FORCE) +set(CUDA_SDK_ROOT_DIR_INTERNAL "${CUDA_SDK_ROOT_DIR}" CACHE INTERNAL + "This is the value of the last time CUDA_SDK_ROOT_DIR was set successfully." FORCE) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(CUDA DEFAULT_MSG + CUDA_TOOLKIT_ROOT_DIR + CUDA_NVCC_EXECUTABLE + CUDA_INCLUDE_DIRS + CUDA_CUDART_LIBRARY + _cuda_version_acceptable + ) + + + +############################################################################### +############################################################################### +# Macros +############################################################################### +############################################################################### + +############################################################################### +# Add include directories to pass to the nvcc command. +macro(CUDA_INCLUDE_DIRECTORIES) + foreach(dir ${ARGN}) + list(APPEND CUDA_NVCC_INCLUDE_ARGS_USER "-I${dir}") + endforeach(dir ${ARGN}) +endmacro(CUDA_INCLUDE_DIRECTORIES) + + +############################################################################## +cuda_find_helper_file(parse_cubin cmake) +cuda_find_helper_file(make2cmake cmake) +cuda_find_helper_file(run_nvcc cmake) + +############################################################################## +# Separate the OPTIONS out from the sources +# +macro(CUDA_GET_SOURCES_AND_OPTIONS _sources _cmake_options _options) + set( ${_sources} ) + set( ${_cmake_options} ) + set( ${_options} ) + set( _found_options FALSE ) + foreach(arg ${ARGN}) + if(arg STREQUAL "OPTIONS") + set( _found_options TRUE ) + elseif( + arg STREQUAL "WIN32" OR + arg STREQUAL "MACOSX_BUNDLE" OR + arg STREQUAL "EXCLUDE_FROM_ALL" OR + arg STREQUAL "STATIC" OR + arg STREQUAL "SHARED" OR + arg STREQUAL "MODULE" + ) + list(APPEND ${_cmake_options} "${arg}") + else() + if ( _found_options ) + list(APPEND ${_options} "${arg}") + else() + # Assume this is a file + list(APPEND ${_sources} "${arg}") + endif() + endif() + endforeach() +endmacro() + +############################################################################## +# Parse the OPTIONS from ARGN and set the variables prefixed by _option_prefix +# +macro(CUDA_PARSE_NVCC_OPTIONS _option_prefix) + set( _found_config ) + foreach(arg ${ARGN}) + # Determine if we are dealing with a perconfiguration flag + foreach(config ${CUDA_configuration_types}) + string(TOUPPER ${config} config_upper) + if (arg STREQUAL "${config_upper}") + set( _found_config _${arg}) + # Set arg to nothing to keep it from being processed further + set( arg ) + endif() + endforeach() + + if ( arg ) + list(APPEND ${_option_prefix}${_found_config} "${arg}") + endif() + endforeach() +endmacro() + +############################################################################## +# Helper to add the include directory for CUDA only once +function(CUDA_ADD_CUDA_INCLUDE_ONCE) + get_directory_property(_include_directories INCLUDE_DIRECTORIES) + set(_add TRUE) + if(_include_directories) + foreach(dir ${_include_directories}) + if("${dir}" STREQUAL "${CUDA_INCLUDE_DIRS}") + set(_add FALSE) + endif() + endforeach() + endif() + if(_add) + include_directories(${CUDA_INCLUDE_DIRS}) + endif() +endfunction() + +function(CUDA_BUILD_SHARED_LIBRARY shared_flag) + set(cmake_args ${ARGN}) + # If SHARED, MODULE, or STATIC aren't already in the list of arguments, then + # add SHARED or STATIC based on the value of BUILD_SHARED_LIBS. + list(FIND cmake_args SHARED _cuda_found_SHARED) + list(FIND cmake_args MODULE _cuda_found_MODULE) + list(FIND cmake_args STATIC _cuda_found_STATIC) + if( _cuda_found_SHARED GREATER -1 OR + _cuda_found_MODULE GREATER -1 OR + _cuda_found_STATIC GREATER -1) + set(_cuda_build_shared_libs) + else() + if (BUILD_SHARED_LIBS) + set(_cuda_build_shared_libs SHARED) + else() + set(_cuda_build_shared_libs STATIC) + endif() + endif() + set(${shared_flag} ${_cuda_build_shared_libs} PARENT_SCOPE) +endfunction() + +############################################################################## +# This helper macro populates the following variables and setups up custom +# commands and targets to invoke the nvcc compiler to generate C or PTX source +# dependent upon the format parameter. The compiler is invoked once with -M +# to generate a dependency file and a second time with -cuda or -ptx to generate +# a .cpp or .ptx file. +# INPUT: +# cuda_target - Target name +# format - PTX or OBJ +# FILE1 .. FILEN - The remaining arguments are the sources to be wrapped. +# OPTIONS - Extra options to NVCC +# OUTPUT: +# generated_files - List of generated files +############################################################################## +############################################################################## + +macro(CUDA_WRAP_SRCS cuda_target format generated_files) + + if( ${format} MATCHES "PTX" ) + set( compile_to_ptx ON ) + elseif( ${format} MATCHES "OBJ") + set( compile_to_ptx OFF ) + else() + message( FATAL_ERROR "Invalid format flag passed to CUDA_WRAP_SRCS: '${format}'. Use OBJ or PTX.") + endif() + + # Set up all the command line flags here, so that they can be overriden on a per target basis. + + set(nvcc_flags "") + + # Emulation if the card isn't present. + if (CUDA_BUILD_EMULATION) + # Emulation. + set(nvcc_flags ${nvcc_flags} --device-emulation -D_DEVICEEMU -g) + else(CUDA_BUILD_EMULATION) + # Device mode. No flags necessary. + endif(CUDA_BUILD_EMULATION) + + if(CUDA_HOST_COMPILATION_CPP) + set(CUDA_C_OR_CXX CXX) + else(CUDA_HOST_COMPILATION_CPP) + if(CUDA_VERSION VERSION_LESS "3.0") + set(nvcc_flags ${nvcc_flags} --host-compilation C) + else() + message(WARNING "--host-compilation flag is deprecated in CUDA version >= 3.0. Removing --host-compilation C flag" ) + endif() + set(CUDA_C_OR_CXX C) + endif(CUDA_HOST_COMPILATION_CPP) + + set(generated_extension ${CMAKE_${CUDA_C_OR_CXX}_OUTPUT_EXTENSION}) + + if(CUDA_64_BIT_DEVICE_CODE) + set(nvcc_flags ${nvcc_flags} -m64) + else() + set(nvcc_flags ${nvcc_flags} -m32) + endif() + + # This needs to be passed in at this stage, because VS needs to fill out the + # value of VCInstallDir from within VS. + if(CMAKE_GENERATOR MATCHES "Visual Studio") + if( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + # Add nvcc flag for 64b Windows + set(ccbin_flags -D "\"CCBIN:PATH=$(VCInstallDir)bin\"" ) + endif() + endif() + + # Figure out which configure we will use and pass that in as an argument to + # the script. We need to defer the decision until compilation time, because + # for VS projects we won't know if we are making a debug or release build + # until build time. + if(CMAKE_GENERATOR MATCHES "Visual Studio") + set( CUDA_build_configuration "$(ConfigurationName)" ) + else() + set( CUDA_build_configuration "${CMAKE_BUILD_TYPE}") + endif() + + # Initialize our list of includes with the user ones followed by the CUDA system ones. + set(CUDA_NVCC_INCLUDE_ARGS ${CUDA_NVCC_INCLUDE_ARGS_USER} "-I${CUDA_INCLUDE_DIRS}") + # Get the include directories for this directory and use them for our nvcc command. + get_directory_property(CUDA_NVCC_INCLUDE_DIRECTORIES INCLUDE_DIRECTORIES) + if(CUDA_NVCC_INCLUDE_DIRECTORIES) + foreach(dir ${CUDA_NVCC_INCLUDE_DIRECTORIES}) + list(APPEND CUDA_NVCC_INCLUDE_ARGS "-I${dir}") + endforeach() + endif() + + # Reset these variables + set(CUDA_WRAP_OPTION_NVCC_FLAGS) + foreach(config ${CUDA_configuration_types}) + string(TOUPPER ${config} config_upper) + set(CUDA_WRAP_OPTION_NVCC_FLAGS_${config_upper}) + endforeach() + + CUDA_GET_SOURCES_AND_OPTIONS(_cuda_wrap_sources _cuda_wrap_cmake_options _cuda_wrap_options ${ARGN}) + CUDA_PARSE_NVCC_OPTIONS(CUDA_WRAP_OPTION_NVCC_FLAGS ${_cuda_wrap_options}) + + # Figure out if we are building a shared library. BUILD_SHARED_LIBS is + # respected in CUDA_ADD_LIBRARY. + set(_cuda_build_shared_libs FALSE) + # SHARED, MODULE + list(FIND _cuda_wrap_cmake_options SHARED _cuda_found_SHARED) + list(FIND _cuda_wrap_cmake_options MODULE _cuda_found_MODULE) + if(_cuda_found_SHARED GREATER -1 OR _cuda_found_MODULE GREATER -1) + set(_cuda_build_shared_libs TRUE) + endif() + # STATIC + list(FIND _cuda_wrap_cmake_options STATIC _cuda_found_STATIC) + if(_cuda_found_STATIC GREATER -1) + set(_cuda_build_shared_libs FALSE) + endif() + + # CUDA_HOST_FLAGS + if(_cuda_build_shared_libs) + # If we are setting up code for a shared library, then we need to add extra flags for + # compiling objects for shared libraries. + set(CUDA_HOST_SHARED_FLAGS ${CMAKE_SHARED_LIBRARY_${CUDA_C_OR_CXX}_FLAGS}) + else() + set(CUDA_HOST_SHARED_FLAGS) + endif() + # Only add the CMAKE_{C,CXX}_FLAGS if we are propagating host flags. We + # always need to set the SHARED_FLAGS, though. + if(CUDA_PROPAGATE_HOST_FLAGS) + set(CUDA_HOST_FLAGS "set(CMAKE_HOST_FLAGS ${CMAKE_${CUDA_C_OR_CXX}_FLAGS} ${CUDA_HOST_SHARED_FLAGS})") + else() + set(CUDA_HOST_FLAGS "set(CMAKE_HOST_FLAGS ${CUDA_HOST_SHARED_FLAGS})") + endif() + + set(CUDA_NVCC_FLAGS_CONFIG "# Build specific configuration flags") + # Loop over all the configuration types to generate appropriate flags for run_nvcc.cmake + foreach(config ${CUDA_configuration_types}) + string(TOUPPER ${config} config_upper) + # CMAKE_FLAGS are strings and not lists. By not putting quotes around CMAKE_FLAGS + # we convert the strings to lists (like we want). + + if(CUDA_PROPAGATE_HOST_FLAGS) + # nvcc chokes on -g3 in versions previous to 3.0, so replace it with -g + if(CMAKE_COMPILER_IS_GNUCC AND CUDA_VERSION VERSION_LESS "3.0") + string(REPLACE "-g3" "-g" _cuda_C_FLAGS "${CMAKE_${CUDA_C_OR_CXX}_FLAGS_${config_upper}}") + else() + set(_cuda_C_FLAGS "${CMAKE_${CUDA_C_OR_CXX}_FLAGS_${config_upper}}") + endif() + + set(CUDA_HOST_FLAGS "${CUDA_HOST_FLAGS}\nset(CMAKE_HOST_FLAGS_${config_upper} ${_cuda_C_FLAGS})") + endif() + + # Note that if we ever want CUDA_NVCC_FLAGS_ to be string (instead of a list + # like it is currently), we can remove the quotes around the + # ${CUDA_NVCC_FLAGS_${config_upper}} variable like the CMAKE_HOST_FLAGS_ variable. + set(CUDA_NVCC_FLAGS_CONFIG "${CUDA_NVCC_FLAGS_CONFIG}\nset(CUDA_NVCC_FLAGS_${config_upper} \"${CUDA_NVCC_FLAGS_${config_upper}};;${CUDA_WRAP_OPTION_NVCC_FLAGS_${config_upper}}\")") + endforeach() + + if(compile_to_ptx) + # Don't use any of the host compilation flags for PTX targets. + set(CUDA_HOST_FLAGS) + set(CUDA_NVCC_FLAGS_CONFIG) + endif() + + # Get the list of definitions from the directory property + get_directory_property(CUDA_NVCC_DEFINITIONS COMPILE_DEFINITIONS) + if(CUDA_NVCC_DEFINITIONS) + foreach(_definition ${CUDA_NVCC_DEFINITIONS}) + list(APPEND nvcc_flags "-D${_definition}") + endforeach() + endif() + + if(_cuda_build_shared_libs) + list(APPEND nvcc_flags "-D${cuda_target}_EXPORTS") + endif() + + # Determine output directory + if(CUDA_GENERATED_OUTPUT_DIR) + set(cuda_compile_output_dir "${CUDA_GENERATED_OUTPUT_DIR}") + else() + set(cuda_compile_output_dir "${CMAKE_CURRENT_BINARY_DIR}") + endif() + + # Reset the output variable + set(_cuda_wrap_generated_files "") + + # Iterate over the macro arguments and create custom + # commands for all the .cu files. + foreach(file ${ARGN}) + # Ignore any file marked as a HEADER_FILE_ONLY + get_source_file_property(_is_header ${file} HEADER_FILE_ONLY) + if(${file} MATCHES ".*\\.cu$" AND NOT _is_header) + + # Add a custom target to generate a c or ptx file. ###################### + + get_filename_component( basename ${file} NAME ) + if( compile_to_ptx ) + set(generated_file_path "${cuda_compile_output_dir}") + set(generated_file_basename "${cuda_target}_generated_${basename}.ptx") + set(format_flag "-ptx") + file(MAKE_DIRECTORY "${cuda_compile_output_dir}") + else( compile_to_ptx ) + set(generated_file_path "${cuda_compile_output_dir}/${CMAKE_CFG_INTDIR}") + set(generated_file_basename "${cuda_target}_generated_${basename}${generated_extension}") + set(format_flag "-c") + endif( compile_to_ptx ) + + # Set all of our file names. Make sure that whatever filenames that have + # generated_file_path in them get passed in through as a command line + # argument, so that the ${CMAKE_CFG_INTDIR} gets expanded at run time + # instead of configure time. + set(generated_file "${generated_file_path}/${generated_file_basename}") + set(cmake_dependency_file "${CMAKE_CURRENT_BINARY_DIR}/CMakeFiles/${generated_file_basename}.depend") + set(NVCC_generated_dependency_file "${CMAKE_CURRENT_BINARY_DIR}/CMakeFiles/${generated_file_basename}.NVCC-depend") + set(generated_cubin_file "${generated_file_path}/${generated_file_basename}.cubin.txt") + set(custom_target_script "${CMAKE_CURRENT_BINARY_DIR}/CMakeFiles/${generated_file_basename}.cmake") + + # Setup properties for obj files: + if( NOT compile_to_ptx ) + set_source_files_properties("${generated_file}" + PROPERTIES + EXTERNAL_OBJECT true # This is an object file not to be compiled, but only be linked. + ) + endif() + + # Don't add CMAKE_CURRENT_SOURCE_DIR if the path is already an absolute path. + get_filename_component(file_path "${file}" PATH) + if(IS_ABSOLUTE "${file_path}") + set(source_file "${file}") + else() + set(source_file "${CMAKE_CURRENT_SOURCE_DIR}/${file}") + endif() + + # Bring in the dependencies. Creates a variable CUDA_NVCC_DEPEND ####### + cuda_include_nvcc_dependencies(${cmake_dependency_file}) + + # Convience string for output ########################################### + if(CUDA_BUILD_EMULATION) + set(cuda_build_type "Emulation") + else(CUDA_BUILD_EMULATION) + set(cuda_build_type "Device") + endif(CUDA_BUILD_EMULATION) + + # Build the NVCC made dependency file ################################### + set(build_cubin OFF) + if ( NOT CUDA_BUILD_EMULATION AND CUDA_BUILD_CUBIN ) + if ( NOT compile_to_ptx ) + set ( build_cubin ON ) + endif( NOT compile_to_ptx ) + endif( NOT CUDA_BUILD_EMULATION AND CUDA_BUILD_CUBIN ) + + # Configure the build script + configure_file("${CUDA_run_nvcc}" "${custom_target_script}" @ONLY) + + # So if a user specifies the same cuda file as input more than once, you + # can have bad things happen with dependencies. Here we check an option + # to see if this is the behavior they want. + if(CUDA_ATTACH_VS_BUILD_RULE_TO_CUDA_FILE) + set(main_dep MAIN_DEPENDENCY ${source_file}) + else() + set(main_dep DEPENDS ${source_file}) + endif() + + if(CUDA_VERBOSE_BUILD) + set(verbose_output ON) + elseif(CMAKE_GENERATOR MATCHES "Makefiles") + set(verbose_output "$(VERBOSE)") + else() + set(verbose_output OFF) + endif() + + # Create up the comment string + file(RELATIVE_PATH generated_file_relative_path "${CMAKE_BINARY_DIR}" "${generated_file}") + if(compile_to_ptx) + set(cuda_build_comment_string "Building NVCC ptx file ${generated_file_relative_path}") + else() + set(cuda_build_comment_string "Building NVCC (${cuda_build_type}) object ${generated_file_relative_path}") + endif() + + # Build the generated file and dependency file ########################## + add_custom_command( + OUTPUT ${generated_file} + # These output files depend on the source_file and the contents of cmake_dependency_file + ${main_dep} + DEPENDS ${CUDA_NVCC_DEPEND} + DEPENDS ${custom_target_script} + # Make sure the output directory exists before trying to write to it. + COMMAND ${CMAKE_COMMAND} -E make_directory "${generated_file_path}" + COMMAND ${CMAKE_COMMAND} ARGS + -D verbose:BOOL=${verbose_output} + ${ccbin_flags} + -D build_configuration:STRING=${CUDA_build_configuration} + -D "generated_file:STRING=${generated_file}" + -D "generated_cubin_file:STRING=${generated_cubin_file}" + -P "${custom_target_script}" + COMMENT "${cuda_build_comment_string}" + ) + + # Make sure the build system knows the file is generated. + set_source_files_properties(${generated_file} PROPERTIES GENERATED TRUE) + + # Don't add the object file to the list of generated files if we are using + # visual studio and we are attaching the build rule to the cuda file. VS + # will add our object file to the linker automatically for us. + set(cuda_add_generated_file TRUE) + + if(NOT compile_to_ptx AND CMAKE_GENERATOR MATCHES "Visual Studio" AND CUDA_ATTACH_VS_BUILD_RULE_TO_CUDA_FILE) + # Visual Studio 8 crashes when you close the solution when you don't add the object file. + if(NOT CMAKE_GENERATOR MATCHES "Visual Studio 8") + #message("Not adding ${generated_file}") + set(cuda_add_generated_file FALSE) + endif() + endif() + + if(cuda_add_generated_file) + list(APPEND _cuda_wrap_generated_files ${generated_file}) + endif() + + # Add the other files that we want cmake to clean on a cleanup ########## + list(APPEND CUDA_ADDITIONAL_CLEAN_FILES "${cmake_dependency_file}") + list(REMOVE_DUPLICATES CUDA_ADDITIONAL_CLEAN_FILES) + set(CUDA_ADDITIONAL_CLEAN_FILES ${CUDA_ADDITIONAL_CLEAN_FILES} CACHE INTERNAL "List of intermediate files that are part of the cuda dependency scanning.") + + endif(${file} MATCHES ".*\\.cu$" AND NOT _is_header) + endforeach(file) + + # Set the return parameter + set(${generated_files} ${_cuda_wrap_generated_files}) +endmacro(CUDA_WRAP_SRCS) + + +############################################################################### +############################################################################### +# ADD LIBRARY +############################################################################### +############################################################################### +macro(CUDA_ADD_LIBRARY cuda_target) + + CUDA_ADD_CUDA_INCLUDE_ONCE() + + # Separate the sources from the options + CUDA_GET_SOURCES_AND_OPTIONS(_sources _cmake_options _options ${ARGN}) + CUDA_BUILD_SHARED_LIBRARY(_cuda_shared_flag ${ARGN}) + # Create custom commands and targets for each file. + CUDA_WRAP_SRCS( ${cuda_target} OBJ _generated_files ${_sources} + ${_cmake_options} ${_cuda_shared_flag} + OPTIONS ${_options} ) + + # Add the library. + add_library(${cuda_target} ${_cmake_options} + ${_generated_files} + ${_sources} + ) + + target_link_libraries(${cuda_target} + ${CUDA_LIBRARIES} + ) + + # We need to set the linker language based on what the expected generated file + # would be. CUDA_C_OR_CXX is computed based on CUDA_HOST_COMPILATION_CPP. + set_target_properties(${cuda_target} + PROPERTIES + LINKER_LANGUAGE ${CUDA_C_OR_CXX} + ) + +endmacro(CUDA_ADD_LIBRARY cuda_target) + + +############################################################################### +############################################################################### +# ADD EXECUTABLE +############################################################################### +############################################################################### +macro(CUDA_ADD_EXECUTABLE cuda_target) + + CUDA_ADD_CUDA_INCLUDE_ONCE() + + # Separate the sources from the options + CUDA_GET_SOURCES_AND_OPTIONS(_sources _cmake_options _options ${ARGN}) + # Create custom commands and targets for each file. + CUDA_WRAP_SRCS( ${cuda_target} OBJ _generated_files ${_sources} OPTIONS ${_options} ) + + # Add the library. + add_executable(${cuda_target} ${_cmake_options} + ${_generated_files} + ${_sources} + ) + + target_link_libraries(${cuda_target} + ${CUDA_LIBRARIES} + ) + + # We need to set the linker language based on what the expected generated file + # would be. CUDA_C_OR_CXX is computed based on CUDA_HOST_COMPILATION_CPP. + set_target_properties(${cuda_target} + PROPERTIES + LINKER_LANGUAGE ${CUDA_C_OR_CXX} + ) + +endmacro(CUDA_ADD_EXECUTABLE cuda_target) + + +############################################################################### +############################################################################### +# CUDA COMPILE +############################################################################### +############################################################################### +macro(CUDA_COMPILE generated_files) + + # Separate the sources from the options + CUDA_GET_SOURCES_AND_OPTIONS(_sources _cmake_options _options ${ARGN}) + # Create custom commands and targets for each file. + CUDA_WRAP_SRCS( cuda_compile OBJ _generated_files ${_sources} ${_cmake_options} + OPTIONS ${_options} ) + + set( ${generated_files} ${_generated_files}) + +endmacro(CUDA_COMPILE) + + +############################################################################### +############################################################################### +# CUDA COMPILE PTX +############################################################################### +############################################################################### +macro(CUDA_COMPILE_PTX generated_files) + + # Separate the sources from the options + CUDA_GET_SOURCES_AND_OPTIONS(_sources _cmake_options _options ${ARGN}) + # Create custom commands and targets for each file. + CUDA_WRAP_SRCS( cuda_compile_ptx PTX _generated_files ${_sources} ${_cmake_options} + OPTIONS ${_options} ) + + set( ${generated_files} ${_generated_files}) + +endmacro(CUDA_COMPILE_PTX) + +############################################################################### +############################################################################### +# CUDA ADD CUFFT TO TARGET +############################################################################### +############################################################################### +macro(CUDA_ADD_CUFFT_TO_TARGET target) + if (CUDA_BUILD_EMULATION) + target_link_libraries(${target} ${CUDA_cufftemu_LIBRARY}) + else() + target_link_libraries(${target} ${CUDA_cufft_LIBRARY}) + endif() +endmacro() + +############################################################################### +############################################################################### +# CUDA ADD CUBLAS TO TARGET +############################################################################### +############################################################################### +macro(CUDA_ADD_CUBLAS_TO_TARGET target) + if (CUDA_BUILD_EMULATION) + target_link_libraries(${target} ${CUDA_cublasemu_LIBRARY}) + else() + target_link_libraries(${target} ${CUDA_cublas_LIBRARY}) + endif() +endmacro() + +############################################################################### +############################################################################### +# CUDA BUILD CLEAN TARGET +############################################################################### +############################################################################### +macro(CUDA_BUILD_CLEAN_TARGET) + # Call this after you add all your CUDA targets, and you will get a convience + # target. You should also make clean after running this target to get the + # build system to generate all the code again. + + set(cuda_clean_target_name clean_cuda_depends) + if (CMAKE_GENERATOR MATCHES "Visual Studio") + string(TOUPPER ${cuda_clean_target_name} cuda_clean_target_name) + endif() + add_custom_target(${cuda_clean_target_name} + COMMAND ${CMAKE_COMMAND} -E remove ${CUDA_ADDITIONAL_CLEAN_FILES}) + + # Clear out the variable, so the next time we configure it will be empty. + # This is useful so that the files won't persist in the list after targets + # have been removed. + set(CUDA_ADDITIONAL_CLEAN_FILES "" CACHE INTERNAL "List of intermediate files that are part of the cuda dependency scanning.") +endmacro(CUDA_BUILD_CLEAN_TARGET) diff --git a/uav_simulator/local_sensing/CMakeModules/FindCUDA/make2cmake.cmake b/uav_simulator/local_sensing/CMakeModules/FindCUDA/make2cmake.cmake new file mode 100644 index 000000000..7fce167c2 --- /dev/null +++ b/uav_simulator/local_sensing/CMakeModules/FindCUDA/make2cmake.cmake @@ -0,0 +1,79 @@ +# James Bigler, NVIDIA Corp (nvidia.com - jbigler) +# Abe Stephens, SCI Institute -- http://www.sci.utah.edu/~abe/FindCuda.html +# +# Copyright (c) 2008 - 2009 NVIDIA Corporation. All rights reserved. +# +# Copyright (c) 2007-2009 +# Scientific Computing and Imaging Institute, University of Utah +# +# This code is licensed under the MIT License. See the FindCUDA.cmake script +# for the text of the license. + +# The MIT License +# +# License for the specific language governing rights and limitations under +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the "Software"), +# to deal in the Software without restriction, including without limitation +# the rights to use, copy, modify, merge, publish, distribute, sublicense, +# and/or sell copies of the Software, and to permit persons to whom the +# Software is furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +# DEALINGS IN THE SOFTWARE. +# + +####################################################################### +# This converts a file written in makefile syntax into one that can be included +# by CMake. + +file(READ ${input_file} depend_text) + +if (${depend_text} MATCHES ".+") + + # message("FOUND DEPENDS") + + # Remember, four backslashes is escaped to one backslash in the string. + string(REGEX REPLACE "\\\\ " " " depend_text ${depend_text}) + + # This works for the nvcc -M generated dependency files. + string(REGEX REPLACE "^.* : " "" depend_text ${depend_text}) + string(REGEX REPLACE "[ \\\\]*\n" ";" depend_text ${depend_text}) + + set(dependency_list "") + + foreach(file ${depend_text}) + + string(REGEX REPLACE "^ +" "" file ${file}) + + if(NOT IS_DIRECTORY ${file}) + # If softlinks start to matter, we should change this to REALPATH. For now we need + # to flatten paths, because nvcc can generate stuff like /bin/../include instead of + # just /include. + get_filename_component(file_absolute "${file}" ABSOLUTE) + list(APPEND dependency_list "${file_absolute}") + endif(NOT IS_DIRECTORY ${file}) + + endforeach(file) + +else() + # message("FOUND NO DEPENDS") +endif() + +# Remove the duplicate entries and sort them. +list(REMOVE_DUPLICATES dependency_list) +list(SORT dependency_list) + +foreach(file ${dependency_list}) + set(cuda_nvcc_depend "${cuda_nvcc_depend} \"${file}\"\n") +endforeach() + +file(WRITE ${output_file} "# Generated by: make2cmake.cmake\nSET(CUDA_NVCC_DEPEND\n ${cuda_nvcc_depend})\n\n") diff --git a/uav_simulator/local_sensing/CMakeModules/FindCUDA/parse_cubin.cmake b/uav_simulator/local_sensing/CMakeModules/FindCUDA/parse_cubin.cmake new file mode 100644 index 000000000..2518c6852 --- /dev/null +++ b/uav_simulator/local_sensing/CMakeModules/FindCUDA/parse_cubin.cmake @@ -0,0 +1,112 @@ +# James Bigler, NVIDIA Corp (nvidia.com - jbigler) +# Abe Stephens, SCI Institute -- http://www.sci.utah.edu/~abe/FindCuda.html +# +# Copyright (c) 2008 - 2009 NVIDIA Corporation. All rights reserved. +# +# Copyright (c) 2007-2009 +# Scientific Computing and Imaging Institute, University of Utah +# +# This code is licensed under the MIT License. See the FindCUDA.cmake script +# for the text of the license. + +# The MIT License +# +# License for the specific language governing rights and limitations under +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the "Software"), +# to deal in the Software without restriction, including without limitation +# the rights to use, copy, modify, merge, publish, distribute, sublicense, +# and/or sell copies of the Software, and to permit persons to whom the +# Software is furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +# DEALINGS IN THE SOFTWARE. +# + +####################################################################### +# Parses a .cubin file produced by nvcc and reports statistics about the file. + + +file(READ ${input_file} file_text) + +if (${file_text} MATCHES ".+") + + # Remember, four backslashes is escaped to one backslash in the string. + string(REGEX REPLACE ";" "\\\\;" file_text ${file_text}) + string(REGEX REPLACE "\ncode" ";code" file_text ${file_text}) + + list(LENGTH file_text len) + + foreach(line ${file_text}) + + # Only look at "code { }" blocks. + if(line MATCHES "^code") + + # Break into individual lines. + string(REGEX REPLACE "\n" ";" line ${line}) + + foreach(entry ${line}) + + # Extract kernel names. + if (${entry} MATCHES "[^g]name = ([^ ]+)") + string(REGEX REPLACE ".* = ([^ ]+)" "\\1" entry ${entry}) + + # Check to see if the kernel name starts with "_" + set(skip FALSE) + # if (${entry} MATCHES "^_") + # Skip the rest of this block. + # message("Skipping ${entry}") + # set(skip TRUE) + # else (${entry} MATCHES "^_") + message("Kernel: ${entry}") + # endif (${entry} MATCHES "^_") + + endif(${entry} MATCHES "[^g]name = ([^ ]+)") + + # Skip the rest of the block if necessary + if(NOT skip) + + # Registers + if (${entry} MATCHES "reg([ ]+)=([ ]+)([^ ]+)") + string(REGEX REPLACE ".*([ ]+)=([ ]+)([^ ]+)" "\\3" entry ${entry}) + message("Registers: ${entry}") + endif() + + # Local memory + if (${entry} MATCHES "lmem([ ]+)=([ ]+)([^ ]+)") + string(REGEX REPLACE ".*([ ]+)=([ ]+)([^ ]+)" "\\3" entry ${entry}) + message("Local: ${entry}") + endif() + + # Shared memory + if (${entry} MATCHES "smem([ ]+)=([ ]+)([^ ]+)") + string(REGEX REPLACE ".*([ ]+)=([ ]+)([^ ]+)" "\\3" entry ${entry}) + message("Shared: ${entry}") + endif() + + if (${entry} MATCHES "^}") + message("") + endif() + + endif(NOT skip) + + + endforeach(entry) + + endif(line MATCHES "^code") + + endforeach(line) + +else() + # message("FOUND NO DEPENDS") +endif() + + diff --git a/uav_simulator/local_sensing/CMakeModules/FindCUDA/run_nvcc.cmake b/uav_simulator/local_sensing/CMakeModules/FindCUDA/run_nvcc.cmake new file mode 100644 index 000000000..dce98e39c --- /dev/null +++ b/uav_simulator/local_sensing/CMakeModules/FindCUDA/run_nvcc.cmake @@ -0,0 +1,280 @@ +# James Bigler, NVIDIA Corp (nvidia.com - jbigler) +# +# Copyright (c) 2008 - 2009 NVIDIA Corporation. All rights reserved. +# +# This code is licensed under the MIT License. See the FindCUDA.cmake script +# for the text of the license. + +# The MIT License +# +# License for the specific language governing rights and limitations under +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the "Software"), +# to deal in the Software without restriction, including without limitation +# the rights to use, copy, modify, merge, publish, distribute, sublicense, +# and/or sell copies of the Software, and to permit persons to whom the +# Software is furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +# DEALINGS IN THE SOFTWARE. + + +########################################################################## +# This file runs the nvcc commands to produce the desired output file along with +# the dependency file needed by CMake to compute dependencies. In addition the +# file checks the output of each command and if the command fails it deletes the +# output files. + +# Input variables +# +# verbose:BOOL=<> OFF: Be as quiet as possible (default) +# ON : Describe each step +# +# build_configuration:STRING=<> Typically one of Debug, MinSizeRel, Release, or +# RelWithDebInfo, but it should match one of the +# entries in CUDA_HOST_FLAGS. This is the build +# configuration used when compiling the code. If +# blank or unspecified Debug is assumed as this is +# what CMake does. +# +# generated_file:STRING=<> File to generate. This argument must be passed in. +# +# generated_cubin_file:STRING=<> File to generate. This argument must be passed +# in if build_cubin is true. + +if(NOT generated_file) + message(FATAL_ERROR "You must specify generated_file on the command line") +endif() + +# Set these up as variables to make reading the generated file easier +set(CMAKE_COMMAND "@CMAKE_COMMAND@") +set(source_file "@source_file@") +set(NVCC_generated_dependency_file "@NVCC_generated_dependency_file@") +set(cmake_dependency_file "@cmake_dependency_file@") +set(CUDA_make2cmake "@CUDA_make2cmake@") +set(CUDA_parse_cubin "@CUDA_parse_cubin@") +set(build_cubin @build_cubin@) +# We won't actually use these variables for now, but we need to set this, in +# order to force this file to be run again if it changes. +set(generated_file_path "@generated_file_path@") +set(generated_file_internal "@generated_file@") +set(generated_cubin_file_internal "@generated_cubin_file@") + +set(CUDA_NVCC_EXECUTABLE "@CUDA_NVCC_EXECUTABLE@") +set(CUDA_NVCC_FLAGS "@CUDA_NVCC_FLAGS@;;@CUDA_WRAP_OPTION_NVCC_FLAGS@") +@CUDA_NVCC_FLAGS_CONFIG@ +set(nvcc_flags @nvcc_flags@) +set(CUDA_NVCC_INCLUDE_ARGS "@CUDA_NVCC_INCLUDE_ARGS@") +set(format_flag "@format_flag@") + +if(build_cubin AND NOT generated_cubin_file) + message(FATAL_ERROR "You must specify generated_cubin_file on the command line") +endif() + +# This is the list of host compilation flags. It C or CXX should already have +# been chosen by FindCUDA.cmake. +@CUDA_HOST_FLAGS@ + +# Take the compiler flags and package them up to be sent to the compiler via -Xcompiler +set(nvcc_host_compiler_flags "") +# If we weren't given a build_configuration, use Debug. +if(NOT build_configuration) + set(build_configuration Debug) +endif() +string(TOUPPER "${build_configuration}" build_configuration) +#message("CUDA_NVCC_HOST_COMPILER_FLAGS = ${CUDA_NVCC_HOST_COMPILER_FLAGS}") +foreach(flag ${CMAKE_HOST_FLAGS} ${CMAKE_HOST_FLAGS_${build_configuration}}) + # Extra quotes are added around each flag to help nvcc parse out flags with spaces. + set(nvcc_host_compiler_flags "${nvcc_host_compiler_flags},\"${flag}\"") +endforeach() +if (nvcc_host_compiler_flags) + set(nvcc_host_compiler_flags "-Xcompiler" ${nvcc_host_compiler_flags}) +endif() +#message("nvcc_host_compiler_flags = \"${nvcc_host_compiler_flags}\"") +# Add the build specific configuration flags +list(APPEND CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS_${build_configuration}}) + +if(DEFINED CCBIN) + set(CCBIN -ccbin "${CCBIN}") +endif() + +# cuda_execute_process - Executes a command with optional command echo and status message. +# +# status - Status message to print if verbose is true +# command - COMMAND argument from the usual execute_process argument structure +# ARGN - Remaining arguments are the command with arguments +# +# CUDA_result - return value from running the command +# +# Make this a macro instead of a function, so that things like RESULT_VARIABLE +# and other return variables are present after executing the process. +macro(cuda_execute_process status command) + set(_command ${command}) + if(NOT _command STREQUAL "COMMAND") + message(FATAL_ERROR "Malformed call to cuda_execute_process. Missing COMMAND as second argument. (command = ${command})") + endif() + if(verbose) + execute_process(COMMAND "${CMAKE_COMMAND}" -E echo -- ${status}) + # Now we need to build up our command string. We are accounting for quotes + # and spaces, anything else is left up to the user to fix if they want to + # copy and paste a runnable command line. + set(cuda_execute_process_string) + foreach(arg ${ARGN}) + # If there are quotes, excape them, so they come through. + string(REPLACE "\"" "\\\"" arg ${arg}) + # Args with spaces need quotes around them to get them to be parsed as a single argument. + if(arg MATCHES " ") + list(APPEND cuda_execute_process_string "\"${arg}\"") + else() + list(APPEND cuda_execute_process_string ${arg}) + endif() + endforeach() + # Echo the command + execute_process(COMMAND ${CMAKE_COMMAND} -E echo ${cuda_execute_process_string}) + endif(verbose) + # Run the command + execute_process(COMMAND ${ARGN} RESULT_VARIABLE CUDA_result ) +endmacro() + +# Delete the target file +cuda_execute_process( + "Removing ${generated_file}" + COMMAND "${CMAKE_COMMAND}" -E remove "${generated_file}" + ) + +# For CUDA 2.3 and below, -G -M doesn't work, so remove the -G flag +# for dependency generation and hope for the best. +set(depends_CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS}") +set(CUDA_VERSION @CUDA_VERSION@) +if(CUDA_VERSION VERSION_LESS "3.0") + cmake_policy(PUSH) + # CMake policy 0007 NEW states that empty list elements are not + # ignored. I'm just setting it to avoid the warning that's printed. + cmake_policy(SET CMP0007 NEW) + # Note that this will remove all occurances of -G. + list(REMOVE_ITEM depends_CUDA_NVCC_FLAGS "-G") + cmake_policy(POP) +endif() + +# nvcc doesn't define __CUDACC__ for some reason when generating dependency files. This +# can cause incorrect dependencies when #including files based on this macro which is +# defined in the generating passes of nvcc invokation. We will go ahead and manually +# define this for now until a future version fixes this bug. +set(CUDACC_DEFINE -D__CUDACC__) + +# Generate the dependency file +cuda_execute_process( + "Generating dependency file: ${NVCC_generated_dependency_file}" + COMMAND "${CUDA_NVCC_EXECUTABLE}" + -M + ${CUDACC_DEFINE} + "${source_file}" + -o "${NVCC_generated_dependency_file}" + ${CCBIN} + ${nvcc_flags} + ${nvcc_host_compiler_flags} + ${depends_CUDA_NVCC_FLAGS} + -DNVCC + ${CUDA_NVCC_INCLUDE_ARGS} + ) + +if(CUDA_result) + message(FATAL_ERROR "Error generating ${generated_file}") +endif() + +# Generate the cmake readable dependency file to a temp file. Don't put the +# quotes just around the filenames for the input_file and output_file variables. +# CMake will pass the quotes through and not be able to find the file. +cuda_execute_process( + "Generating temporary cmake readable file: ${cmake_dependency_file}.tmp" + COMMAND "${CMAKE_COMMAND}" + -D "input_file:FILEPATH=${NVCC_generated_dependency_file}" + -D "output_file:FILEPATH=${cmake_dependency_file}.tmp" + -P "${CUDA_make2cmake}" + ) + +if(CUDA_result) + message(FATAL_ERROR "Error generating ${generated_file}") +endif() + +# Copy the file if it is different +cuda_execute_process( + "Copy if different ${cmake_dependency_file}.tmp to ${cmake_dependency_file}" + COMMAND "${CMAKE_COMMAND}" -E copy_if_different "${cmake_dependency_file}.tmp" "${cmake_dependency_file}" + ) + +if(CUDA_result) + message(FATAL_ERROR "Error generating ${generated_file}") +endif() + +# Delete the temporary file +cuda_execute_process( + "Removing ${cmake_dependency_file}.tmp and ${NVCC_generated_dependency_file}" + COMMAND "${CMAKE_COMMAND}" -E remove "${cmake_dependency_file}.tmp" "${NVCC_generated_dependency_file}" + ) + +if(CUDA_result) + message(FATAL_ERROR "Error generating ${generated_file}") +endif() + +# Generate the code +cuda_execute_process( + "Generating ${generated_file}" + COMMAND "${CUDA_NVCC_EXECUTABLE}" + "${source_file}" + ${format_flag} -o "${generated_file}" + ${CCBIN} + ${nvcc_flags} + ${nvcc_host_compiler_flags} + ${CUDA_NVCC_FLAGS} + -DNVCC + ${CUDA_NVCC_INCLUDE_ARGS} + ) + +if(CUDA_result) + # Since nvcc can sometimes leave half done files make sure that we delete the output file. + cuda_execute_process( + "Removing ${generated_file}" + COMMAND "${CMAKE_COMMAND}" -E remove "${generated_file}" + ) + message(FATAL_ERROR "Error generating file ${generated_file}") +else() + if(verbose) + message("Generated ${generated_file} successfully.") + endif() +endif() + +# Cubin resource report commands. +if( build_cubin ) + # Run with -cubin to produce resource usage report. + cuda_execute_process( + "Generating ${generated_cubin_file}" + COMMAND "${CUDA_NVCC_EXECUTABLE}" + "${source_file}" + ${CUDA_NVCC_FLAGS} + ${nvcc_flags} + ${CCBIN} + ${nvcc_host_compiler_flags} + -DNVCC + -cubin + -o "${generated_cubin_file}" + ${CUDA_NVCC_INCLUDE_ARGS} + ) + + # Execute the parser script. + cuda_execute_process( + "Executing the parser script" + COMMAND "${CMAKE_COMMAND}" + -D "input_file:STRING=${generated_cubin_file}" + -P "${CUDA_parse_cubin}" + ) + +endif( build_cubin ) diff --git a/uav_simulator/local_sensing/CMakeModules/FindEigen.cmake b/uav_simulator/local_sensing/CMakeModules/FindEigen.cmake new file mode 100644 index 000000000..cac56d82f --- /dev/null +++ b/uav_simulator/local_sensing/CMakeModules/FindEigen.cmake @@ -0,0 +1,81 @@ +############################################################################### +# +# CMake script for finding the Eigen library. +# +# http://eigen.tuxfamily.org/index.php?title=Main_Page +# +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD +# license. +# +# +# Input variables: +# +# - Eigen_ROOT_DIR (optional): When specified, header files and libraries +# will be searched for in `${Eigen_ROOT_DIR}/include` and +# `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order +# will be ignored. When unspecified, the default CMake search order is used. +# This variable can be specified either as a CMake or environment variable. +# If both are set, preference is given to the CMake variable. +# Use this variable for finding packages installed in a nonstandard location, +# or for enforcing that one of multiple package installations is picked up. +# +# Cache variables (not intended to be used in CMakeLists.txt files) +# +# - Eigen_INCLUDE_DIR: Absolute path to package headers. +# +# +# Output variables: +# +# - Eigen_FOUND: Boolean that indicates if the package was found +# - Eigen_INCLUDE_DIRS: Paths to the necessary header files +# - Eigen_VERSION: Version of Eigen library found +# - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen +# +# +# Example usage: +# +# # Passing the version means Eigen_FOUND will only be TRUE if a +# # version >= the provided version is found. +# find_package(Eigen 3.1.2) +# if(NOT Eigen_FOUND) +# # Error handling +# endif() +# ... +# add_definitions(${Eigen_DEFINITIONS}) +# ... +# include_directories(${Eigen_INCLUDE_DIRS} ...) +# +############################################################################### + +find_package(PkgConfig) +pkg_check_modules(PC_EIGEN eigen3) +set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) + +set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3/") +find_path(EIGEN_INCLUDE_DIR Eigen/Core + HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} + "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" + "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility + PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" + "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" + PATH_SUFFIXES eigen3 include/eigen3 include) + +set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) + +mark_as_advanced(EIGEN_INCLUDE_DIR) + +if(EIGEN_FOUND) + message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") +endif(EIGEN_FOUND) + + +set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) +set(Eigen_FOUND ${EIGEN_FOUND}) +set(Eigen_VERSION ${EIGEN_VERSION}) +set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) diff --git a/uav_simulator/local_sensing/CMakeModules/FindEigen.cmake~ b/uav_simulator/local_sensing/CMakeModules/FindEigen.cmake~ new file mode 100644 index 000000000..8587367d3 --- /dev/null +++ b/uav_simulator/local_sensing/CMakeModules/FindEigen.cmake~ @@ -0,0 +1,81 @@ +############################################################################### +# +# CMake script for finding the Eigen library. +# +# http://eigen.tuxfamily.org/index.php?title=Main_Page +# +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD +# license. +# +# +# Input variables: +# +# - Eigen_ROOT_DIR (optional): When specified, header files and libraries +# will be searched for in `${Eigen_ROOT_DIR}/include` and +# `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order +# will be ignored. When unspecified, the default CMake search order is used. +# This variable can be specified either as a CMake or environment variable. +# If both are set, preference is given to the CMake variable. +# Use this variable for finding packages installed in a nonstandard location, +# or for enforcing that one of multiple package installations is picked up. +# +# Cache variables (not intended to be used in CMakeLists.txt files) +# +# - Eigen_INCLUDE_DIR: Absolute path to package headers. +# +# +# Output variables: +# +# - Eigen_FOUND: Boolean that indicates if the package was found +# - Eigen_INCLUDE_DIRS: Paths to the necessary header files +# - Eigen_VERSION: Version of Eigen library found +# - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen +# +# +# Example usage: +# +# # Passing the version means Eigen_FOUND will only be TRUE if a +# # version >= the provided version is found. +# find_package(Eigen 3.1.2) +# if(NOT Eigen_FOUND) +# # Error handling +# endif() +# ... +# add_definitions(${Eigen_DEFINITIONS}) +# ... +# include_directories(${Eigen_INCLUDE_DIRS} ...) +# +############################################################################### + +find_package(PkgConfig) +pkg_check_modules(PC_EIGEN eigen3) +set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) + + +find_path(EIGEN_INCLUDE_DIR Eigen/Core + HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} + "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" + "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility + PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" + "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" + PATH_SUFFIXES eigen3 include/eigen3 include) + +set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) + +mark_as_advanced(EIGEN_INCLUDE_DIR) + +if(EIGEN_FOUND) + message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") +endif(EIGEN_FOUND) + + +set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) +set(Eigen_FOUND ${EIGEN_FOUND}) +set(Eigen_VERSION ${EIGEN_VERSION}) +set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) diff --git a/uav_simulator/local_sensing/cfg/local_sensing_node.cfg b/uav_simulator/local_sensing/cfg/local_sensing_node.cfg new file mode 100755 index 000000000..ac82ee45a --- /dev/null +++ b/uav_simulator/local_sensing/cfg/local_sensing_node.cfg @@ -0,0 +1,15 @@ +#!/usr/bin/env python +PACKAGE = "local_sensing_node" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("tx", double_t, 0, "tx", 0.0, -1000.0, 1000.0) +gen.add("ty", double_t, 0, "ty", 0.0, -1000.0, 1000.0) +gen.add("tz", double_t, 0, "tz", 0.0, -1000.0, 1000.0) +gen.add("axis_x", double_t, 0, "axis_x", 0.0, -360.0, 360.0) +gen.add("axis_y", double_t, 0, "axis_y", 0.0, -360.0, 360.0) +gen.add("axis_z", double_t, 0, "axis_z", 0.0, -360.0, 360.0) + +exit(gen.generate(PACKAGE, "local_sensing_node", "local_sensing_node")) \ No newline at end of file diff --git a/uav_simulator/local_sensing/package.xml b/uav_simulator/local_sensing/package.xml new file mode 100644 index 000000000..ae0110c17 --- /dev/null +++ b/uav_simulator/local_sensing/package.xml @@ -0,0 +1,38 @@ + + + local_sensing_node + 0.1.0 + + render depth from depth + + Matia Pizzoli + Matia Pizzoli + GPLv3 + + + catkin + + + cmake_modules + roscpp + roslib + svo_msgs + cv_bridge + image_transport + vikit_ros + pcl_ros + dynamic_reconfigure + quadrotor_msgs + + + roscpp + roslib + svo_msgs + cv_bridge + image_transport + vikit_ros + pcl_ros + dynamic_reconfigure + quadrotor_msgs + + diff --git a/uav_simulator/local_sensing/params/camera.yaml b/uav_simulator/local_sensing/params/camera.yaml new file mode 100644 index 000000000..7bb4e2a97 --- /dev/null +++ b/uav_simulator/local_sensing/params/camera.yaml @@ -0,0 +1,12 @@ +cam_width: 640 +cam_height: 480 +cam_fx: 385.754 +cam_fy: 257.296 +cam_cx: 385.754 +cam_cy: 236.743 +# cam_width: 752 +# cam_height: 480 +# cam_fx: 258.654 +# cam_fy: 257.296 +# cam_cx: 367.215 +# cam_cy: 248.375 \ No newline at end of file diff --git a/uav_simulator/local_sensing/src/AlignError.h b/uav_simulator/local_sensing/src/AlignError.h new file mode 100644 index 000000000..04d4076e3 --- /dev/null +++ b/uav_simulator/local_sensing/src/AlignError.h @@ -0,0 +1,86 @@ +#include +#include + +struct AlignError { + AlignError( const Eigen::Quaterniond camera_pose, const Eigen::Vector3d camera_trans, + const Eigen::Quaterniond velodyne_pose, const Eigen::Vector3d velodyne_trans) + { + camera_q[0] = camera_pose.x(); + camera_q[1] = camera_pose.y(); + camera_q[2] = camera_pose.z(); + camera_q[3] = camera_pose.w(); + camera_t[0] = camera_trans.x(); + camera_t[1] = camera_trans.y(); + camera_t[2] = camera_trans.z(); + + velodyne_q[0] = velodyne_pose.x(); + velodyne_q[1] = velodyne_pose.y(); + velodyne_q[2] = velodyne_pose.z(); + velodyne_q[3] = velodyne_pose.w(); + velodyne_t[0] = velodyne_trans.x(); + velodyne_t[1] = velodyne_trans.y(); + velodyne_t[2] = velodyne_trans.z(); + } + + // Factory to hide the construction of the CostFunction object from the client code. + static ceres::CostFunction* Create( const Eigen::Quaterniond camera_pose, const Eigen::Vector3d camera_trans, + const Eigen::Quaterniond velodyne_pose, const Eigen::Vector3d velodyne_trans) + { + return (new ceres::AutoDiffCostFunction( + new AlignError(camera_pose, camera_trans, velodyne_pose, velodyne_trans))); + } + + template + bool operator()(const T* const world_rotation, const T* const world_translation, + const T* const v2c_rotation, const T* const v2c_translation, + T* residuals) const + { + Eigen::Quaternion q_world = Eigen::Map< const Eigen::Quaternion >(world_rotation); + Eigen::Matrix t_world = Eigen::Map< const Eigen::Matrix >(world_translation); + + Eigen::Quaternion q_v2c = Eigen::Map< const Eigen::Quaternion >(v2c_rotation); + Eigen::Matrix t_v2c = Eigen::Map< const Eigen::Matrix >(v2c_translation); + + Eigen::Quaternion q_c; + Eigen::Matrix t_c; + q_c.x() = T(camera_q[0]); + q_c.y() = T(camera_q[1]); + q_c.z() = T(camera_q[2]); + q_c.w() = T(camera_q[3]); + t_c << T(camera_t[0]), T(camera_t[1]), T(camera_t[2]); + + Eigen::Quaternion q_v; + Eigen::Matrix t_v; + q_v.x() = T(velodyne_q[0]); + q_v.y() = T(velodyne_q[1]); + q_v.z() = T(velodyne_q[2]); + q_v.w() = T(velodyne_q[3]); + t_v << T(velodyne_t[0]), T(velodyne_t[1]), T(velodyne_t[2]); + + Eigen::Quaternion q_left; + Eigen::Matrix t_left; + + q_left = q_world * q_c * q_v2c; + t_left = q_world * q_c * t_v2c + q_world * t_c + t_world; + + Eigen::Quaternion q_diff; + Eigen::Matrix t_diff; + q_diff = q_left * q_v.inverse(); + // t_diff = t_left - q_left * q_v.inverse() * t_v; + t_diff = t_left - t_v; + + residuals[0] = q_diff.x(); + residuals[1] = q_diff.y(); + residuals[2] = q_diff.z(); + residuals[3] = t_diff(0); + residuals[4] = t_diff(1); + residuals[5] = t_diff(2); + + return true; + } + + double camera_q[4]; + double camera_t[3]; + double velodyne_q[4]; + double velodyne_t[3]; +}; diff --git a/uav_simulator/local_sensing/src/ceres_extensions.h b/uav_simulator/local_sensing/src/ceres_extensions.h new file mode 100644 index 000000000..a79fcf4e0 --- /dev/null +++ b/uav_simulator/local_sensing/src/ceres_extensions.h @@ -0,0 +1,161 @@ +// +// ceres_extensions.h +// Bundle_Adjust_Test +// +// Created by Lloyd Hughes on 2014/04/11. +// Copyright (c) 2014 Lloyd Hughes. All rights reserved. +// hughes.lloyd@gmail.com +// + +#ifndef CERES_EXTENSIONS_H +#define CERES_EXTENSIONS_H + +#include +#include +#include + +namespace ceres_ext { + + // Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x + // with * being the quaternion multiplication operator. Here we assume + // that the first element of the quaternion vector is the real (cos + // theta) part. + class EigenQuaternionParameterization : public ceres::LocalParameterization + { + public: + virtual ~EigenQuaternionParameterization() {} + + virtual bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const + { + const Eigen::Map x(x_raw); + const Eigen::Map delta(delta_raw); + + Eigen::Map x_plus_delta(x_plus_delta_raw); + + const double delta_norm = delta.norm(); + if ( delta_norm > 0.0 ) + { + const double sin_delta_by_delta = sin(delta_norm) / delta_norm; + Eigen::Quaterniond tmp( cos(delta_norm), sin_delta_by_delta*delta[0], sin_delta_by_delta*delta[1], sin_delta_by_delta*delta[2] ); + + x_plus_delta = tmp*x; + } + else + { + x_plus_delta = x; + } + return true; + } + + virtual bool ComputeJacobian(const double* x, double* jacobian) const + { + jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT x + jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT y + jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT z + jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT w + return true; + } + + virtual int GlobalSize() const { return 4; } + virtual int LocalSize() const { return 3; } + + }; + + template inline + void EigenQuaternionToScaledRotation(const T q[4], T R[3 * 3]) { + EigenQuaternionToScaledRotation(q, RowMajorAdapter3x3(R)); + } + + template inline + void EigenQuaternionToScaledRotation(const T q[4], + const ceres::MatrixAdapter& R) { + // Make convenient names for elements of q. + T a = q[3]; + T b = q[0]; + T c = q[1]; + T d = q[2]; + // This is not to eliminate common sub-expression, but to + // make the lines shorter so that they fit in 80 columns! + T aa = a * a; + T ab = a * b; + T ac = a * c; + T ad = a * d; + T bb = b * b; + T bc = b * c; + T bd = b * d; + T cc = c * c; + T cd = c * d; + T dd = d * d; + + R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); // NOLINT + R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); // NOLINT + R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; // NOLINT + } + + template inline + void EigenQuaternionToRotation(const T q[4], T R[3 * 3]) { + EigenQuaternionToRotation(q, RowMajorAdapter3x3(R)); + } + + template inline + void EigenQuaternionToRotation(const T q[4], + const ceres::MatrixAdapter& R) { + EigenQuaternionToScaledRotation(q, R); + + T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; + CHECK_NE(normalizer, T(0)); + normalizer = T(1) / normalizer; + + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + R(i, j) *= normalizer; + } + } + } + + template inline + void EigenUnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + const T t2 = q[3] * q[0]; + const T t3 = q[3] * q[1]; + const T t4 = q[3] * q[2]; + const T t5 = -q[0] * q[0]; + const T t6 = q[0] * q[1]; + const T t7 = q[0] * q[2]; + const T t8 = -q[1] * q[1]; + const T t9 = q[1] * q[2]; + const T t1 = -q[2] * q[2]; + result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT + result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT + result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT + } + + template inline + void EigenQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + // 'scale' is 1 / norm(q). + const T scale = T(1) / sqrt(q[0] * q[0] + + q[1] * q[1] + + q[2] * q[2] + + q[3] * q[3]); + + // Make unit-norm version of q. + const T unit[4] = { + scale * q[0], + scale * q[1], + scale * q[2], + scale * q[3], + }; + + EigenUnitQuaternionRotatePoint(unit, pt, result); + } + + template inline + void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) { + zw[0] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; + zw[1] = - z[0] * w[2] + z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; + zw[2] = z[0] * w[1] - z[1] * w[0] + z[2] * w[3] + z[3] * w[2]; + zw[3] = - z[0] * w[0] - z[1] * w[1] - z[2] * w[2] + z[3] * w[3]; + } + +} + +#endif \ No newline at end of file diff --git a/uav_simulator/local_sensing/src/csv_convert.py b/uav_simulator/local_sensing/src/csv_convert.py new file mode 100644 index 000000000..0040baa8e --- /dev/null +++ b/uav_simulator/local_sensing/src/csv_convert.py @@ -0,0 +1,15 @@ +import csv +file_location = '/home/wang/bag/banchmark/EuRoC/ViconRoom101/state_groundtruth_estimate0/data.csv' +with open('/home/wang/bag/banchmark/EuRoC/ViconRoom101/state_groundtruth_estimate0/data.txt', 'w') as txt_f: + with open(file_location) as f: + f_csv = csv.reader(f) + headers = next(f_csv) + for row in f_csv: + txt_f.write('%lf\n'% (float(row[0]) / 1000000000.0) ) + txt_f.write('%lf\n'% (float(row[1])) ) + txt_f.write('%lf\n'% (float(row[2])) ) + txt_f.write('%lf\n'% (float(row[3])) ) + txt_f.write('%lf\n'% (float(row[4])) ) + txt_f.write('%lf\n'% (float(row[5])) ) + txt_f.write('%lf\n'% (float(row[6])) ) + txt_f.write('%lf\n'% (float(row[7])) ) \ No newline at end of file diff --git a/uav_simulator/local_sensing/src/cuda_exception.cuh b/uav_simulator/local_sensing/src/cuda_exception.cuh new file mode 100644 index 000000000..cf8213fd5 --- /dev/null +++ b/uav_simulator/local_sensing/src/cuda_exception.cuh @@ -0,0 +1,45 @@ +// This file is part of REMODE - REgularized MOnocular Depth Estimation. +// +// Copyright (C) 2014 Matia Pizzoli +// Robotics and Perception Group, University of Zurich, Switzerland +// http://rpg.ifi.uzh.ch +// +// REMODE 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 any later version. +// +// REMODE 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 . + +#ifndef RMD_CUDA_EXCEPTION_CUH_ +#define RMD_CUDA_EXCEPTION_CUH_ + +#include +#include + + +struct CudaException : public std::exception +{ + CudaException(const std::string& what, cudaError err) + : what_(what), err_(err) {} + virtual ~CudaException() throw() {} + virtual const char* what() const throw() + { + std::stringstream description; + description << "CudaException: " << what_ << std::endl; + if(err_ != cudaSuccess) + { + description << "cudaError code: " << cudaGetErrorString(err_); + description << " (" << err_ << ")" << std::endl; + } + return description.str().c_str(); + } + std::string what_; + cudaError err_; +}; + +#endif /* RMD_CUDA_EXCEPTION_CUH_ */ diff --git a/uav_simulator/local_sensing/src/depth_render.cu b/uav_simulator/local_sensing/src/depth_render.cu new file mode 100644 index 000000000..183678057 --- /dev/null +++ b/uav_simulator/local_sensing/src/depth_render.cu @@ -0,0 +1,197 @@ +#include "depth_render.cuh" +__global__ void render(float3 *data_devptr, Parameter *para_devptr, DeviceImage *depth_devptr) +{ + const int index = threadIdx.x + blockIdx.x * blockDim.x; + const Parameter para = *para_devptr; + if(index >= para.point_number) + return; + float3 my_point = data_devptr[index]; + + //transform + float3 trans_point; + trans_point.x = my_point.x * para.r[0][0] + my_point.y * para.r[0][1] + my_point.z * para.r[0][2] + para.t[0]; + trans_point.y = my_point.x * para.r[1][0] + my_point.y * para.r[1][1] + my_point.z * para.r[1][2] + para.t[1]; + trans_point.z = my_point.x * para.r[2][0] + my_point.y * para.r[2][1] + my_point.z * para.r[2][2] + para.t[2]; + + if(trans_point.z <= 0.0f) + return; + + //project + int2 projected; + projected.x = trans_point.x / trans_point.z * para.fx + para.cx + 0.5; + projected.y = trans_point.y / trans_point.z * para.fy + para.cy + 0.5; + if(projected.x < 0 || projected.x >= para.width || projected.y < 0 || projected.y >= para.height) + return; + + // float dist = length(trans_point); + float dist = trans_point.z; + int dist_mm = dist * 1000.0f + 0.5f; + + //int r = 0.0173 * para.fx / dist + 0.5f; +// int r = 0.0473 * para.fx / dist + 0.5f; + int r = 0.0573 * para.fx / dist + 0.5f; + for(int i = -r; i <= r; i++) + for(int j = -r; j <= r; j++) + { + int to_x = projected.x + j; + int to_y = projected.y + i; + if(to_x < 0 || to_x >= para.width || to_y < 0 || to_y >= para.height) + continue; + int *dist_ptr = &(depth_devptr->atXY(to_x, to_y)); + atomicMin(dist_ptr, dist_mm); + } +} + +__global__ void depth_initial(DeviceImage *depth_devptr) +{ + const int x = threadIdx.x + blockIdx.x * blockDim.x; + const int y = threadIdx.y + blockIdx.y * blockDim.y; + int width = depth_devptr->width; + int height = depth_devptr->height; + + if(x >= width || y >= height) + return; + + depth_devptr->atXY(x,y) = 999999; +} + +DepthRender::DepthRender(): + cloud_size(0), + host_cloud_ptr(NULL), + dev_cloud_ptr(NULL), + has_devptr(false) +{ +} + +DepthRender::~DepthRender() +{ + if(has_devptr) + { + free(host_cloud_ptr); + cudaFree(dev_cloud_ptr); + cudaFree(parameter_devptr); + } +} + +void DepthRender::set_para(float _fx, float _fy, float _cx, float _cy, int _width, int _height) +{ + parameter.fx = _fx; + parameter.fy = _fy; + parameter.cx = _cx; + parameter.cy = _cy; + parameter.width = _width; + parameter.height = _height; +} + +void DepthRender::set_data(vector &cloud_data) +{ + cloud_size = cloud_data.size() / 3; + parameter.point_number = cloud_size; + + host_cloud_ptr = (float3*) malloc(cloud_size * sizeof(float3)); + for(int i = 0; i < cloud_size; i++) + host_cloud_ptr[i] = make_float3(cloud_data[3*i], cloud_data[3*i+1], cloud_data[3*i+2]); + + cudaError err = cudaMalloc(&dev_cloud_ptr, cloud_size * sizeof(float3)); + if(err != cudaSuccess) + throw CudaException("DeviceLinear: unable to allocate linear memory.", err); + err = cudaMemcpy( + dev_cloud_ptr, + host_cloud_ptr, + cloud_size * sizeof(float3), + cudaMemcpyHostToDevice); + if(err != cudaSuccess) + throw CudaException("DeviceLinear: unable to copy data from host to device.", err); + + err = cudaMalloc(¶meter_devptr, sizeof(Parameter)); + if(err != cudaSuccess) + throw CudaException("DeviceLinear: unable to allocate linear memory.", err); + err = cudaMemcpy( + parameter_devptr, + ¶meter, + sizeof(Parameter), + cudaMemcpyHostToDevice); + if(err != cudaSuccess) + throw CudaException("DeviceLinear: unable to copy data from host to device.", err); + + has_devptr = true; + + //printf("load points done!\n"); +} + +/*void DepthRender::render_pose( Matrix3d &rotation, Vector3d &translation, int *host_ptr) +{ + for(int i = 0; i < 3; i++) + { + parameter.t[i] = translation(i); + for(int j = 0; j < 3; j++) + { + parameter.r[i][j] = rotation(i,j); + } + } + cudaError err = cudaMemcpy( + parameter_devptr, + ¶meter, + sizeof(Parameter), + cudaMemcpyHostToDevice); + if(err != cudaSuccess) + throw CudaException("DeviceLinear: unable to copy data from host to device.", err); + + DeviceImage depth_output(parameter.width, parameter.height); + depth_output.zero(); + + dim3 depth_block; + dim3 depth_grid; + depth_block.x = 16; + depth_block.y = 16; + depth_grid.x = (parameter.width + depth_block.x - 1 ) / depth_block.x; + depth_grid.y = (parameter.height + depth_block.y - 1 ) / depth_block.y; + depth_initial<<>>(depth_output.dev_ptr); + + dim3 render_block; + dim3 render_grid; + render_block.x = 64; + render_grid.x = (cloud_size + render_block.x - 1) / render_block.x; + render<<>>(dev_cloud_ptr, parameter_devptr, depth_output.dev_ptr); + + depth_output.getDevData(host_ptr); +} +*/ +//void DepthRender::render_pose( Matrix4d &transformation, int *host_ptr) +void DepthRender::render_pose( double * transformation, int *host_ptr) +{ + for(int i = 0; i < 3; i++) + { + parameter.t[i] = transformation[4 * i + 3];//transformation(i,3); + for(int j = 0; j < 3; j++) + { + parameter.r[i][j] = transformation[4 * i + j];//transformation(i,j); + } + } + cudaError err = cudaMemcpy( + parameter_devptr, + ¶meter, + sizeof(Parameter), + cudaMemcpyHostToDevice); + if(err != cudaSuccess) + throw CudaException("DeviceLinear: unable to copy data from host to device.", err); + + DeviceImage depth_output(parameter.width, parameter.height); + depth_output.zero(); + + dim3 depth_block; + dim3 depth_grid; + depth_block.x = 16; + depth_block.y = 16; + depth_grid.x = (parameter.width + depth_block.x - 1 ) / depth_block.x; + depth_grid.y = (parameter.height + depth_block.y - 1 ) / depth_block.y; + depth_initial<<>>(depth_output.dev_ptr); + + dim3 render_block; + dim3 render_grid; + render_block.x = 64; + render_grid.x = (cloud_size + render_block.x - 1) / render_block.x; + render<<>>(dev_cloud_ptr, parameter_devptr, depth_output.dev_ptr); + + depth_output.getDevData(host_ptr); +} \ No newline at end of file diff --git a/uav_simulator/local_sensing/src/depth_render.cuh b/uav_simulator/local_sensing/src/depth_render.cuh new file mode 100644 index 000000000..26b851815 --- /dev/null +++ b/uav_simulator/local_sensing/src/depth_render.cuh @@ -0,0 +1,52 @@ +#ifndef DEPTH_RENDER_CUH +#define DEPTH_RENDER_CUH + +#include +#include +#include +#include +#include +#include +//#include + +#include "device_image.cuh" +#include "cuda_exception.cuh" +#include "helper_math.h" + +using namespace std; +//using namespace Eigen; + +struct Parameter +{ + int point_number; + float fx, fy, cx, cy; + int width, height; + float r[3][3]; + float t[3]; +}; + +class DepthRender +{ +public: + DepthRender(); + void set_para(float _fx, float _fy, float _cx, float _cy, int _width, int _height); + ~DepthRender(); + void set_data(vector &cloud_data); + //void render_pose( Matrix3d &rotation, Vector3d &translation, int *host_ptr); + //void render_pose( Matrix4d &transformation, int *host_ptr); + void render_pose( double * transformation, int *host_ptr); + +private: + int cloud_size; + + //data + float3 *host_cloud_ptr; + float3 *dev_cloud_ptr; + bool has_devptr; + + //camera + Parameter parameter; + Parameter* parameter_devptr; +}; + +#endif \ No newline at end of file diff --git a/uav_simulator/local_sensing/src/device_image.cuh b/uav_simulator/local_sensing/src/device_image.cuh new file mode 100644 index 000000000..84e77ef36 --- /dev/null +++ b/uav_simulator/local_sensing/src/device_image.cuh @@ -0,0 +1,179 @@ +// This file is part of REMODE - REgularized MOnocular Depth Estimation. +// +// Copyright (C) 2014 Matia Pizzoli +// Robotics and Perception Group, University of Zurich, Switzerland +// http://rpg.ifi.uzh.ch +// +// REMODE 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 any later version. +// +// REMODE 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 . + +#ifndef DEVICE_IMAGE_CUH +#define DEVICE_IMAGE_CUH + +#include +#include +#include "cuda_exception.cuh" + +struct Size +{ + int width; + int height; +}; + +template +struct DeviceImage +{ + __host__ + DeviceImage(size_t width, size_t height) + : width(width), + height(height) + { + cudaError err = cudaMallocPitch( + &data, + &pitch, + width*sizeof(ElementType), + height); + if(err != cudaSuccess) + throw CudaException("Image: unable to allocate pitched memory.", err); + + stride = pitch / sizeof(ElementType); + + err = cudaMalloc( + &dev_ptr, + sizeof(*this)); + if(err != cudaSuccess) + throw CudaException("DeviceData, cannot allocate device memory to store image parameters.", err); + + err = cudaMemcpy( + dev_ptr, + this, + sizeof(*this), + cudaMemcpyHostToDevice); + if(err != cudaSuccess) + throw CudaException("DeviceData, cannot copy image parameters to device memory.", err); + } + + __device__ + ElementType & operator()(size_t x, size_t y) + { + return atXY(x, y); + } + + __device__ + const ElementType & operator()(size_t x, size_t y) const + { + return atXY(x, y); + } + + __device__ + ElementType & atXY(size_t x, size_t y) + { + return data[y*stride+x]; + } + + __device__ + const ElementType & atXY(size_t x, size_t y) const + { + return data[y*stride+x]; + } + + /// Upload aligned_data_row_major to device memory + __host__ + void setDevData(const ElementType * aligned_data_row_major) + { + const cudaError err = cudaMemcpy2D( + data, + pitch, + aligned_data_row_major, + width*sizeof(ElementType), + width*sizeof(ElementType), + height, + cudaMemcpyHostToDevice); + if(err != cudaSuccess) + throw CudaException("Image: unable to copy data from host to device.", err); + } + + /// Download the data from the device memory to aligned_data_row_major, a preallocated array in host memory + __host__ + void getDevData(ElementType* aligned_data_row_major) const + { + const cudaError err = cudaMemcpy2D( + aligned_data_row_major, // destination memory address + width*sizeof(ElementType), // pitch of destination memory + data, // source memory address + pitch, // pitch of source memory + width*sizeof(ElementType), // width of matrix transfer (columns in bytes) + height, // height of matrix transfer + cudaMemcpyDeviceToHost); + if(err != cudaSuccess) + throw CudaException("Image: unable to copy data from device to host.", err); + } + + __host__ + ~DeviceImage() + { + cudaError err = cudaFree(data); + if(err != cudaSuccess) + throw CudaException("Image: unable to free allocated memory.", err); + err = cudaFree(dev_ptr); + if(err != cudaSuccess) + throw CudaException("Image: unable to free allocated memory.", err); + } + + __host__ + cudaChannelFormatDesc getCudaChannelFormatDesc() const + { + return cudaCreateChannelDesc(); + } + + __host__ + void zero() + { + const cudaError err = cudaMemset2D( + data, + pitch, + 0, + width*sizeof(ElementType), + height); + if(err != cudaSuccess) + throw CudaException("Image: unable to zero.", err); + } + + __host__ + DeviceImage & operator=(const DeviceImage &other_image) + { + if(this != &other_image) + { + assert(width == other_image.width && + height == other_image.height); + const cudaError err = cudaMemcpy2D(data, + pitch, + other_image.data, + other_image.pitch, + width*sizeof(ElementType), + height, + cudaMemcpyDeviceToDevice); + if(err != cudaSuccess) + throw CudaException("Image, operator '=': unable to copy data from another image.", err); + } + return *this; + } + + // fields + size_t width; + size_t height; + size_t pitch; + size_t stride; + ElementType * data; + DeviceImage *dev_ptr; +}; + +#endif // DEVICE_IMAGE_CUH diff --git a/uav_simulator/local_sensing/src/empty.cpp b/uav_simulator/local_sensing/src/empty.cpp new file mode 100644 index 000000000..694822396 --- /dev/null +++ b/uav_simulator/local_sensing/src/empty.cpp @@ -0,0 +1 @@ +#include "empty.h" \ No newline at end of file diff --git a/uav_simulator/local_sensing/src/empty.h b/uav_simulator/local_sensing/src/empty.h new file mode 100644 index 000000000..e69de29bb diff --git a/uav_simulator/local_sensing/src/euroc.cpp b/uav_simulator/local_sensing/src/euroc.cpp new file mode 100644 index 000000000..307e255dd --- /dev/null +++ b/uav_simulator/local_sensing/src/euroc.cpp @@ -0,0 +1,378 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include "opencv2/highgui/highgui.hpp" +#include +#include + +#include "depth_render.cuh" + +using namespace cv; +using namespace std; +using namespace Eigen; + +typedef message_filters::sync_policies::ApproximateTime approx_policy; + +int *depth_hostptr; +cv::Mat depth_mat; + +int width, height; +cv::Mat cv_K, cv_D; +double fx,fy,cx,cy; +cv::Mat undist_map1, undist_map2; +bool is_distorted(false); + +DepthRender depthrender; +ros::Publisher pub_depth; +ros::Publisher pub_color; +ros::Publisher pub_posedimage; + +Matrix4d vicon2body; +Matrix4d cam02body; +Matrix4d cam2world; +Matrix4d vicon2leica; + +ros::Time receive_stamp; + +cv::Mat undistorted_image; + +struct PoseInfo +{ + Matrix4d pose; + double time; +}; + +vector gt_pose_vect; + +void render_currentpose(); + +bool read_pose(fstream &file) +{ + int count = 0; + bool good = true; + double para[16]; + while(good) + { + count ++; + for(int i = 0; i < 8 && good; i++) + { + good = good && file >> para[i]; + } + if(good) + { + Eigen::Vector3d request_position; + Eigen::Quaterniond request_pose; + request_position.x() = para[1]; + request_position.y() = para[2]; + request_position.z() = para[3]; + request_pose.w() = para[4]; + request_pose.x() = para[5]; + request_pose.y() = para[6]; + request_pose.z() = para[7]; + + PoseInfo info; + info.time = para[0]; + info.pose = Matrix4d::Identity(); + info.pose.block<3,3>(0,0) = request_pose.toRotationMatrix(); + info.pose(0,3) = para[1]; + info.pose(1,3) = para[2]; + info.pose(2,3) = para[3]; + gt_pose_vect.push_back(info); + } + } + printf("we have %d poses.\n", count); + return true; +} + +vector pts_3; +vector pts_2; +void imageBackFunc(int event, int x, int y, int flags, void* userdata) +{ + if( event == EVENT_LBUTTONDOWN ) + { + cout << "image is clicked - position (" << x << ", " << y << ")" << endl; + pts_2.push_back(cv::Point2f(x,y)); + } +} +void depthBackFunc(int event, int x, int y, int flags, void* userdata) +{ + if( event == EVENT_LBUTTONDOWN ) + { + cout << "depth is clicked - position (" << x << ", " << y << ")" << endl; + double depth = depth_mat.at(y,x); + double space_x = (x - cx) * depth / fx; + double space_y = (y - cy) * depth / fy; + double space_z = depth; + pts_3.push_back(cv::Point3f(space_x, space_y, space_z)); + } +} + +void solve_pnp() +{ +// translation : +// 0.994976 -0.0431638 0.0903361 -0.0338185 +// 0.0444475 0.998937 -0.012246 0.0541652 +// -0.0897114 0.0161996 0.995836 0.0384018 +// 0 0 0 1 + printf("we have %d pair points.\n", pts_3.size()); + if(pts_3.size() < 5 || pts_2.size() < 5) + { + return; + } + if(pts_3.size() != pts_2.size()) + { + printf("error, not equal!\n"); + return; + } + + cv::Mat r, rvec, t; + cv::solvePnP(pts_3, pts_2, cv_K, cv::Mat::zeros(4,1,CV_32FC1), rvec, t); + cv::Rodrigues(rvec, r); + Matrix3d R_ref; + for(int i=0;i<3;i++) + for(int j=0;j<3;j++) + { + R_ref(i,j) = r.at(i, j); + } + Matrix4d pnp_result = Matrix4d::Identity(); + pnp_result.block<3,3>(0,0) = R_ref; + pnp_result(0,3) = t.at(0, 0); + pnp_result(1,3) = t.at(1, 0); + pnp_result(2,3) = t.at(2, 0); + + vicon2leica = pnp_result.inverse(); + cout << "translation : " << endl << pnp_result << endl; +} + +void image_pose_callback( + const sensor_msgs::ImageConstPtr &image_input, + const geometry_msgs::TransformStampedConstPtr &pose_input) +{ + //time diff + double time_diff = fabs(image_input->header.stamp.toSec() - pose_input->header.stamp.toSec()) * 1000.0; + printf("time diff is %lf ms.\n", time_diff); + + //pose + Matrix4d Pose_receive = Matrix4d::Identity(); + + //using vicon + // Eigen::Vector3d request_position; + // Eigen::Quaterniond request_pose; + // request_position.x() = pose_input->transform.translation.x; + // request_position.y() = pose_input->transform.translation.y; + // request_position.z() = pose_input->transform.translation.z; + // request_pose.x() = pose_input->transform.rotation.x; + // request_pose.y() = pose_input->transform.rotation.y; + // request_pose.z() = pose_input->transform.rotation.z; + // request_pose.w() = pose_input->transform.rotation.w; + // Pose_receive.block<3,3>(0,0) = request_pose.toRotationMatrix(); + // Pose_receive(0,3) = request_position(0); + // Pose_receive(1,3) = request_position(1); + // Pose_receive(2,3) = request_position(2); + + //using ground truth + double image_time = image_input->header.stamp.toSec(); + double min_time_diff = 999.9; + int min_time_index = 0; + for(int i = 1; i < gt_pose_vect.size(); i++) + { + double time_diff = fabs(image_time - gt_pose_vect[i].time); + if(time_diff < min_time_diff) + { + min_time_diff = time_diff; + min_time_index = i; + } + } + printf("min time diff index %d, with diff time %lf ms.\n", min_time_index, min_time_diff*1000.0f); + Pose_receive = gt_pose_vect[min_time_index].pose; + + //convert to body pose + // Matrix4d body_pose = Pose_receive * vicon2body.inverse(); + Matrix4d body_pose = Pose_receive; + + //convert to cam pose + cam2world = body_pose * cam02body * vicon2leica; + + receive_stamp = pose_input->header.stamp; + + //image + cv_bridge::CvImageConstPtr cv_img_ptr = cv_bridge::toCvShare(image_input, sensor_msgs::image_encodings::MONO8); + cv::Mat img_8uC1 = cv_img_ptr->image; + undistorted_image.create(height, width, CV_8UC1); + if(is_distorted) + { + cv::remap(img_8uC1, undistorted_image, undist_map1, undist_map2, CV_INTER_LINEAR); + } + else + undistorted_image = img_8uC1; + + render_currentpose(); +} + +void render_currentpose() +{ + solve_pnp(); + + double this_time = ros::Time::now().toSec(); + + Matrix4d cam_pose = cam2world.inverse(); + + depthrender.render_pose(cam_pose, depth_hostptr); + + depth_mat = cv::Mat::zeros(height, width, CV_32FC1); + double min = 0.5; + double max = 1.0f; + for(int i = 0; i < height; i++) + for(int j = 0; j < width; j++) + { + float depth = (float)depth_hostptr[i * width + j] / 1000.0f; + depth = depth < 500.0f ? depth : 0; + max = depth > max ? depth : max; + depth_mat.at(i,j) = depth; + } + ROS_INFO("render cost %lf ms.", (ros::Time::now().toSec() - this_time) * 1000.0f); + printf("max_depth %lf.\n", max); + + cv_bridge::CvImage out_msg; + out_msg.header.stamp = receive_stamp; + out_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + out_msg.image = depth_mat.clone(); + pub_depth.publish(out_msg.toImageMsg()); + + cv::Mat adjMap; + depth_mat.convertTo(adjMap,CV_8UC1, 255 / (max-min), -min); + cv::Mat falseColorsMap; + cv::applyColorMap(adjMap, falseColorsMap, cv::COLORMAP_RAINBOW); + cv::Mat bgr_image; + cv::cvtColor(undistorted_image, bgr_image, cv::COLOR_GRAY2BGR); + cv::addWeighted(bgr_image, 0.2, falseColorsMap, 0.8, 0.0, falseColorsMap); + cv_bridge::CvImage cv_image_colored; + cv_image_colored.header.frame_id = "depthmap"; + cv_image_colored.encoding = sensor_msgs::image_encodings::BGR8; + cv_image_colored.image = falseColorsMap; + pub_color.publish(cv_image_colored.toImageMsg()); + + cv::imshow("bluefox_image", bgr_image); + cv::imshow("depth_image", adjMap); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "cloud_banchmark"); + ros::NodeHandle nh("~"); + + nh.getParam("cam_width", width); + nh.getParam("cam_height", height); + nh.getParam("cam_fx", fx); + nh.getParam("cam_fy", fy); + nh.getParam("cam_cx", cx); + nh.getParam("cam_cy", cy); + + depthrender.set_para(fx, fy, cx, cy, width, height); + + cv_K = (cv::Mat_(3, 3) << fx, 0.0f, cx, 0.0f, fy, cy, 0.0f, 0.0f, 1.0f); + if(nh.hasParam("cam_k1") && + nh.hasParam("cam_k2") && + nh.hasParam("cam_r1") && + nh.hasParam("cam_r2") ) + { + float k1, k2, r1, r2; + nh.getParam("cam_k1", k1); + nh.getParam("cam_k2", k2); + nh.getParam("cam_r1", r1); + nh.getParam("cam_r2", r2); + cv_D = (cv::Mat_(1, 4) << k1, k2, r1, r2); + cv::initUndistortRectifyMap( + cv_K, + cv_D, + cv::Mat_::eye(3,3), + cv_K, + cv::Size(width, height), + CV_16SC2, + undist_map1, undist_map2); + is_distorted = true; + } + if(is_distorted) + printf("need to rectify.\n"); + else + printf("do not need to rectify.\n"); + + vicon2body << 0.33638, -0.01749, 0.94156, 0.06901, + -0.02078, -0.99972, -0.01114, -0.02781, + 0.94150, -0.01582, -0.33665, -0.12395, + 0.0, 0.0, 0.0, 1.0; + cam02body << 0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, + 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, + -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, + 0.0, 0.0, 0.0, 1.0; + cam2world = Matrix4d::Identity(); + + string cloud_path; + nh.getParam("cloud_path", cloud_path); + printf("cloud file %s\n", cloud_path.c_str()); + std::fstream data_file; + data_file.open(cloud_path.c_str(), ios::in); + vector cloud_data; + double x, y, z, i, r, g, b; + while(data_file >> x >> y >> z >> i >> r >> g >> b) + { + cloud_data.push_back(x); + cloud_data.push_back(y); + cloud_data.push_back(z); + } + data_file.close(); + printf("has points %d.\n", cloud_data.size() / 3 ); + + + string groundtruth_path = string("/home/denny/Downloads/wkx_bag/data.txt"); + std::fstream gt_file; + gt_file.open(groundtruth_path.c_str(), ios::in); + read_pose(gt_file); + gt_file.close(); + + //pass cloud_data to depth render + depthrender.set_data(cloud_data); + depth_hostptr = (int*) malloc(width * height * sizeof(int)); + + message_filters::Subscriber image_sub(nh, "/cam0/image_raw", 30); + message_filters::Subscriber pose_sub(nh, "/vicon/firefly_sbx/firefly_sbx", 30); + message_filters::Synchronizer sync2(approx_policy(100), image_sub, pose_sub); + sync2.registerCallback(boost::bind(image_pose_callback, _1, _2)); + + //publisher depth image and color image + pub_depth = nh.advertise("depth",1000); + pub_color = nh.advertise("colordepth",1000); + // pub_posedimage = nh.advertise("posedimage",1000); + + undistorted_image.create(height, width, CV_8UC1); + + cv::namedWindow("bluefox_image",1); + cv::namedWindow("depth_image",1); + setMouseCallback("bluefox_image", imageBackFunc, NULL); + setMouseCallback("depth_image", depthBackFunc, NULL); + vicon2leica = Matrix4d::Identity(); + + while(ros::ok()) + { + ros::spinOnce(); + cv::waitKey(30); + } +} \ No newline at end of file diff --git a/uav_simulator/local_sensing/src/helper_math.h b/uav_simulator/local_sensing/src/helper_math.h new file mode 100644 index 000000000..c9c07c3e7 --- /dev/null +++ b/uav_simulator/local_sensing/src/helper_math.h @@ -0,0 +1,1453 @@ +/** + * Copyright 1993-2013 NVIDIA Corporation. All rights reserved. + * + * Please refer to the NVIDIA end user license agreement (EULA) associated + * with this source code for terms and conditions that govern your use of + * this software. Any use, reproduction, disclosure, or distribution of + * this software and related documentation outside the terms of the EULA + * is strictly prohibited. + * + */ + +/* + * This file implements common mathematical operations on vector types + * (float3, float4 etc.) since these are not provided as standard by CUDA. + * + * The syntax is modeled on the Cg standard library. + * + * This is part of the Helper library includes + * + * Thanks to Linh Hah for additions and fixes. + */ + +#ifndef HELPER_MATH_H +#define HELPER_MATH_H + +#include "cuda_runtime.h" + +typedef unsigned int uint; +typedef unsigned short ushort; + +#ifndef EXIT_WAIVED +#define EXIT_WAIVED 2 +#endif + +#ifndef __CUDACC__ +#include + +//////////////////////////////////////////////////////////////////////////////// +// host implementations of CUDA functions +//////////////////////////////////////////////////////////////////////////////// + +inline float fminf(float a, float b) +{ + return a < b ? a : b; +} + +inline float fmaxf(float a, float b) +{ + return a > b ? a : b; +} + +inline int max(int a, int b) +{ + return a > b ? a : b; +} + +inline int min(int a, int b) +{ + return a < b ? a : b; +} + +inline float rsqrtf(float x) +{ + return 1.0f / sqrtf(x); +} +#endif + +//////////////////////////////////////////////////////////////////////////////// +// constructors +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 make_float2(float s) +{ + return make_float2(s, s); +} +inline __host__ __device__ float2 make_float2(float3 a) +{ + return make_float2(a.x, a.y); +} +inline __host__ __device__ float2 make_float2(int2 a) +{ + return make_float2(float(a.x), float(a.y)); +} +inline __host__ __device__ float2 make_float2(uint2 a) +{ + return make_float2(float(a.x), float(a.y)); +} + +inline __host__ __device__ int2 make_int2(int s) +{ + return make_int2(s, s); +} +inline __host__ __device__ int2 make_int2(int3 a) +{ + return make_int2(a.x, a.y); +} +inline __host__ __device__ int2 make_int2(uint2 a) +{ + return make_int2(int(a.x), int(a.y)); +} +inline __host__ __device__ int2 make_int2(float2 a) +{ + return make_int2(int(a.x), int(a.y)); +} + +inline __host__ __device__ uint2 make_uint2(uint s) +{ + return make_uint2(s, s); +} +inline __host__ __device__ uint2 make_uint2(uint3 a) +{ + return make_uint2(a.x, a.y); +} +inline __host__ __device__ uint2 make_uint2(int2 a) +{ + return make_uint2(uint(a.x), uint(a.y)); +} + +inline __host__ __device__ float3 make_float3(float s) +{ + return make_float3(s, s, s); +} +inline __host__ __device__ float3 make_float3(float2 a) +{ + return make_float3(a.x, a.y, 0.0f); +} +inline __host__ __device__ float3 make_float3(float2 a, float s) +{ + return make_float3(a.x, a.y, s); +} +inline __host__ __device__ float3 make_float3(float4 a) +{ + return make_float3(a.x, a.y, a.z); +} +inline __host__ __device__ float3 make_float3(int3 a) +{ + return make_float3(float(a.x), float(a.y), float(a.z)); +} +inline __host__ __device__ float3 make_float3(uint3 a) +{ + return make_float3(float(a.x), float(a.y), float(a.z)); +} + +inline __host__ __device__ int3 make_int3(int s) +{ + return make_int3(s, s, s); +} +inline __host__ __device__ int3 make_int3(int2 a) +{ + return make_int3(a.x, a.y, 0); +} +inline __host__ __device__ int3 make_int3(int2 a, int s) +{ + return make_int3(a.x, a.y, s); +} +inline __host__ __device__ int3 make_int3(uint3 a) +{ + return make_int3(int(a.x), int(a.y), int(a.z)); +} +inline __host__ __device__ int3 make_int3(float3 a) +{ + return make_int3(int(a.x), int(a.y), int(a.z)); +} + +inline __host__ __device__ uint3 make_uint3(uint s) +{ + return make_uint3(s, s, s); +} +inline __host__ __device__ uint3 make_uint3(uint2 a) +{ + return make_uint3(a.x, a.y, 0); +} +inline __host__ __device__ uint3 make_uint3(uint2 a, uint s) +{ + return make_uint3(a.x, a.y, s); +} +inline __host__ __device__ uint3 make_uint3(uint4 a) +{ + return make_uint3(a.x, a.y, a.z); +} +inline __host__ __device__ uint3 make_uint3(int3 a) +{ + return make_uint3(uint(a.x), uint(a.y), uint(a.z)); +} + +inline __host__ __device__ float4 make_float4(float s) +{ + return make_float4(s, s, s, s); +} +inline __host__ __device__ float4 make_float4(float3 a) +{ + return make_float4(a.x, a.y, a.z, 0.0f); +} +inline __host__ __device__ float4 make_float4(float3 a, float w) +{ + return make_float4(a.x, a.y, a.z, w); +} +inline __host__ __device__ float4 make_float4(int4 a) +{ + return make_float4(float(a.x), float(a.y), float(a.z), float(a.w)); +} +inline __host__ __device__ float4 make_float4(uint4 a) +{ + return make_float4(float(a.x), float(a.y), float(a.z), float(a.w)); +} + +inline __host__ __device__ int4 make_int4(int s) +{ + return make_int4(s, s, s, s); +} +inline __host__ __device__ int4 make_int4(int3 a) +{ + return make_int4(a.x, a.y, a.z, 0); +} +inline __host__ __device__ int4 make_int4(int3 a, int w) +{ + return make_int4(a.x, a.y, a.z, w); +} +inline __host__ __device__ int4 make_int4(uint4 a) +{ + return make_int4(int(a.x), int(a.y), int(a.z), int(a.w)); +} +inline __host__ __device__ int4 make_int4(float4 a) +{ + return make_int4(int(a.x), int(a.y), int(a.z), int(a.w)); +} + + +inline __host__ __device__ uint4 make_uint4(uint s) +{ + return make_uint4(s, s, s, s); +} +inline __host__ __device__ uint4 make_uint4(uint3 a) +{ + return make_uint4(a.x, a.y, a.z, 0); +} +inline __host__ __device__ uint4 make_uint4(uint3 a, uint w) +{ + return make_uint4(a.x, a.y, a.z, w); +} +inline __host__ __device__ uint4 make_uint4(int4 a) +{ + return make_uint4(uint(a.x), uint(a.y), uint(a.z), uint(a.w)); +} + +//////////////////////////////////////////////////////////////////////////////// +// negate +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 operator-(float2 &a) +{ + return make_float2(-a.x, -a.y); +} +inline __host__ __device__ int2 operator-(int2 &a) +{ + return make_int2(-a.x, -a.y); +} +inline __host__ __device__ float3 operator-(float3 &a) +{ + return make_float3(-a.x, -a.y, -a.z); +} +inline __host__ __device__ int3 operator-(int3 &a) +{ + return make_int3(-a.x, -a.y, -a.z); +} +inline __host__ __device__ float4 operator-(float4 &a) +{ + return make_float4(-a.x, -a.y, -a.z, -a.w); +} +inline __host__ __device__ int4 operator-(int4 &a) +{ + return make_int4(-a.x, -a.y, -a.z, -a.w); +} + +//////////////////////////////////////////////////////////////////////////////// +// addition +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 operator+(float2 a, float2 b) +{ + return make_float2(a.x + b.x, a.y + b.y); +} +inline __host__ __device__ void operator+=(float2 &a, float2 b) +{ + a.x += b.x; + a.y += b.y; +} +inline __host__ __device__ float2 operator+(float2 a, float b) +{ + return make_float2(a.x + b, a.y + b); +} +inline __host__ __device__ float2 operator+(float b, float2 a) +{ + return make_float2(a.x + b, a.y + b); +} +inline __host__ __device__ void operator+=(float2 &a, float b) +{ + a.x += b; + a.y += b; +} + +inline __host__ __device__ int2 operator+(int2 a, int2 b) +{ + return make_int2(a.x + b.x, a.y + b.y); +} +inline __host__ __device__ void operator+=(int2 &a, int2 b) +{ + a.x += b.x; + a.y += b.y; +} +inline __host__ __device__ int2 operator+(int2 a, int b) +{ + return make_int2(a.x + b, a.y + b); +} +inline __host__ __device__ int2 operator+(int b, int2 a) +{ + return make_int2(a.x + b, a.y + b); +} +inline __host__ __device__ void operator+=(int2 &a, int b) +{ + a.x += b; + a.y += b; +} + +inline __host__ __device__ uint2 operator+(uint2 a, uint2 b) +{ + return make_uint2(a.x + b.x, a.y + b.y); +} +inline __host__ __device__ void operator+=(uint2 &a, uint2 b) +{ + a.x += b.x; + a.y += b.y; +} +inline __host__ __device__ uint2 operator+(uint2 a, uint b) +{ + return make_uint2(a.x + b, a.y + b); +} +inline __host__ __device__ uint2 operator+(uint b, uint2 a) +{ + return make_uint2(a.x + b, a.y + b); +} +inline __host__ __device__ void operator+=(uint2 &a, uint b) +{ + a.x += b; + a.y += b; +} + + +inline __host__ __device__ float3 operator+(float3 a, float3 b) +{ + return make_float3(a.x + b.x, a.y + b.y, a.z + b.z); +} +inline __host__ __device__ void operator+=(float3 &a, float3 b) +{ + a.x += b.x; + a.y += b.y; + a.z += b.z; +} +inline __host__ __device__ float3 operator+(float3 a, float b) +{ + return make_float3(a.x + b, a.y + b, a.z + b); +} +inline __host__ __device__ void operator+=(float3 &a, float b) +{ + a.x += b; + a.y += b; + a.z += b; +} + +inline __host__ __device__ int3 operator+(int3 a, int3 b) +{ + return make_int3(a.x + b.x, a.y + b.y, a.z + b.z); +} +inline __host__ __device__ void operator+=(int3 &a, int3 b) +{ + a.x += b.x; + a.y += b.y; + a.z += b.z; +} +inline __host__ __device__ int3 operator+(int3 a, int b) +{ + return make_int3(a.x + b, a.y + b, a.z + b); +} +inline __host__ __device__ void operator+=(int3 &a, int b) +{ + a.x += b; + a.y += b; + a.z += b; +} + +inline __host__ __device__ uint3 operator+(uint3 a, uint3 b) +{ + return make_uint3(a.x + b.x, a.y + b.y, a.z + b.z); +} +inline __host__ __device__ void operator+=(uint3 &a, uint3 b) +{ + a.x += b.x; + a.y += b.y; + a.z += b.z; +} +inline __host__ __device__ uint3 operator+(uint3 a, uint b) +{ + return make_uint3(a.x + b, a.y + b, a.z + b); +} +inline __host__ __device__ void operator+=(uint3 &a, uint b) +{ + a.x += b; + a.y += b; + a.z += b; +} + +inline __host__ __device__ int3 operator+(int b, int3 a) +{ + return make_int3(a.x + b, a.y + b, a.z + b); +} +inline __host__ __device__ uint3 operator+(uint b, uint3 a) +{ + return make_uint3(a.x + b, a.y + b, a.z + b); +} +inline __host__ __device__ float3 operator+(float b, float3 a) +{ + return make_float3(a.x + b, a.y + b, a.z + b); +} + +inline __host__ __device__ float4 operator+(float4 a, float4 b) +{ + return make_float4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); +} +inline __host__ __device__ void operator+=(float4 &a, float4 b) +{ + a.x += b.x; + a.y += b.y; + a.z += b.z; + a.w += b.w; +} +inline __host__ __device__ float4 operator+(float4 a, float b) +{ + return make_float4(a.x + b, a.y + b, a.z + b, a.w + b); +} +inline __host__ __device__ float4 operator+(float b, float4 a) +{ + return make_float4(a.x + b, a.y + b, a.z + b, a.w + b); +} +inline __host__ __device__ void operator+=(float4 &a, float b) +{ + a.x += b; + a.y += b; + a.z += b; + a.w += b; +} + +inline __host__ __device__ int4 operator+(int4 a, int4 b) +{ + return make_int4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); +} +inline __host__ __device__ void operator+=(int4 &a, int4 b) +{ + a.x += b.x; + a.y += b.y; + a.z += b.z; + a.w += b.w; +} +inline __host__ __device__ int4 operator+(int4 a, int b) +{ + return make_int4(a.x + b, a.y + b, a.z + b, a.w + b); +} +inline __host__ __device__ int4 operator+(int b, int4 a) +{ + return make_int4(a.x + b, a.y + b, a.z + b, a.w + b); +} +inline __host__ __device__ void operator+=(int4 &a, int b) +{ + a.x += b; + a.y += b; + a.z += b; + a.w += b; +} + +inline __host__ __device__ uint4 operator+(uint4 a, uint4 b) +{ + return make_uint4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); +} +inline __host__ __device__ void operator+=(uint4 &a, uint4 b) +{ + a.x += b.x; + a.y += b.y; + a.z += b.z; + a.w += b.w; +} +inline __host__ __device__ uint4 operator+(uint4 a, uint b) +{ + return make_uint4(a.x + b, a.y + b, a.z + b, a.w + b); +} +inline __host__ __device__ uint4 operator+(uint b, uint4 a) +{ + return make_uint4(a.x + b, a.y + b, a.z + b, a.w + b); +} +inline __host__ __device__ void operator+=(uint4 &a, uint b) +{ + a.x += b; + a.y += b; + a.z += b; + a.w += b; +} + +//////////////////////////////////////////////////////////////////////////////// +// subtract +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 operator-(float2 a, float2 b) +{ + return make_float2(a.x - b.x, a.y - b.y); +} +inline __host__ __device__ void operator-=(float2 &a, float2 b) +{ + a.x -= b.x; + a.y -= b.y; +} +inline __host__ __device__ float2 operator-(float2 a, float b) +{ + return make_float2(a.x - b, a.y - b); +} +inline __host__ __device__ float2 operator-(float b, float2 a) +{ + return make_float2(b - a.x, b - a.y); +} +inline __host__ __device__ void operator-=(float2 &a, float b) +{ + a.x -= b; + a.y -= b; +} + +inline __host__ __device__ int2 operator-(int2 a, int2 b) +{ + return make_int2(a.x - b.x, a.y - b.y); +} +inline __host__ __device__ void operator-=(int2 &a, int2 b) +{ + a.x -= b.x; + a.y -= b.y; +} +inline __host__ __device__ int2 operator-(int2 a, int b) +{ + return make_int2(a.x - b, a.y - b); +} +inline __host__ __device__ int2 operator-(int b, int2 a) +{ + return make_int2(b - a.x, b - a.y); +} +inline __host__ __device__ void operator-=(int2 &a, int b) +{ + a.x -= b; + a.y -= b; +} + +inline __host__ __device__ uint2 operator-(uint2 a, uint2 b) +{ + return make_uint2(a.x - b.x, a.y - b.y); +} +inline __host__ __device__ void operator-=(uint2 &a, uint2 b) +{ + a.x -= b.x; + a.y -= b.y; +} +inline __host__ __device__ uint2 operator-(uint2 a, uint b) +{ + return make_uint2(a.x - b, a.y - b); +} +inline __host__ __device__ uint2 operator-(uint b, uint2 a) +{ + return make_uint2(b - a.x, b - a.y); +} +inline __host__ __device__ void operator-=(uint2 &a, uint b) +{ + a.x -= b; + a.y -= b; +} + +inline __host__ __device__ float3 operator-(float3 a, float3 b) +{ + return make_float3(a.x - b.x, a.y - b.y, a.z - b.z); +} +inline __host__ __device__ void operator-=(float3 &a, float3 b) +{ + a.x -= b.x; + a.y -= b.y; + a.z -= b.z; +} +inline __host__ __device__ float3 operator-(float3 a, float b) +{ + return make_float3(a.x - b, a.y - b, a.z - b); +} +inline __host__ __device__ float3 operator-(float b, float3 a) +{ + return make_float3(b - a.x, b - a.y, b - a.z); +} +inline __host__ __device__ void operator-=(float3 &a, float b) +{ + a.x -= b; + a.y -= b; + a.z -= b; +} + +inline __host__ __device__ int3 operator-(int3 a, int3 b) +{ + return make_int3(a.x - b.x, a.y - b.y, a.z - b.z); +} +inline __host__ __device__ void operator-=(int3 &a, int3 b) +{ + a.x -= b.x; + a.y -= b.y; + a.z -= b.z; +} +inline __host__ __device__ int3 operator-(int3 a, int b) +{ + return make_int3(a.x - b, a.y - b, a.z - b); +} +inline __host__ __device__ int3 operator-(int b, int3 a) +{ + return make_int3(b - a.x, b - a.y, b - a.z); +} +inline __host__ __device__ void operator-=(int3 &a, int b) +{ + a.x -= b; + a.y -= b; + a.z -= b; +} + +inline __host__ __device__ uint3 operator-(uint3 a, uint3 b) +{ + return make_uint3(a.x - b.x, a.y - b.y, a.z - b.z); +} +inline __host__ __device__ void operator-=(uint3 &a, uint3 b) +{ + a.x -= b.x; + a.y -= b.y; + a.z -= b.z; +} +inline __host__ __device__ uint3 operator-(uint3 a, uint b) +{ + return make_uint3(a.x - b, a.y - b, a.z - b); +} +inline __host__ __device__ uint3 operator-(uint b, uint3 a) +{ + return make_uint3(b - a.x, b - a.y, b - a.z); +} +inline __host__ __device__ void operator-=(uint3 &a, uint b) +{ + a.x -= b; + a.y -= b; + a.z -= b; +} + +inline __host__ __device__ float4 operator-(float4 a, float4 b) +{ + return make_float4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w); +} +inline __host__ __device__ void operator-=(float4 &a, float4 b) +{ + a.x -= b.x; + a.y -= b.y; + a.z -= b.z; + a.w -= b.w; +} +inline __host__ __device__ float4 operator-(float4 a, float b) +{ + return make_float4(a.x - b, a.y - b, a.z - b, a.w - b); +} +inline __host__ __device__ void operator-=(float4 &a, float b) +{ + a.x -= b; + a.y -= b; + a.z -= b; + a.w -= b; +} + +inline __host__ __device__ int4 operator-(int4 a, int4 b) +{ + return make_int4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w); +} +inline __host__ __device__ void operator-=(int4 &a, int4 b) +{ + a.x -= b.x; + a.y -= b.y; + a.z -= b.z; + a.w -= b.w; +} +inline __host__ __device__ int4 operator-(int4 a, int b) +{ + return make_int4(a.x - b, a.y - b, a.z - b, a.w - b); +} +inline __host__ __device__ int4 operator-(int b, int4 a) +{ + return make_int4(b - a.x, b - a.y, b - a.z, b - a.w); +} +inline __host__ __device__ void operator-=(int4 &a, int b) +{ + a.x -= b; + a.y -= b; + a.z -= b; + a.w -= b; +} + +inline __host__ __device__ uint4 operator-(uint4 a, uint4 b) +{ + return make_uint4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w); +} +inline __host__ __device__ void operator-=(uint4 &a, uint4 b) +{ + a.x -= b.x; + a.y -= b.y; + a.z -= b.z; + a.w -= b.w; +} +inline __host__ __device__ uint4 operator-(uint4 a, uint b) +{ + return make_uint4(a.x - b, a.y - b, a.z - b, a.w - b); +} +inline __host__ __device__ uint4 operator-(uint b, uint4 a) +{ + return make_uint4(b - a.x, b - a.y, b - a.z, b - a.w); +} +inline __host__ __device__ void operator-=(uint4 &a, uint b) +{ + a.x -= b; + a.y -= b; + a.z -= b; + a.w -= b; +} + +//////////////////////////////////////////////////////////////////////////////// +// multiply +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 operator*(float2 a, float2 b) +{ + return make_float2(a.x * b.x, a.y * b.y); +} +inline __host__ __device__ void operator*=(float2 &a, float2 b) +{ + a.x *= b.x; + a.y *= b.y; +} +inline __host__ __device__ float2 operator*(float2 a, float b) +{ + return make_float2(a.x * b, a.y * b); +} +inline __host__ __device__ float2 operator*(float b, float2 a) +{ + return make_float2(b * a.x, b * a.y); +} +inline __host__ __device__ void operator*=(float2 &a, float b) +{ + a.x *= b; + a.y *= b; +} + +inline __host__ __device__ int2 operator*(int2 a, int2 b) +{ + return make_int2(a.x * b.x, a.y * b.y); +} +inline __host__ __device__ void operator*=(int2 &a, int2 b) +{ + a.x *= b.x; + a.y *= b.y; +} +inline __host__ __device__ int2 operator*(int2 a, int b) +{ + return make_int2(a.x * b, a.y * b); +} +inline __host__ __device__ int2 operator*(int b, int2 a) +{ + return make_int2(b * a.x, b * a.y); +} +inline __host__ __device__ void operator*=(int2 &a, int b) +{ + a.x *= b; + a.y *= b; +} + +inline __host__ __device__ uint2 operator*(uint2 a, uint2 b) +{ + return make_uint2(a.x * b.x, a.y * b.y); +} +inline __host__ __device__ void operator*=(uint2 &a, uint2 b) +{ + a.x *= b.x; + a.y *= b.y; +} +inline __host__ __device__ uint2 operator*(uint2 a, uint b) +{ + return make_uint2(a.x * b, a.y * b); +} +inline __host__ __device__ uint2 operator*(uint b, uint2 a) +{ + return make_uint2(b * a.x, b * a.y); +} +inline __host__ __device__ void operator*=(uint2 &a, uint b) +{ + a.x *= b; + a.y *= b; +} + +inline __host__ __device__ float3 operator*(float3 a, float3 b) +{ + return make_float3(a.x * b.x, a.y * b.y, a.z * b.z); +} +inline __host__ __device__ void operator*=(float3 &a, float3 b) +{ + a.x *= b.x; + a.y *= b.y; + a.z *= b.z; +} +inline __host__ __device__ float3 operator*(float3 a, float b) +{ + return make_float3(a.x * b, a.y * b, a.z * b); +} +inline __host__ __device__ float3 operator*(float b, float3 a) +{ + return make_float3(b * a.x, b * a.y, b * a.z); +} +inline __host__ __device__ void operator*=(float3 &a, float b) +{ + a.x *= b; + a.y *= b; + a.z *= b; +} + +inline __host__ __device__ int3 operator*(int3 a, int3 b) +{ + return make_int3(a.x * b.x, a.y * b.y, a.z * b.z); +} +inline __host__ __device__ void operator*=(int3 &a, int3 b) +{ + a.x *= b.x; + a.y *= b.y; + a.z *= b.z; +} +inline __host__ __device__ int3 operator*(int3 a, int b) +{ + return make_int3(a.x * b, a.y * b, a.z * b); +} +inline __host__ __device__ int3 operator*(int b, int3 a) +{ + return make_int3(b * a.x, b * a.y, b * a.z); +} +inline __host__ __device__ void operator*=(int3 &a, int b) +{ + a.x *= b; + a.y *= b; + a.z *= b; +} + +inline __host__ __device__ uint3 operator*(uint3 a, uint3 b) +{ + return make_uint3(a.x * b.x, a.y * b.y, a.z * b.z); +} +inline __host__ __device__ void operator*=(uint3 &a, uint3 b) +{ + a.x *= b.x; + a.y *= b.y; + a.z *= b.z; +} +inline __host__ __device__ uint3 operator*(uint3 a, uint b) +{ + return make_uint3(a.x * b, a.y * b, a.z * b); +} +inline __host__ __device__ uint3 operator*(uint b, uint3 a) +{ + return make_uint3(b * a.x, b * a.y, b * a.z); +} +inline __host__ __device__ void operator*=(uint3 &a, uint b) +{ + a.x *= b; + a.y *= b; + a.z *= b; +} + +inline __host__ __device__ float4 operator*(float4 a, float4 b) +{ + return make_float4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w); +} +inline __host__ __device__ void operator*=(float4 &a, float4 b) +{ + a.x *= b.x; + a.y *= b.y; + a.z *= b.z; + a.w *= b.w; +} +inline __host__ __device__ float4 operator*(float4 a, float b) +{ + return make_float4(a.x * b, a.y * b, a.z * b, a.w * b); +} +inline __host__ __device__ float4 operator*(float b, float4 a) +{ + return make_float4(b * a.x, b * a.y, b * a.z, b * a.w); +} +inline __host__ __device__ void operator*=(float4 &a, float b) +{ + a.x *= b; + a.y *= b; + a.z *= b; + a.w *= b; +} + +inline __host__ __device__ int4 operator*(int4 a, int4 b) +{ + return make_int4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w); +} +inline __host__ __device__ void operator*=(int4 &a, int4 b) +{ + a.x *= b.x; + a.y *= b.y; + a.z *= b.z; + a.w *= b.w; +} +inline __host__ __device__ int4 operator*(int4 a, int b) +{ + return make_int4(a.x * b, a.y * b, a.z * b, a.w * b); +} +inline __host__ __device__ int4 operator*(int b, int4 a) +{ + return make_int4(b * a.x, b * a.y, b * a.z, b * a.w); +} +inline __host__ __device__ void operator*=(int4 &a, int b) +{ + a.x *= b; + a.y *= b; + a.z *= b; + a.w *= b; +} + +inline __host__ __device__ uint4 operator*(uint4 a, uint4 b) +{ + return make_uint4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w); +} +inline __host__ __device__ void operator*=(uint4 &a, uint4 b) +{ + a.x *= b.x; + a.y *= b.y; + a.z *= b.z; + a.w *= b.w; +} +inline __host__ __device__ uint4 operator*(uint4 a, uint b) +{ + return make_uint4(a.x * b, a.y * b, a.z * b, a.w * b); +} +inline __host__ __device__ uint4 operator*(uint b, uint4 a) +{ + return make_uint4(b * a.x, b * a.y, b * a.z, b * a.w); +} +inline __host__ __device__ void operator*=(uint4 &a, uint b) +{ + a.x *= b; + a.y *= b; + a.z *= b; + a.w *= b; +} + +//////////////////////////////////////////////////////////////////////////////// +// divide +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 operator/(float2 a, float2 b) +{ + return make_float2(a.x / b.x, a.y / b.y); +} +inline __host__ __device__ void operator/=(float2 &a, float2 b) +{ + a.x /= b.x; + a.y /= b.y; +} +inline __host__ __device__ float2 operator/(float2 a, float b) +{ + return make_float2(a.x / b, a.y / b); +} +inline __host__ __device__ void operator/=(float2 &a, float b) +{ + a.x /= b; + a.y /= b; +} +inline __host__ __device__ float2 operator/(float b, float2 a) +{ + return make_float2(b / a.x, b / a.y); +} + +inline __host__ __device__ float3 operator/(float3 a, float3 b) +{ + return make_float3(a.x / b.x, a.y / b.y, a.z / b.z); +} +inline __host__ __device__ void operator/=(float3 &a, float3 b) +{ + a.x /= b.x; + a.y /= b.y; + a.z /= b.z; +} +inline __host__ __device__ float3 operator/(float3 a, float b) +{ + return make_float3(a.x / b, a.y / b, a.z / b); +} +inline __host__ __device__ void operator/=(float3 &a, float b) +{ + a.x /= b; + a.y /= b; + a.z /= b; +} +inline __host__ __device__ float3 operator/(float b, float3 a) +{ + return make_float3(b / a.x, b / a.y, b / a.z); +} + +inline __host__ __device__ float4 operator/(float4 a, float4 b) +{ + return make_float4(a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w); +} +inline __host__ __device__ void operator/=(float4 &a, float4 b) +{ + a.x /= b.x; + a.y /= b.y; + a.z /= b.z; + a.w /= b.w; +} +inline __host__ __device__ float4 operator/(float4 a, float b) +{ + return make_float4(a.x / b, a.y / b, a.z / b, a.w / b); +} +inline __host__ __device__ void operator/=(float4 &a, float b) +{ + a.x /= b; + a.y /= b; + a.z /= b; + a.w /= b; +} +inline __host__ __device__ float4 operator/(float b, float4 a) +{ + return make_float4(b / a.x, b / a.y, b / a.z, b / a.w); +} + +//////////////////////////////////////////////////////////////////////////////// +// min +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 fminf(float2 a, float2 b) +{ + return make_float2(fminf(a.x,b.x), fminf(a.y,b.y)); +} +inline __host__ __device__ float3 fminf(float3 a, float3 b) +{ + return make_float3(fminf(a.x,b.x), fminf(a.y,b.y), fminf(a.z,b.z)); +} +inline __host__ __device__ float4 fminf(float4 a, float4 b) +{ + return make_float4(fminf(a.x,b.x), fminf(a.y,b.y), fminf(a.z,b.z), fminf(a.w,b.w)); +} + +inline __host__ __device__ int2 min(int2 a, int2 b) +{ + return make_int2(min(a.x,b.x), min(a.y,b.y)); +} +inline __host__ __device__ int3 min(int3 a, int3 b) +{ + return make_int3(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z)); +} +inline __host__ __device__ int4 min(int4 a, int4 b) +{ + return make_int4(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z), min(a.w,b.w)); +} + +inline __host__ __device__ uint2 min(uint2 a, uint2 b) +{ + return make_uint2(min(a.x,b.x), min(a.y,b.y)); +} +inline __host__ __device__ uint3 min(uint3 a, uint3 b) +{ + return make_uint3(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z)); +} +inline __host__ __device__ uint4 min(uint4 a, uint4 b) +{ + return make_uint4(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z), min(a.w,b.w)); +} + +//////////////////////////////////////////////////////////////////////////////// +// max +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 fmaxf(float2 a, float2 b) +{ + return make_float2(fmaxf(a.x,b.x), fmaxf(a.y,b.y)); +} +inline __host__ __device__ float3 fmaxf(float3 a, float3 b) +{ + return make_float3(fmaxf(a.x,b.x), fmaxf(a.y,b.y), fmaxf(a.z,b.z)); +} +inline __host__ __device__ float4 fmaxf(float4 a, float4 b) +{ + return make_float4(fmaxf(a.x,b.x), fmaxf(a.y,b.y), fmaxf(a.z,b.z), fmaxf(a.w,b.w)); +} + +inline __host__ __device__ int2 max(int2 a, int2 b) +{ + return make_int2(max(a.x,b.x), max(a.y,b.y)); +} +inline __host__ __device__ int3 max(int3 a, int3 b) +{ + return make_int3(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z)); +} +inline __host__ __device__ int4 max(int4 a, int4 b) +{ + return make_int4(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z), max(a.w,b.w)); +} + +inline __host__ __device__ uint2 max(uint2 a, uint2 b) +{ + return make_uint2(max(a.x,b.x), max(a.y,b.y)); +} +inline __host__ __device__ uint3 max(uint3 a, uint3 b) +{ + return make_uint3(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z)); +} +inline __host__ __device__ uint4 max(uint4 a, uint4 b) +{ + return make_uint4(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z), max(a.w,b.w)); +} + +//////////////////////////////////////////////////////////////////////////////// +// lerp +// - linear interpolation between a and b, based on value t in [0, 1] range +//////////////////////////////////////////////////////////////////////////////// + +inline __device__ __host__ float lerp(float a, float b, float t) +{ + return a + t*(b-a); +} +inline __device__ __host__ float2 lerp(float2 a, float2 b, float t) +{ + return a + t*(b-a); +} +inline __device__ __host__ float3 lerp(float3 a, float3 b, float t) +{ + return a + t*(b-a); +} +inline __device__ __host__ float4 lerp(float4 a, float4 b, float t) +{ + return a + t*(b-a); +} + +//////////////////////////////////////////////////////////////////////////////// +// clamp +// - clamp the value v to be in the range [a, b] +//////////////////////////////////////////////////////////////////////////////// + +inline __device__ __host__ float clamp(float f, float a, float b) +{ + return fmaxf(a, fminf(f, b)); +} +inline __device__ __host__ int clamp(int f, int a, int b) +{ + return max(a, min(f, b)); +} +inline __device__ __host__ uint clamp(uint f, uint a, uint b) +{ + return max(a, min(f, b)); +} + +inline __device__ __host__ float2 clamp(float2 v, float a, float b) +{ + return make_float2(clamp(v.x, a, b), clamp(v.y, a, b)); +} +inline __device__ __host__ float2 clamp(float2 v, float2 a, float2 b) +{ + return make_float2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y)); +} +inline __device__ __host__ float3 clamp(float3 v, float a, float b) +{ + return make_float3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b)); +} +inline __device__ __host__ float3 clamp(float3 v, float3 a, float3 b) +{ + return make_float3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z)); +} +inline __device__ __host__ float4 clamp(float4 v, float a, float b) +{ + return make_float4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b)); +} +inline __device__ __host__ float4 clamp(float4 v, float4 a, float4 b) +{ + return make_float4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w)); +} + +inline __device__ __host__ int2 clamp(int2 v, int a, int b) +{ + return make_int2(clamp(v.x, a, b), clamp(v.y, a, b)); +} +inline __device__ __host__ int2 clamp(int2 v, int2 a, int2 b) +{ + return make_int2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y)); +} +inline __device__ __host__ int3 clamp(int3 v, int a, int b) +{ + return make_int3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b)); +} +inline __device__ __host__ int3 clamp(int3 v, int3 a, int3 b) +{ + return make_int3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z)); +} +inline __device__ __host__ int4 clamp(int4 v, int a, int b) +{ + return make_int4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b)); +} +inline __device__ __host__ int4 clamp(int4 v, int4 a, int4 b) +{ + return make_int4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w)); +} + +inline __device__ __host__ uint2 clamp(uint2 v, uint a, uint b) +{ + return make_uint2(clamp(v.x, a, b), clamp(v.y, a, b)); +} +inline __device__ __host__ uint2 clamp(uint2 v, uint2 a, uint2 b) +{ + return make_uint2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y)); +} +inline __device__ __host__ uint3 clamp(uint3 v, uint a, uint b) +{ + return make_uint3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b)); +} +inline __device__ __host__ uint3 clamp(uint3 v, uint3 a, uint3 b) +{ + return make_uint3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z)); +} +inline __device__ __host__ uint4 clamp(uint4 v, uint a, uint b) +{ + return make_uint4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b)); +} +inline __device__ __host__ uint4 clamp(uint4 v, uint4 a, uint4 b) +{ + return make_uint4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w)); +} + +//////////////////////////////////////////////////////////////////////////////// +// dot product +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float dot(float2 a, float2 b) +{ + return a.x * b.x + a.y * b.y; +} +inline __host__ __device__ float dot(float3 a, float3 b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z; +} +inline __host__ __device__ float dot(float4 a, float4 b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; +} + +inline __host__ __device__ int dot(int2 a, int2 b) +{ + return a.x * b.x + a.y * b.y; +} +inline __host__ __device__ int dot(int3 a, int3 b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z; +} +inline __host__ __device__ int dot(int4 a, int4 b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; +} + +inline __host__ __device__ uint dot(uint2 a, uint2 b) +{ + return a.x * b.x + a.y * b.y; +} +inline __host__ __device__ uint dot(uint3 a, uint3 b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z; +} +inline __host__ __device__ uint dot(uint4 a, uint4 b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; +} + +//////////////////////////////////////////////////////////////////////////////// +// length +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float length(float2 v) +{ + return sqrtf(dot(v, v)); +} +inline __host__ __device__ float length(float3 v) +{ + return sqrtf(dot(v, v)); +} +inline __host__ __device__ float length(float4 v) +{ + return sqrtf(dot(v, v)); +} + +//////////////////////////////////////////////////////////////////////////////// +// normalize +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 normalize(float2 v) +{ + float invLen = rsqrtf(dot(v, v)); + return v * invLen; +} +inline __host__ __device__ float3 normalize(float3 v) +{ + float invLen = rsqrtf(dot(v, v)); + return v * invLen; +} +inline __host__ __device__ float4 normalize(float4 v) +{ + float invLen = rsqrtf(dot(v, v)); + return v * invLen; +} + +//////////////////////////////////////////////////////////////////////////////// +// floor +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 floorf(float2 v) +{ + return make_float2(floorf(v.x), floorf(v.y)); +} +inline __host__ __device__ float3 floorf(float3 v) +{ + return make_float3(floorf(v.x), floorf(v.y), floorf(v.z)); +} +inline __host__ __device__ float4 floorf(float4 v) +{ + return make_float4(floorf(v.x), floorf(v.y), floorf(v.z), floorf(v.w)); +} + +//////////////////////////////////////////////////////////////////////////////// +// frac - returns the fractional portion of a scalar or each vector component +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float fracf(float v) +{ + return v - floorf(v); +} +inline __host__ __device__ float2 fracf(float2 v) +{ + return make_float2(fracf(v.x), fracf(v.y)); +} +inline __host__ __device__ float3 fracf(float3 v) +{ + return make_float3(fracf(v.x), fracf(v.y), fracf(v.z)); +} +inline __host__ __device__ float4 fracf(float4 v) +{ + return make_float4(fracf(v.x), fracf(v.y), fracf(v.z), fracf(v.w)); +} + +//////////////////////////////////////////////////////////////////////////////// +// fmod +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 fmodf(float2 a, float2 b) +{ + return make_float2(fmodf(a.x, b.x), fmodf(a.y, b.y)); +} +inline __host__ __device__ float3 fmodf(float3 a, float3 b) +{ + return make_float3(fmodf(a.x, b.x), fmodf(a.y, b.y), fmodf(a.z, b.z)); +} +inline __host__ __device__ float4 fmodf(float4 a, float4 b) +{ + return make_float4(fmodf(a.x, b.x), fmodf(a.y, b.y), fmodf(a.z, b.z), fmodf(a.w, b.w)); +} + +//////////////////////////////////////////////////////////////////////////////// +// absolute value +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 fabs(float2 v) +{ + return make_float2(fabs(v.x), fabs(v.y)); +} +inline __host__ __device__ float3 fabs(float3 v) +{ + return make_float3(fabs(v.x), fabs(v.y), fabs(v.z)); +} +inline __host__ __device__ float4 fabs(float4 v) +{ + return make_float4(fabs(v.x), fabs(v.y), fabs(v.z), fabs(v.w)); +} + +inline __host__ __device__ int2 abs(int2 v) +{ + return make_int2(abs(v.x), abs(v.y)); +} +inline __host__ __device__ int3 abs(int3 v) +{ + return make_int3(abs(v.x), abs(v.y), abs(v.z)); +} +inline __host__ __device__ int4 abs(int4 v) +{ + return make_int4(abs(v.x), abs(v.y), abs(v.z), abs(v.w)); +} + +//////////////////////////////////////////////////////////////////////////////// +// reflect +// - returns reflection of incident ray I around surface normal N +// - N should be normalized, reflected vector's length is equal to length of I +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float3 reflect(float3 i, float3 n) +{ + return i - 2.0f * n * dot(n,i); +} + +//////////////////////////////////////////////////////////////////////////////// +// cross product +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float3 cross(float3 a, float3 b) +{ + return make_float3(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x); +} + +//////////////////////////////////////////////////////////////////////////////// +// smoothstep +// - returns 0 if x < a +// - returns 1 if x > b +// - otherwise returns smooth interpolation between 0 and 1 based on x +//////////////////////////////////////////////////////////////////////////////// + +inline __device__ __host__ float smoothstep(float a, float b, float x) +{ + float y = clamp((x - a) / (b - a), 0.0f, 1.0f); + return (y*y*(3.0f - (2.0f*y))); +} +inline __device__ __host__ float2 smoothstep(float2 a, float2 b, float2 x) +{ + float2 y = clamp((x - a) / (b - a), 0.0f, 1.0f); + return (y*y*(make_float2(3.0f) - (make_float2(2.0f)*y))); +} +inline __device__ __host__ float3 smoothstep(float3 a, float3 b, float3 x) +{ + float3 y = clamp((x - a) / (b - a), 0.0f, 1.0f); + return (y*y*(make_float3(3.0f) - (make_float3(2.0f)*y))); +} +inline __device__ __host__ float4 smoothstep(float4 a, float4 b, float4 x) +{ + float4 y = clamp((x - a) / (b - a), 0.0f, 1.0f); + return (y*y*(make_float4(3.0f) - (make_float4(2.0f)*y))); +} + +#endif diff --git a/uav_simulator/local_sensing/src/pcl_render_node.cpp b/uav_simulator/local_sensing/src/pcl_render_node.cpp new file mode 100644 index 000000000..348ee0000 --- /dev/null +++ b/uav_simulator/local_sensing/src/pcl_render_node.cpp @@ -0,0 +1,390 @@ +#include +#include +#include +//include ros dep. +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "tf/tf.h" +#include "tf/transform_datatypes.h" +#include +//include pcl dep +#include +#include +#include +#include + #include +//include opencv and eigen +#include +#include "opencv2/highgui/highgui.hpp" +#include +#include +#include + +//#include +#include "depth_render.cuh" +#include "quadrotor_msgs/PositionCommand.h" +using namespace cv; +using namespace std; +using namespace Eigen; + +int *depth_hostptr; +cv::Mat depth_mat; + +//camera param +int width, height; +double fx,fy,cx,cy; + +DepthRender depthrender; +ros::Publisher pub_depth; +ros::Publisher pub_color; +ros::Publisher pub_pose; +ros::Publisher pub_pcl_wolrd; + +sensor_msgs::PointCloud2 local_map_pcl; +sensor_msgs::PointCloud2 local_depth_pcl; + +ros::Subscriber odom_sub; +ros::Subscriber global_map_sub, local_map_sub; + +ros::Timer local_sensing_timer, estimation_timer; + +bool has_global_map(false); +bool has_local_map(false); +bool has_odom(false); + +Matrix4d cam02body; +Matrix4d cam2world; +Eigen::Quaterniond cam2world_quat; +nav_msgs::Odometry _odom; + +double sensing_horizon, sensing_rate, estimation_rate; +double _x_size, _y_size, _z_size; +double _gl_xl, _gl_yl, _gl_zl; +double _resolution, _inv_resolution; +int _GLX_SIZE, _GLY_SIZE, _GLZ_SIZE; + +ros::Time last_odom_stamp = ros::TIME_MAX; +Eigen::Vector3d last_pose_world; + +void render_currentpose(); +void render_pcl_world(); + +inline Eigen::Vector3d gridIndex2coord(const Eigen::Vector3i & index) +{ + Eigen::Vector3d pt; + pt(0) = ((double)index(0) + 0.5) * _resolution + _gl_xl; + pt(1) = ((double)index(1) + 0.5) * _resolution + _gl_yl; + pt(2) = ((double)index(2) + 0.5) * _resolution + _gl_zl; + + return pt; +}; + +inline Eigen::Vector3i coord2gridIndex(const Eigen::Vector3d & pt) +{ + Eigen::Vector3i idx; + idx(0) = std::min( std::max( int( (pt(0) - _gl_xl) * _inv_resolution), 0), _GLX_SIZE - 1); + idx(1) = std::min( std::max( int( (pt(1) - _gl_yl) * _inv_resolution), 0), _GLY_SIZE - 1); + idx(2) = std::min( std::max( int( (pt(2) - _gl_zl) * _inv_resolution), 0), _GLZ_SIZE - 1); + + return idx; +}; + +void rcvOdometryCallbck(const nav_msgs::Odometry& odom) +{ + /*if(!has_global_map) + return;*/ + has_odom = true; + _odom = odom; + Matrix4d Pose_receive = Matrix4d::Identity(); + + Eigen::Vector3d request_position; + Eigen::Quaterniond request_pose; + request_position.x() = odom.pose.pose.position.x; + request_position.y() = odom.pose.pose.position.y; + request_position.z() = odom.pose.pose.position.z; + request_pose.x() = odom.pose.pose.orientation.x; + request_pose.y() = odom.pose.pose.orientation.y; + request_pose.z() = odom.pose.pose.orientation.z; + request_pose.w() = odom.pose.pose.orientation.w; + Pose_receive.block<3,3>(0,0) = request_pose.toRotationMatrix(); + Pose_receive(0,3) = request_position(0); + Pose_receive(1,3) = request_position(1); + Pose_receive(2,3) = request_position(2); + + Matrix4d body_pose = Pose_receive; + //convert to cam pose + cam2world = body_pose * cam02body; + cam2world_quat = cam2world.block<3,3>(0,0); + + last_odom_stamp = odom.header.stamp; + + last_pose_world(0) = odom.pose.pose.position.x; + last_pose_world(1) = odom.pose.pose.position.y; + last_pose_world(2) = odom.pose.pose.position.z; + + //publish tf + /*static tf::TransformBroadcaster br; + tf::Transform transform; + transform.setOrigin( tf::Vector3(cam2world(0,3), cam2world(1,3), cam2world(2,3) )); + transform.setRotation(tf::Quaternion(cam2world_quat.x(), cam2world_quat.y(), cam2world_quat.z(), cam2world_quat.w())); + br.sendTransform(tf::StampedTransform(transform, last_odom_stamp, "world", "camera")); //publish transform from world frame to quadrotor frame.*/ +} + +void pubCameraPose(const ros::TimerEvent & event) +{ + //cout<<"pub cam pose" + geometry_msgs::PoseStamped camera_pose; + camera_pose.header = _odom.header; + camera_pose.header.frame_id = "/map"; + camera_pose.pose.position.x = cam2world(0,3); + camera_pose.pose.position.y = cam2world(1,3); + camera_pose.pose.position.z = cam2world(2,3); + camera_pose.pose.orientation.w = cam2world_quat.w(); + camera_pose.pose.orientation.x = cam2world_quat.x(); + camera_pose.pose.orientation.y = cam2world_quat.y(); + camera_pose.pose.orientation.z = cam2world_quat.z(); + pub_pose.publish(camera_pose); +} + +void renderSensedPoints(const ros::TimerEvent & event) +{ + //if(! has_global_map || ! has_odom) return; + if( !has_global_map && !has_local_map) return; + + if( !has_odom ) return; + render_currentpose(); + render_pcl_world(); +} + +vector cloud_data; +void rcvGlobalPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map ) +{ + if(has_global_map) + return; + + ROS_WARN("Global Pointcloud received.."); + //load global map + pcl::PointCloud cloudIn; + pcl::PointXYZ pt_in; + //transform map to point cloud format + pcl::fromROSMsg(pointcloud_map, cloudIn); + for(int i = 0; i < int(cloudIn.points.size()); i++){ + pt_in = cloudIn.points[i]; + cloud_data.push_back(pt_in.x); + cloud_data.push_back(pt_in.y); + cloud_data.push_back(pt_in.z); + } + printf("global map has points: %d.\n", (int)cloud_data.size() / 3 ); + //pass cloud_data to depth render + depthrender.set_data(cloud_data); + depth_hostptr = (int*) malloc(width * height * sizeof(int)); + + has_global_map = true; +} + +void rcvLocalPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map ) +{ + //ROS_WARN("Local Pointcloud received.."); + //load local map + pcl::PointCloud cloudIn; + pcl::PointXYZ pt_in; + //transform map to point cloud format + pcl::fromROSMsg(pointcloud_map, cloudIn); + + if(cloudIn.points.size() == 0) return; + for(int i = 0; i < int(cloudIn.points.size()); i++){ + pt_in = cloudIn.points[i]; + Eigen::Vector3d pose_pt(pt_in.x, pt_in.y, pt_in.z); + //pose_pt = gridIndex2coord(coord2gridIndex(pose_pt)); + cloud_data.push_back(pose_pt(0)); + cloud_data.push_back(pose_pt(1)); + cloud_data.push_back(pose_pt(2)); + } + //printf("local map has points: %d.\n", (int)cloud_data.size() / 3 ); + //pass cloud_data to depth render + depthrender.set_data(cloud_data); + depth_hostptr = (int*) malloc(width * height * sizeof(int)); + + has_local_map = true; +} + +void render_pcl_world() +{ + //for debug purpose + pcl::PointCloud localMap; + pcl::PointXYZ pt_in; + + Eigen::Vector4d pose_in_camera; + Eigen::Vector4d pose_in_world; + Eigen::Vector3d pose_pt; + + for(int u = 0; u < width; u++) + for(int v = 0; v < height; v++){ + float depth = depth_mat.at(v,u); + + if(depth == 0.0) + continue; + + pose_in_camera(0) = (u - cx) * depth / fx; + pose_in_camera(1) = (v - cy) * depth / fy; + pose_in_camera(2) = depth; + pose_in_camera(3) = 1.0; + + pose_in_world = cam2world * pose_in_camera; + + if( (pose_in_world.segment(0,3) - last_pose_world).norm() > sensing_horizon ) + continue; + + pose_pt = pose_in_world.head(3); + //pose_pt = gridIndex2coord(coord2gridIndex(pose_pt)); + pt_in.x = pose_pt(0); + pt_in.y = pose_pt(1); + pt_in.z = pose_pt(2); + + localMap.points.push_back(pt_in); + } + + localMap.width = localMap.points.size(); + localMap.height = 1; + localMap.is_dense = true; + + pcl::toROSMsg(localMap, local_map_pcl); + local_map_pcl.header.frame_id = "/map"; + local_map_pcl.header.stamp = last_odom_stamp; + + pub_pcl_wolrd.publish(local_map_pcl); +} + +void render_currentpose() +{ + double this_time = ros::Time::now().toSec(); + + Matrix4d cam_pose = cam2world.inverse(); + + double pose[4 * 4]; + + for(int i = 0; i < 4; i ++) + for(int j = 0; j < 4; j ++) + pose[j + 4 * i] = cam_pose(i, j); + + depthrender.render_pose(pose, depth_hostptr); + //depthrender.render_pose(cam_pose, depth_hostptr); + + depth_mat = cv::Mat::zeros(height, width, CV_32FC1); + double min = 0.5; + double max = 1.0f; + for(int i = 0; i < height; i++) + for(int j = 0; j < width; j++) + { + float depth = (float)depth_hostptr[i * width + j] / 1000.0f; + depth = depth < 500.0f ? depth : 0; + max = depth > max ? depth : max; + depth_mat.at(i,j) = depth; + } + //ROS_INFO("render cost %lf ms.", (ros::Time::now().toSec() - this_time) * 1000.0f); + //printf("max_depth %lf.\n", max); + + cv_bridge::CvImage out_msg; + out_msg.header.stamp = last_odom_stamp; + out_msg.header.frame_id = "camera"; + out_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + out_msg.image = depth_mat.clone(); + pub_depth.publish(out_msg.toImageMsg()); + + cv::Mat adjMap; + // depth_mat.convertTo(adjMap,CV_8UC1, 255 / (max-min), -min); + depth_mat.convertTo(adjMap,CV_8UC1, 255 /13.0, -min); + cv::Mat falseColorsMap; + cv::applyColorMap(adjMap, falseColorsMap, cv::COLORMAP_RAINBOW); + cv_bridge::CvImage cv_image_colored; + cv_image_colored.header.frame_id = "depthmap"; + cv_image_colored.header.stamp = last_odom_stamp; + cv_image_colored.encoding = sensor_msgs::image_encodings::BGR8; + cv_image_colored.image = falseColorsMap; + pub_color.publish(cv_image_colored.toImageMsg()); + //cv::imshow("depth_image", adjMap); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "pcl_render"); + ros::NodeHandle nh("~"); + + nh.getParam("cam_width", width); + nh.getParam("cam_height", height); + nh.getParam("cam_fx", fx); + nh.getParam("cam_fy", fy); + nh.getParam("cam_cx", cx); + nh.getParam("cam_cy", cy); + nh.getParam("sensing_horizon", sensing_horizon); + nh.getParam("sensing_rate", sensing_rate); + nh.getParam("estimation_rate", estimation_rate); + + nh.getParam("map/x_size", _x_size); + nh.getParam("map/y_size", _y_size); + nh.getParam("map/z_size", _z_size); + + depthrender.set_para(fx, fy, cx, cy, width, height); + + // cam02body << 0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, + // 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, + // -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, + // 0.0, 0.0, 0.0, 1.0; + + cam02body << 0.0, 0.0, 1.0, 0.0, + -1.0, 0.0, 0.0, 0.0, + 0.0, -1.0,0.0, 0.0, + 0.0, 0.0, 0.0, 1.0; + + //init cam2world transformation + cam2world = Matrix4d::Identity(); + //subscribe point cloud + global_map_sub = nh.subscribe( "global_map", 1, rcvGlobalPointCloudCallBack); + local_map_sub = nh.subscribe( "local_map", 1, rcvLocalPointCloudCallBack); + odom_sub = nh.subscribe( "odometry", 50, rcvOdometryCallbck ); + + //publisher depth image and color image + pub_depth = nh.advertise("depth",1000); + pub_color = nh.advertise("colordepth",1000); + pub_pose = nh.advertise("camera_pose",1000); + pub_pcl_wolrd = nh.advertise("rendered_pcl",1); + + double sensing_duration = 1.0 / sensing_rate; + double estimate_duration = 1.0 / estimation_rate; + + local_sensing_timer = nh.createTimer(ros::Duration(sensing_duration), renderSensedPoints); + estimation_timer = nh.createTimer(ros::Duration(estimate_duration), pubCameraPose); + //cv::namedWindow("depth_image",1); + + _inv_resolution = 1.0 / _resolution; + + _gl_xl = -_x_size/2.0; + _gl_yl = -_y_size/2.0; + _gl_zl = 0.0; + + _GLX_SIZE = (int)(_x_size * _inv_resolution); + _GLY_SIZE = (int)(_y_size * _inv_resolution); + _GLZ_SIZE = (int)(_z_size * _inv_resolution); + + ros::Rate rate(100); + bool status = ros::ok(); + while(status) + { + ros::spinOnce(); + status = ros::ok(); + rate.sleep(); + } +} diff --git a/uav_simulator/local_sensing/src/pointcloud_render_node.cpp b/uav_simulator/local_sensing/src/pointcloud_render_node.cpp new file mode 100644 index 000000000..c95910fd3 --- /dev/null +++ b/uav_simulator/local_sensing/src/pointcloud_render_node.cpp @@ -0,0 +1,199 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace Eigen; + +ros::Publisher pub_cloud; + +sensor_msgs::PointCloud2 local_map_pcl; +sensor_msgs::PointCloud2 local_depth_pcl; + +ros::Subscriber odom_sub; +ros::Subscriber global_map_sub, local_map_sub; + +ros::Timer local_sensing_timer; + +bool has_global_map(false); +bool has_local_map(false); +bool has_odom(false); + +nav_msgs::Odometry _odom; + +double sensing_horizon, sensing_rate, estimation_rate; +double _x_size, _y_size, _z_size; +double _gl_xl, _gl_yl, _gl_zl; +double _resolution, _inv_resolution; +int _GLX_SIZE, _GLY_SIZE, _GLZ_SIZE; + +ros::Time last_odom_stamp = ros::TIME_MAX; + +inline Eigen::Vector3d gridIndex2coord(const Eigen::Vector3i& index) { + Eigen::Vector3d pt; + pt(0) = ((double)index(0) + 0.5) * _resolution + _gl_xl; + pt(1) = ((double)index(1) + 0.5) * _resolution + _gl_yl; + pt(2) = ((double)index(2) + 0.5) * _resolution + _gl_zl; + + return pt; +}; + +inline Eigen::Vector3i coord2gridIndex(const Eigen::Vector3d& pt) { + Eigen::Vector3i idx; + idx(0) = std::min(std::max(int((pt(0) - _gl_xl) * _inv_resolution), 0), + _GLX_SIZE - 1); + idx(1) = std::min(std::max(int((pt(1) - _gl_yl) * _inv_resolution), 0), + _GLY_SIZE - 1); + idx(2) = std::min(std::max(int((pt(2) - _gl_zl) * _inv_resolution), 0), + _GLZ_SIZE - 1); + + return idx; +}; + +void rcvOdometryCallbck(const nav_msgs::Odometry& odom) { + /*if(!has_global_map) + return;*/ + has_odom = true; + _odom = odom; +} + +pcl::PointCloud _cloud_all_map, _local_map; +pcl::VoxelGrid _voxel_sampler; +sensor_msgs::PointCloud2 _local_map_pcd; + +pcl::search::KdTree _kdtreeLocalMap; +vector _pointIdxRadiusSearch; +vector _pointRadiusSquaredDistance; + +void rcvGlobalPointCloudCallBack( + const sensor_msgs::PointCloud2& pointcloud_map) { + if (has_global_map) return; + + ROS_WARN("Global Pointcloud received.."); + + pcl::PointCloud cloud_input; + pcl::fromROSMsg(pointcloud_map, cloud_input); + + _voxel_sampler.setLeafSize(0.1f, 0.1f, 0.1f); + _voxel_sampler.setInputCloud(cloud_input.makeShared()); + _voxel_sampler.filter(_cloud_all_map); + + _kdtreeLocalMap.setInputCloud(_cloud_all_map.makeShared()); + + has_global_map = true; +} + +void renderSensedPoints(const ros::TimerEvent& event) { + if (!has_global_map || !has_odom) return; + + Eigen::Quaterniond q; + q.x() = _odom.pose.pose.orientation.x; + q.y() = _odom.pose.pose.orientation.y; + q.z() = _odom.pose.pose.orientation.z; + q.w() = _odom.pose.pose.orientation.w; + + Eigen::Matrix3d rot; + rot = q; + Eigen::Vector3d yaw_vec = rot.col(0); + + _local_map.points.clear(); + pcl::PointXYZ searchPoint(_odom.pose.pose.position.x, + _odom.pose.pose.position.y, + _odom.pose.pose.position.z); + _pointIdxRadiusSearch.clear(); + _pointRadiusSquaredDistance.clear(); + + pcl::PointXYZ pt; + if (_kdtreeLocalMap.radiusSearch(searchPoint, sensing_horizon, + _pointIdxRadiusSearch, + _pointRadiusSquaredDistance) > 0) { + for (size_t i = 0; i < _pointIdxRadiusSearch.size(); ++i) { + pt = _cloud_all_map.points[_pointIdxRadiusSearch[i]]; + + if ((fabs(pt.z - _odom.pose.pose.position.z) / (sensing_horizon)) > + tan(M_PI / 12.0)) + continue; + + Vector3d pt_vec(pt.x - _odom.pose.pose.position.x, + pt.y - _odom.pose.pose.position.y, + pt.z - _odom.pose.pose.position.z); + + if (pt_vec.dot(yaw_vec) < 0) continue; + + _local_map.points.push_back(pt); + } + } else { + return; + } + + _local_map.width = _local_map.points.size(); + _local_map.height = 1; + _local_map.is_dense = true; + + pcl::toROSMsg(_local_map, _local_map_pcd); + _local_map_pcd.header.frame_id = "map"; + + pub_cloud.publish(_local_map_pcd); +} + +void rcvLocalPointCloudCallBack( + const sensor_msgs::PointCloud2& pointcloud_map) { + // do nothing, fix later +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "pcl_render"); + ros::NodeHandle nh("~"); + + nh.getParam("sensing_horizon", sensing_horizon); + nh.getParam("sensing_rate", sensing_rate); + nh.getParam("estimation_rate", estimation_rate); + + nh.getParam("map/x_size", _x_size); + nh.getParam("map/y_size", _y_size); + nh.getParam("map/z_size", _z_size); + + // subscribe point cloud + global_map_sub = nh.subscribe("global_map", 1, rcvGlobalPointCloudCallBack); + local_map_sub = nh.subscribe("local_map", 1, rcvLocalPointCloudCallBack); + odom_sub = nh.subscribe("odometry", 50, rcvOdometryCallbck); + + // publisher depth image and color image + pub_cloud = + nh.advertise("/pcl_render_node/cloud", 10); + + double sensing_duration = 1.0 / sensing_rate * 2.5; + + local_sensing_timer = + nh.createTimer(ros::Duration(sensing_duration), renderSensedPoints); + + _inv_resolution = 1.0 / _resolution; + + _gl_xl = -_x_size / 2.0; + _gl_yl = -_y_size / 2.0; + _gl_zl = 0.0; + + _GLX_SIZE = (int)(_x_size * _inv_resolution); + _GLY_SIZE = (int)(_y_size * _inv_resolution); + _GLZ_SIZE = (int)(_z_size * _inv_resolution); + + ros::Rate rate(100); + bool status = ros::ok(); + while (status) { + ros::spinOnce(); + status = ros::ok(); + rate.sleep(); + } +} diff --git a/dyn_planner/.vscode/c_cpp_properties.json b/uav_simulator/map_generator/.vscode/c_cpp_properties.json similarity index 52% rename from dyn_planner/.vscode/c_cpp_properties.json rename to uav_simulator/map_generator/.vscode/c_cpp_properties.json index ea8fa2a98..7a1e36dc0 100644 --- a/dyn_planner/.vscode/c_cpp_properties.json +++ b/uav_simulator/map_generator/.vscode/c_cpp_properties.json @@ -4,18 +4,14 @@ "name": "Linux", "includePath": [ "${workspaceFolder}/**", - "/opt/ros/kinetic/include", - "/usr/include/eigen3", - "/home/bzhouai/workspaces/plan_ws/devel/include/quadrotor_msgs/", - "/home/bzhouai/workspaces/plan_ws/devel/include", "/usr/include/pcl-1.7", - "${workspaceFolder}/plan_env/include" + "/usr/include/eigen3", + "/opt/ros/kinetic/include" ], "defines": [], "compilerPath": "/usr/bin/gcc", "cStandard": "c11", - "cppStandard": "c++17", - "intelliSenseMode": "clang-x64" + "cppStandard": "c++17" } ], "version": 4 diff --git a/uav_simulator/map_generator/src/random_forest_sensing.cpp b/uav_simulator/map_generator/src/random_forest_sensing.cpp index 3fdba3cf7..fd7505f1f 100644 --- a/uav_simulator/map_generator/src/random_forest_sensing.cpp +++ b/uav_simulator/map_generator/src/random_forest_sensing.cpp @@ -31,6 +31,7 @@ uniform_real_distribution rand_h; ros::Publisher _local_map_pub; ros::Publisher _all_map_pub; +ros::Publisher click_map_pub_; ros::Subscriber _odom_sub; vector _state; @@ -43,12 +44,21 @@ double _z_limit, _sensing_range, _resolution, _sense_rate, _init_x, _init_y; bool _map_ok = false; bool _has_odom = false; -sensor_msgs::PointCloud2 localMap_pcd; +int circle_num_; +double radius_l_, radius_h_, z_l_, z_h_; +double theta_; +uniform_real_distribution rand_radius_; +uniform_real_distribution rand_radius2_; +uniform_real_distribution rand_theta_; +uniform_real_distribution rand_z_; + sensor_msgs::PointCloud2 globalMap_pcd; pcl::PointCloud cloudMap; -void RandomMapGenerate() -{ +sensor_msgs::PointCloud2 localMap_pcd; +pcl::PointCloud clicked_cloud_; + +void RandomMapGenerate() { pcl::PointXYZ pt_random; rand_x = uniform_real_distribution(_x_l, _x_h); @@ -56,15 +66,27 @@ void RandomMapGenerate() rand_w = uniform_real_distribution(_w_l, _w_h); rand_h = uniform_real_distribution(_h_l, _h_h); - for (int i = 0; i < _obs_num; i++) - { + rand_radius_ = uniform_real_distribution(radius_l_, radius_h_); + rand_radius2_ = uniform_real_distribution(radius_l_, 1.2); + rand_theta_ = uniform_real_distribution(-theta_, theta_); + rand_z_ = uniform_real_distribution(z_l_, z_h_); + + // generate polar obs + for (int i = 0; i < _obs_num; i++) { double x, y, w, h; x = rand_x(eng); y = rand_y(eng); w = rand_w(eng); - if (sqrt(pow(x - _init_x, 2) + pow(y - _init_y, 2)) < 2.0) + if (sqrt(pow(x - _init_x, 2) + pow(y - _init_y, 2)) < 2.0) { + i--; continue; + } + + if (sqrt(pow(x - 19.0, 2) + pow(y - 0.0, 2)) < 2.0) { + i--; + continue; + } x = floor(x / _resolution) * _resolution + _resolution / 2.0; y = floor(y / _resolution) * _resolution + _resolution / 2.0; @@ -72,12 +94,10 @@ void RandomMapGenerate() int widNum = ceil(w / _resolution); for (int r = -widNum / 2.0; r < widNum / 2.0; r++) - for (int s = -widNum / 2.0; s < widNum / 2.0; s++) - { + for (int s = -widNum / 2.0; s < widNum / 2.0; s++) { h = rand_h(eng); int heiNum = ceil(h / _resolution); - for (int t = -10; t < heiNum; t++) - { + for (int t = -30; t < heiNum; t++) { pt_random.x = x + (r + 0.5) * _resolution + 1e-2; pt_random.y = y + (s + 0.5) * _resolution + 1e-2; pt_random.z = (t + 0.5) * _resolution + 1e-2; @@ -86,6 +106,60 @@ void RandomMapGenerate() } } + // generate circle obs + for (int i = 0; i < circle_num_; ++i) { + double x, y, z; + x = rand_x(eng); + y = rand_y(eng); + z = rand_z_(eng); + + if (sqrt(pow(x - _init_x, 2) + pow(y - _init_y, 2)) < 2.0) { + i--; + continue; + } + + if (sqrt(pow(x - 19.0, 2) + pow(y - 0.0, 2)) < 2.0) { + i--; + continue; + } + + x = floor(x / _resolution) * _resolution + _resolution / 2.0; + y = floor(y / _resolution) * _resolution + _resolution / 2.0; + z = floor(z / _resolution) * _resolution + _resolution / 2.0; + + Eigen::Vector3d translate(x, y, z); + + double theta = rand_theta_(eng); + Eigen::Matrix3d rotate; + rotate << cos(theta), -sin(theta), 0.0, sin(theta), cos(theta), 0.0, 0, 0, + 1; + + double radius1 = rand_radius_(eng); + double radius2 = rand_radius2_(eng); + + // draw a circle centered at (x,y,z) + Eigen::Vector3d cpt; + for (double angle = 0.0; angle < 6.282; angle += _resolution / 2) { + cpt(0) = 0.0; + cpt(1) = radius1 * cos(angle); + cpt(2) = radius2 * sin(angle); + + // inflate + Eigen::Vector3d cpt_if; + for (int ifx = -0; ifx <= 0; ++ifx) + for (int ify = -0; ify <= 0; ++ify) + for (int ifz = -0; ifz <= 0; ++ifz) { + cpt_if = cpt + Eigen::Vector3d(ifx * _resolution, ify * _resolution, + ifz * _resolution); + cpt_if = rotate * cpt_if + Eigen::Vector3d(x, y, z); + pt_random.x = cpt_if(0); + pt_random.y = cpt_if(1); + pt_random.z = cpt_if(2); + cloudMap.push_back(pt_random); + } + } + } + cloudMap.width = cloudMap.points.size(); cloudMap.height = 1; cloudMap.is_dense = true; @@ -97,37 +171,33 @@ void RandomMapGenerate() _map_ok = true; } -void rcvOdometryCallbck(const nav_msgs::Odometry odom) -{ - if (odom.child_frame_id == "X" || odom.child_frame_id == "O") - return; +void rcvOdometryCallbck(const nav_msgs::Odometry odom) { + if (odom.child_frame_id == "X" || odom.child_frame_id == "O") return; _has_odom = true; - _state = { odom.pose.pose.position.x, - odom.pose.pose.position.y, - odom.pose.pose.position.z, - odom.twist.twist.linear.x, - odom.twist.twist.linear.y, - odom.twist.twist.linear.z, - 0.0, - 0.0, - 0.0 }; + _state = {odom.pose.pose.position.x, + odom.pose.pose.position.y, + odom.pose.pose.position.z, + odom.twist.twist.linear.x, + odom.twist.twist.linear.y, + odom.twist.twist.linear.z, + 0.0, + 0.0, + 0.0}; } int i = 0; -void pubSensedPoints() -{ +void pubSensedPoints() { // if (i < 10) { pcl::toROSMsg(cloudMap, globalMap_pcd); globalMap_pcd.header.frame_id = "world"; _all_map_pub.publish(globalMap_pcd); // } - // i++; return; - if (!_map_ok || !_has_odom) - return; + /* ---------- only publish points around current position ---------- */ + if (!_map_ok || !_has_odom) return; pcl::PointCloud localMap; @@ -140,16 +210,14 @@ void pubSensedPoints() if (isnan(searchPoint.x) || isnan(searchPoint.y) || isnan(searchPoint.z)) return; - if (kdtreeLocalMap.radiusSearch(searchPoint, _sensing_range, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) - { - for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) - { + if (kdtreeLocalMap.radiusSearch(searchPoint, _sensing_range, + pointIdxRadiusSearch, + pointRadiusSquaredDistance) > 0) { + for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) { pt = cloudMap.points[pointIdxRadiusSearch[i]]; localMap.points.push_back(pt); } - } - else - { + } else { ROS_ERROR("[Map server] No obstacles ."); return; } @@ -163,16 +231,56 @@ void pubSensedPoints() _local_map_pub.publish(localMap_pcd); } -int main(int argc, char** argv) -{ +void clickCallback(const geometry_msgs::PoseStamped& msg) { + double x = msg.pose.position.x; + double y = msg.pose.position.y; + double w = rand_w(eng); + double h; + pcl::PointXYZ pt_random; + + x = floor(x / _resolution) * _resolution + _resolution / 2.0; + y = floor(y / _resolution) * _resolution + _resolution / 2.0; + + int widNum = ceil(w / _resolution); + + for (int r = -widNum / 2.0; r < widNum / 2.0; r++) + for (int s = -widNum / 2.0; s < widNum / 2.0; s++) { + h = rand_h(eng); + int heiNum = ceil(h / _resolution); + for (int t = -1; t < heiNum; t++) { + pt_random.x = x + (r + 0.5) * _resolution + 1e-2; + pt_random.y = y + (s + 0.5) * _resolution + 1e-2; + pt_random.z = (t + 0.5) * _resolution + 1e-2; + clicked_cloud_.points.push_back(pt_random); + cloudMap.points.push_back(pt_random); + } + } + clicked_cloud_.width = clicked_cloud_.points.size(); + clicked_cloud_.height = 1; + clicked_cloud_.is_dense = true; + + pcl::toROSMsg(clicked_cloud_, localMap_pcd); + localMap_pcd.header.frame_id = "world"; + click_map_pub_.publish(localMap_pcd); + + cloudMap.width = cloudMap.points.size(); + + return; +} + +int main(int argc, char** argv) { ros::init(argc, argv, "random_map_sensing"); ros::NodeHandle n("~"); - _local_map_pub = n.advertise("random_forest", 1); - _all_map_pub = n.advertise("all_map", 1); + _local_map_pub = n.advertise("/map_generator/local_cloud", 1); + _all_map_pub = n.advertise("/map_generator/global_cloud", 1); _odom_sub = n.subscribe("odometry", 50, rcvOdometryCallbck); + click_map_pub_ = + n.advertise("/pcl_render_node/local_map", 1); + // ros::Subscriber click_sub = n.subscribe("/goal", 10, clickCallback); + n.param("init_state_x", _init_x, 0.0); n.param("init_state_y", _init_y, 0.0); @@ -180,13 +288,20 @@ int main(int argc, char** argv) n.param("map/y_size", _y_size, 50.0); n.param("map/z_size", _z_size, 5.0); n.param("map/obs_num", _obs_num, 30); - n.param("map/resolution", _resolution, 0.2); + n.param("map/resolution", _resolution, 0.1); + n.param("map/circle_num", circle_num_, 30); n.param("ObstacleShape/lower_rad", _w_l, 0.3); n.param("ObstacleShape/upper_rad", _w_h, 0.8); n.param("ObstacleShape/lower_hei", _h_l, 3.0); n.param("ObstacleShape/upper_hei", _h_h, 7.0); + n.param("ObstacleShape/radius_l", radius_l_, 7.0); + n.param("ObstacleShape/radius_h", radius_h_, 7.0); + n.param("ObstacleShape/z_l", z_l_, 7.0); + n.param("ObstacleShape/z_h", z_h_, 7.0); + n.param("ObstacleShape/theta", theta_, 7.0); + n.param("sensing/radius", _sensing_range, 10.0); n.param("sensing/radius", _sense_rate, 10.0); @@ -205,8 +320,7 @@ int main(int argc, char** argv) ros::Rate loop_rate(_sense_rate); - while (ros::ok()) - { + while (ros::ok()) { pubSensedPoints(); ros::spinOnce(); loop_rate.sleep(); diff --git a/uav_simulator/so3_disturbance_generator/CMakeLists.txt b/uav_simulator/so3_disturbance_generator/CMakeLists.txt index 61ab1f0cc..235b84204 100755 --- a/uav_simulator/so3_disturbance_generator/CMakeLists.txt +++ b/uav_simulator/so3_disturbance_generator/CMakeLists.txt @@ -1,3 +1,42 @@ +#cmake_minimum_required(VERSION 2.4.6) +#include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +#rosbuild_init() + +#set the default path for built executables to the "bin" directory +#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +#set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) + +#add dynamic reconfigure api +#rosbuild_find_ros_package(dynamic_reconfigure) +#include(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake) +#gencfg() + +#rosbuild_add_executable(so3_disturbance_generator src/so3_disturbance_generator.cpp) +#target_link_libraries(so3_disturbance_generator pose_utils) +#---------------------------------- cmake_minimum_required(VERSION 2.8.3) project(so3_disturbance_generator) @@ -15,14 +54,10 @@ find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure ) -catkin_package() - -########### -## Build ## -########### find_package(Armadillo REQUIRED) include_directories(${ARMADILLO_INCLUDE_DIRS}) +catkin_package() ## Specify additional locations of header files ## Your package locations should be listed before other locations @@ -33,7 +68,6 @@ include_directories( add_executable(so3_disturbance_generator src/so3_disturbance_generator.cpp) add_dependencies(so3_disturbance_generator ${PROJECT_NAME}_gencfg) - target_link_libraries(so3_disturbance_generator ${catkin_LIBRARIES} ${ARMADILLO_LIBRARIES}