├── src ├── uav_simulator │ ├── local_sensing │ │ ├── src │ │ │ ├── empty.h │ │ │ ├── empty.cpp │ │ │ ├── csv_convert.py │ │ │ ├── depth_render.cuh │ │ │ ├── cuda_exception.cuh │ │ │ ├── AlignError.h │ │ │ ├── device_image.cuh │ │ │ ├── ceres_extensions.h │ │ │ └── depth_render.cu │ │ ├── .vscode │ │ │ └── settings.json │ │ ├── params │ │ │ └── camera.yaml │ │ ├── cfg │ │ │ └── local_sensing_node.cfg │ │ ├── package.xml │ │ ├── CMakeModules │ │ │ ├── FindEigen.cmake~ │ │ │ ├── FindEigen.cmake │ │ │ └── FindCUDA │ │ │ │ ├── make2cmake.cmake │ │ │ │ └── parse_cubin.cmake │ │ └── CMakeLists.txt │ ├── Utils │ │ ├── quadrotor_msgs │ │ │ ├── src │ │ │ │ ├── quadrotor_msgs │ │ │ │ │ ├── __init__.py │ │ │ │ │ ├── __init__.pyc │ │ │ │ │ └── msg │ │ │ │ │ │ ├── __init__.py │ │ │ │ │ │ ├── _Gains.py │ │ │ │ │ │ ├── _Corrections.py │ │ │ │ │ │ ├── _AuxCommand.py │ │ │ │ │ │ └── _StatusData.py │ │ │ │ ├── encode_msgs.cpp │ │ │ │ └── decode_msgs.cpp │ │ │ ├── msg │ │ │ │ ├── Corrections.msg │ │ │ │ ├── Gains.msg │ │ │ │ ├── StatusData.msg │ │ │ │ ├── TRPYCommand.msg │ │ │ │ ├── AuxCommand.msg │ │ │ │ ├── SO3Command.msg │ │ │ │ ├── Odometry.msg │ │ │ │ ├── OutputData.msg │ │ │ │ ├── PPROutputData.msg │ │ │ │ ├── PositionCommand.msg~ │ │ │ │ ├── Serial.msg │ │ │ │ ├── PositionCommand.msg │ │ │ │ ├── PolynomialTrajectory.msg~ │ │ │ │ ├── LQRTrajectory.msg │ │ │ │ └── PolynomialTrajectory.msg │ │ │ ├── include │ │ │ │ └── quadrotor_msgs │ │ │ │ │ ├── encode_msgs.h │ │ │ │ │ ├── decode_msgs.h │ │ │ │ │ └── comm_types.h │ │ │ ├── package.xml │ │ │ ├── cmake │ │ │ │ └── FindEigen3.cmake │ │ │ └── CMakeLists.txt │ │ ├── pose_utils │ │ │ ├── Makefile │ │ │ ├── package.xml │ │ │ ├── include │ │ │ │ └── pose_utils.h │ │ │ └── CMakeLists.txt │ │ └── odom_visualization │ │ │ ├── Makefile │ │ │ ├── meshes │ │ │ └── hummingbird.mesh │ │ │ ├── mainpage.dox │ │ │ └── package.xml │ ├── map_generator │ │ ├── .vscode │ │ │ └── c_cpp_properties.json │ │ ├── CMakeLists.txt │ │ └── package.xml │ └── fake_drone │ │ ├── CMakeLists.txt │ │ ├── package.xml │ │ └── src │ │ └── poscmd_2_odom.cpp └── planner │ ├── traj_utils_so │ ├── msg │ │ ├── MultiBsplines.msg │ │ ├── DataDisp.msg │ │ └── Bspline.msg │ ├── package.xml │ ├── include │ │ └── traj_utils │ │ │ └── planning_visualization.h │ └── CMakeLists.txt │ ├── traj_utils_z │ ├── msg │ │ ├── MultiBsplines.msg │ │ ├── DataDisp.msg │ │ └── Bspline.msg │ ├── package.xml │ ├── include │ │ └── traj_utils │ │ │ └── planning_visualization.h │ └── CMakeLists.txt │ ├── plan_manage │ ├── launch │ │ ├── rviz.launch │ │ ├── simple_run.launch │ │ ├── simulator.launch.py │ │ ├── single_run_in_sim.launch │ │ └── run_in_sim.launch │ ├── config │ │ └── random_forest.yaml │ ├── src │ │ └── ego_planner_node.cpp │ ├── package.xml │ ├── include │ │ └── plan_manage │ │ │ ├── planner_manager.h │ │ │ └── ego_replan_fsm.h │ └── CMakeLists.txt │ ├── new │ ├── package.xml │ ├── CMakeLists.txt │ └── src │ │ └── testpub.cpp │ ├── bridge_test │ ├── package.xml │ ├── CMakeLists.txt │ └── src │ │ ├── test1.cpp │ │ └── test.cpp │ ├── bspline_opt │ ├── include │ │ └── bspline_opt │ │ │ ├── gradient_descent_optimizer.h │ │ │ └── uniform_bspline.h │ ├── CMakeLists.txt │ ├── package.xml │ └── src │ │ └── gradient_descent_optimizer.cpp │ ├── plan_env │ ├── include │ │ └── plan_env │ │ │ ├── raycast.h │ │ │ └── obj_predictor.h │ ├── CMakeLists.txt │ └── package.xml │ └── path_searching │ ├── CMakeLists.txt │ ├── package.xml │ └── include │ └── path_searching │ └── dyn_a_star.h ├── 说明.txt └── README.md /src/uav_simulator/local_sensing/src/empty.h: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/uav_simulator/local_sensing/src/empty.cpp: -------------------------------------------------------------------------------- 1 | #include "empty.h" -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/pose_utils/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /src/uav_simulator/Utils/odom_visualization/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /src/planner/traj_utils_so/msg/MultiBsplines.msg: -------------------------------------------------------------------------------- 1 | int32 drone_id_from 2 | 3 | Bspline[] traj 4 | 5 | -------------------------------------------------------------------------------- /src/planner/traj_utils_z/msg/MultiBsplines.msg: -------------------------------------------------------------------------------- 1 | int32 drone_id_from 2 | 3 | Bspline[] traj 4 | 5 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/msg/Corrections.msg: -------------------------------------------------------------------------------- 1 | float64 kf_correction 2 | float64[2] angle_corrections 3 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/msg/Gains.msg: -------------------------------------------------------------------------------- 1 | float64 Kp 2 | float64 Kd 3 | float64 Kp_yaw 4 | float64 Kd_yaw 5 | -------------------------------------------------------------------------------- /src/uav_simulator/local_sensing/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "*.cuh": "cpp" 4 | } 5 | } -------------------------------------------------------------------------------- /src/planner/traj_utils_so/msg/DataDisp.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | float64 a 4 | float64 b 5 | float64 c 6 | float64 d 7 | float64 e -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/msg/StatusData.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint16 loop_rate 3 | float64 voltage 4 | uint8 seq 5 | -------------------------------------------------------------------------------- /src/planner/traj_utils_z/msg/DataDisp.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | float64 a 4 | float64 b 5 | float64 c 6 | float64 d 7 | float64 e -------------------------------------------------------------------------------- /说明.txt: -------------------------------------------------------------------------------- 1 | 这个 2 | colcon build --symlink-install 3 | ros2 launch ego_planner single_run_in_sim.launch.py 4 | 可直接使用，是单个的精简版， 5 | 然后多个蜂群的launch.py还没有做，还有相关的包也没转，多个的话这个得在全版那里做 6 | 7 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/msg/TRPYCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 thrust 3 | float32 roll 4 | float32 pitch 5 | float32 yaw 6 | quadrotor_msgs/AuxCommand aux 7 | -------------------------------------------------------------------------------- /src/planner/plan_manage/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/odom_visualization/meshes/hummingbird.mesh: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ssssuxin/ego_planner_ros2/HEAD/src/uav_simulator/Utils/odom_visualization/meshes/hummingbird.mesh -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/__init__.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ssssuxin/ego_planner_ros2/HEAD/src/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/__init__.pyc -------------------------------------------------------------------------------- /src/planner/plan_manage/launch/simple_run.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/msg/AuxCommand.msg: -------------------------------------------------------------------------------- 1 | float64 current_yaw 2 | float64 kf_correction 3 | float64[2] angle_corrections# Trims for roll, pitch 4 | bool enable_motors 5 | bool use_external_yaw 6 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/msg/SO3Command.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Vector3 force 3 | geometry_msgs/Quaternion orientation 4 | float64[3] kR 5 | float64[3] kOm 6 | quadrotor_msgs/AuxCommand aux 7 | -------------------------------------------------------------------------------- /src/uav_simulator/local_sensing/params/camera.yaml: -------------------------------------------------------------------------------- 1 | cam_width: 640 2 | cam_height: 480 3 | cam_fx: 387.229248046875 4 | cam_fy: 387.229248046875 5 | cam_cx: 321.04638671875 6 | cam_cy: 243.44969177246094 7 | -------------------------------------------------------------------------------- /src/planner/traj_utils_so/msg/Bspline.msg: -------------------------------------------------------------------------------- 1 | int32 drone_id 2 | int32 order 3 | int64 traj_id 4 | builtin_interfaces/Time start_time 5 | float64[] knots 6 | geometry_msgs/Point[] pos_pts 7 | float64[] yaw_pts 8 | float64 yaw_dt 9 | 10 | -------------------------------------------------------------------------------- /src/planner/traj_utils_z/msg/Bspline.msg: -------------------------------------------------------------------------------- 1 | int32 drone_id 2 | int32 order 3 | int64 traj_id 4 | builtin_interfaces/Time start_time 5 | float64[] knots 6 | geometry_msgs/Point[] pos_pts 7 | float64[] yaw_pts 8 | float64 yaw_dt 9 | 10 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/msg/Odometry.msg: -------------------------------------------------------------------------------- 1 | uint8 STATUS_ODOM_VALID=0 2 | uint8 STATUS_ODOM_INVALID=1 3 | uint8 STATUS_ODOM_LOOPCLOSURE=2 4 | 5 | nav_msgs/Odometry curodom 6 | nav_msgs/Odometry kfodom 7 | uint32 kfid 8 | uint8 status 9 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Ubuntu 20/04 ROS:Foxy 2 | 使用方式： 3 | colcon build --symlink-install 4 | source 5 | ros2 launch ego_planner single_run_in_sim.launch.py 6 | 7 | 路径不要有中文（自定义消息有中文会出bug） 8 | 9 | # Humble版本 10 | 有位老哥fork了本工程做了22.04的https://github.com/Kaede-Rukawa/ego_planner_ros_humble 11 | 12 | ----------------- 13 | 只进行了这个单机demo的迁移，多机的没做迁移 14 | 15 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/__init__.py: -------------------------------------------------------------------------------- 1 | from ._Gains import * 2 | from ._SO3Command import * 3 | from ._TRPYCommand import * 4 | from ._PositionCommand import * 5 | from ._PPROutputData import * 6 | from ._OutputData import * 7 | from ._Corrections import * 8 | from ._Serial import * 9 | from ._AuxCommand import * 10 | from ._StatusData import * 11 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/msg/OutputData.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint16 loop_rate 3 | float64 voltage 4 | geometry_msgs/Quaternion orientation 5 | geometry_msgs/Vector3 angular_velocity 6 | geometry_msgs/Vector3 linear_acceleration 7 | float64 pressure_dheight 8 | float64 pressure_height 9 | geometry_msgs/Vector3 magnetic_field 10 | uint8[8] radio_channel 11 | #uint8[4] motor_rpm 12 | uint8 seq 13 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/msg/PPROutputData.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint16 quad_time 3 | float64 des_thrust 4 | float64 des_roll 5 | float64 des_pitch 6 | float64 des_yaw 7 | float64 est_roll 8 | float64 est_pitch 9 | float64 est_yaw 10 | float64 est_angvel_x 11 | float64 est_angvel_y 12 | float64 est_angvel_z 13 | float64 est_acc_x 14 | float64 est_acc_y 15 | float64 est_acc_z 16 | uint16[4] pwm 17 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg~: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Point position 3 | geometry_msgs/Vector3 velocity 4 | geometry_msgs/Vector3 acceleration 5 | float64 yaw 6 | float64 yaw_dot 7 | float64[3] kx 8 | float64[3] kv 9 | 10 | uint32 trajectory_id 11 | 12 | uint8 TRAJECTORY_STATUS_EMPTY = 0 13 | uint8 TRAJECTORY_STATUS_READY = 1 14 | uint8 TRAJECTORY_STATUS_COMPLETE = 3 15 | uint8 TRAJECTROY_STATUS_ABORT = 8 16 | 17 | uint8 trajectory_flag 18 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/msg/Serial.msg: -------------------------------------------------------------------------------- 1 | # Note: These constants need to be kept in sync with the types 2 | # defined in include/quadrotor_msgs/comm_types.h 3 | uint8 SO3_CMD = 115 # 's' in base 10 4 | uint8 TRPY_CMD = 112 # 'p' in base 10 5 | uint8 STATUS_DATA = 99 # 'c' in base 10 6 | uint8 OUTPUT_DATA = 100 # 'd' in base 10 7 | uint8 PPR_OUTPUT_DATA = 116 # 't' in base 10 8 | uint8 PPR_GAINS = 103 # 'g' 9 | 10 | Header header 11 | uint8 channel 12 | uint8 type # One of the types listed above 13 | uint8[] data 14 | -------------------------------------------------------------------------------- /src/uav_simulator/map_generator/.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Linux", 5 | "includePath": [ 6 | "${workspaceFolder}/**", 7 | "/usr/include/pcl-1.7", 8 | "/usr/include/eigen3", 9 | "/opt/ros/kinetic/include" 10 | ], 11 | "defines": [], 12 | "compilerPath": "/usr/bin/gcc", 13 | "cStandard": "c11", 14 | "cppStandard": "c++17" 15 | } 16 | ], 17 | "version": 4 18 | } -------------------------------------------------------------------------------- /src/uav_simulator/local_sensing/cfg/local_sensing_node.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "local_sensing_node" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("tx", double_t, 0, "tx", 0.0, -1000.0, 1000.0) 9 | gen.add("ty", double_t, 0, "ty", 0.0, -1000.0, 1000.0) 10 | gen.add("tz", double_t, 0, "tz", 0.0, -1000.0, 1000.0) 11 | gen.add("axis_x", double_t, 0, "axis_x", 0.0, -360.0, 360.0) 12 | gen.add("axis_y", double_t, 0, "axis_y", 0.0, -360.0, 360.0) 13 | gen.add("axis_z", double_t, 0, "axis_z", 0.0, -360.0, 360.0) 14 | 15 | exit(gen.generate(PACKAGE, "local_sensing_node", "local_sensing_node")) -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | geometry_msgs/Point position 3 | geometry_msgs/Vector3 velocity 4 | geometry_msgs/Vector3 acceleration 5 | float64 yaw 6 | float64 yaw_dot 7 | float64[3] kx 8 | float64[3] kv 9 | 10 | uint32 trajectory_id 11 | 12 | uint8 TRAJECTORY_STATUS_EMPTY = 0 13 | uint8 TRAJECTORY_STATUS_READY = 1 14 | uint8 TRAJECTORY_STATUS_COMPLETED = 3 15 | uint8 TRAJECTROY_STATUS_ABORT = 4 16 | uint8 TRAJECTORY_STATUS_ILLEGAL_START = 5 17 | uint8 TRAJECTORY_STATUS_ILLEGAL_FINAL = 6 18 | uint8 TRAJECTORY_STATUS_IMPOSSIBLE = 7 19 | 20 | # Its ID number will start from 1, allowing you comparing it with 0. 21 | uint8 trajectory_flag 22 | -------------------------------------------------------------------------------- /src/planner/new/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | new 5 | 0.0.0 6 | TODO: Package description 7 | suxin 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | quadrotor_msgs 16 | 17 | ament_cmake 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/planner/plan_manage/config/random_forest.yaml: -------------------------------------------------------------------------------- 1 | #TAG 注意 这个ID要保证跟节点的名字一样（rename就要跟rename的一样，rqt里面怎么显示，这里就得怎么弄） 2 | random_map_sensing: 3 | ros__parameters: 4 | map/x_size: 26.0 5 | map/y_size: 20.0 6 | map/z_size: 3.0 7 | map/resolution: 0.1 8 | ObstacleShape/seed: 1 9 | map/obs_num: 250 10 | ObstacleShape/lower_rad: 0.5 11 | ObstacleShape/upper_rad: 0.7 12 | ObstacleShape/lower_hei: 0.0 13 | ObstacleShape/upper_hei: 3.0 14 | map/circle_num: 250 15 | ObstacleShape/radius_l: 0.7 16 | ObstacleShape/radius_h: 0.5 17 | ObstacleShape/z_l: 0.7 18 | ObstacleShape/z_h: 0.8 19 | ObstacleShape/theta: 0.5 20 | pub_rate: 1.0 21 | min_distance: 0.8 22 | test_param: true -------------------------------------------------------------------------------- /src/uav_simulator/Utils/pose_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | pose_utils 3 | 0.0.0 4 | pose_utils 5 | Shaojie Shen 6 | BSD 7 | ament_cmake 8 | 9 | ament_lint_auto 10 | ament_lint_common 11 | 13 | 14 | 15 | ament_cmake 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg~: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # the trajectory id, starts from "1". 4 | uint32 trajectory_id 5 | 6 | # the action command for trajectory server. 7 | uint32 ACTION_ADD = 1 8 | uint32 ACTION_ABORT = 2 9 | uint32 ACTION_WARN_START = 3 10 | uint32 ACTION_WARN_FINAL = 4 11 | uint32 ACTION_WARN_IMPOSSIBLE = 5 12 | uint32 action 13 | 14 | # the order of trajectory. 15 | uint32 num_order 16 | uint32 num_segment 17 | 18 | # the polynomial coecfficients of the trajectory. 19 | float64 start_yaw 20 | float64 final_yaw 21 | float64[] coef_x 22 | float64[] coef_y 23 | float64[] coef_z 24 | float64[] time 25 | float64 mag_coeff 26 | string debug_info 27 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/odom_visualization/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b odom_visualization is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/msg/LQRTrajectory.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # the trajectory id, starts from "1". 4 | uint32 trajectory_id 5 | 6 | # the action command for trajectory server. 7 | uint32 ACTION_ADD = 1 8 | uint32 ACTION_ABORT = 2 9 | uint32 ACTION_WARN_START = 3 10 | uint32 ACTION_WARN_FINAL = 4 11 | uint32 ACTION_WARN_IMPOSSIBLE = 5 12 | uint32 action 13 | 14 | # the weight coefficient of the control effort 15 | float64 r 16 | 17 | # the yaw command 18 | float64 start_yaw 19 | float64 final_yaw 20 | 21 | # the initial and final state 22 | float64[6] s0 23 | float64[3] ut 24 | 25 | float64[6] sf 26 | 27 | # the optimal arrival time 28 | float64 t_f 29 | 30 | string debug_info 31 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # the trajectory id, starts from "1". 4 | uint32 trajectory_id 5 | 6 | # the action command for trajectory server. 7 | uint32 ACTION_ADD = 1 8 | uint32 ACTION_ABORT = 2 9 | uint32 ACTION_WARN_START = 3 10 | uint32 ACTION_WARN_FINAL = 4 11 | uint32 ACTION_WARN_IMPOSSIBLE = 5 12 | uint32 action 13 | 14 | # the order of trajectory. 15 | uint32 num_order 16 | uint32 num_segment 17 | 18 | # the polynomial coecfficients of the trajectory. 19 | float64 start_yaw 20 | float64 final_yaw 21 | float64[] coef_x 22 | float64[] coef_y 23 | float64[] coef_z 24 | float64[] time 25 | float64 mag_coeff 26 | uint32[] order 27 | 28 | string debug_info 29 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/encode_msgs.h: -------------------------------------------------------------------------------- 1 | #ifndef __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ 2 | #define __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace quadrotor_msgs 11 | { 12 | 13 | void encodeSO3Command(const quadrotor_msgs::SO3Command &so3_command, 14 | std::vector &output); 15 | void encodeTRPYCommand(const quadrotor_msgs::TRPYCommand &trpy_command, 16 | std::vector &output); 17 | 18 | void encodePPRGains(const quadrotor_msgs::Gains &gains, 19 | std::vector &output); 20 | } 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | quadrotor_msgs 3 | 0.0.0 4 | quadrotor_msgs 5 | Kartik Mohta 6 | http://ros.org/wiki/quadrotor_msgs 7 | BSD 8 | ament_cmake 9 | 10 | rosidl_default_generators 11 | rosidl_default_runtime 12 | rosidl_interface_packages 13 | 14 | 15 | 16 | ament_lint_auto 17 | ament_lint_common 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /src/uav_simulator/local_sensing/src/csv_convert.py: -------------------------------------------------------------------------------- 1 | import csv 2 | file_location = '/home/wang/bag/banchmark/EuRoC/ViconRoom101/state_groundtruth_estimate0/data.csv' 3 | with open('/home/wang/bag/banchmark/EuRoC/ViconRoom101/state_groundtruth_estimate0/data.txt', 'w') as txt_f: 4 | with open(file_location) as f: 5 | f_csv = csv.reader(f) 6 | headers = next(f_csv) 7 | for row in f_csv: 8 | txt_f.write('%lf\n'% (float(row[0]) / 1000000000.0) ) 9 | txt_f.write('%lf\n'% (float(row[1])) ) 10 | txt_f.write('%lf\n'% (float(row[2])) ) 11 | txt_f.write('%lf\n'% (float(row[3])) ) 12 | txt_f.write('%lf\n'% (float(row[4])) ) 13 | txt_f.write('%lf\n'% (float(row[5])) ) 14 | txt_f.write('%lf\n'% (float(row[6])) ) 15 | txt_f.write('%lf\n'% (float(row[7])) ) -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/decode_msgs.h: -------------------------------------------------------------------------------- 1 | #ifndef __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ 2 | #define __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace quadrotor_msgs 11 | { 12 | 13 | bool decodeOutputData(const std::vector &data, 14 | quadrotor_msgs::OutputData &output); 15 | 16 | bool decodeStatusData(const std::vector &data, 17 | quadrotor_msgs::StatusData &status); 18 | 19 | bool decodePPROutputData(const std::vector &data, 20 | quadrotor_msgs::PPROutputData &output); 21 | } 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/odom_visualization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 0.0.0 3 | odom_visualization 4 | 5 | 6 | odom_visualization 7 | 8 | 9 | Shaojie Shen 10 | BSD 11 | http://ros.org/wiki/odom_visualization 12 | 13 | ament_cmake 14 | 15 | ament_lint_auto 16 | ament_lint_common 17 | quadrotor_msgs 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /src/planner/bridge_test/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | bridge_test 5 | 0.0.0 6 | TODO: Package description 7 | weibw 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | rclcpp 12 | custom_msgs 13 | 14 | ament_lint_auto 15 | ament_lint_common 16 | traj_utils 17 | quadrotor_msgs 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /src/planner/bridge_test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(bridge_test) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(traj_utils REQUIRED) 22 | find_package(quadrotor_msgs REQUIRED) 23 | 24 | add_executable(subscriber src/test.cpp) 25 | ament_target_dependencies(subscriber rclcpp traj_utils quadrotor_msgs) 26 | add_executable(subscriber_bspline src/test1.cpp) 27 | ament_target_dependencies(subscriber_bspline rclcpp traj_utils ) 28 | install(TARGETS 29 | subscriber 30 | subscriber_bspline 31 | DESTINATION lib/${PROJECT_NAME}) 32 | 33 | ament_package() 34 | -------------------------------------------------------------------------------- /src/uav_simulator/local_sensing/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | local_sensing_node 4 | 0.1.0 5 | 6 | render depth from depth 7 | 8 | Matia Pizzoli 9 | Matia Pizzoli 10 | GPLv3 11 | 12 | 13 | 14 | 15 | 16 | ament_cmake 17 | 18 | ament_lint_auto 19 | ament_lint_common 20 | quadrotor_msgs 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /src/planner/bridge_test/src/test1.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "rclcpp/rclcpp.hpp" 4 | #include "traj_utils/msg/bspline.hpp" 5 | using std::placeholders::_1; 6 | 7 | class MinimalSubscriber : public rclcpp::Node 8 | { 9 | public: 10 | MinimalSubscriber() 11 | : Node("minimal_subscriber") 12 | { 13 | subscription_ = this->create_subscription( 14 | "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); 15 | } 16 | 17 | private: 18 | void topic_callback(const traj_utils::msg::Bspline::SharedPtr msg) const 19 | { 20 | // RCLCPP_INFO(this->get_logger(), "I heard custom_msgs::msg::TestFoxy : '%d'", msg->position.x); 21 | std::cout<drone_id<::SharedPtr subscription_; 24 | }; 25 | 26 | int main(int argc, char * argv[]) 27 | { 28 | rclcpp::init(argc, argv); 29 | rclcpp::spin(std::make_shared()); 30 | rclcpp::shutdown(); 31 | return 0; 32 | } 33 | -------------------------------------------------------------------------------- /src/planner/bridge_test/src/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "rclcpp/rclcpp.hpp" 4 | #include "quadrotor_msgs/msg/position_command.hpp" 5 | using std::placeholders::_1; 6 | 7 | class MinimalSubscriber : public rclcpp::Node 8 | { 9 | public: 10 | MinimalSubscriber() 11 | : Node("minimal_subscriber") 12 | { 13 | subscription_ = this->create_subscription( 14 | "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); 15 | } 16 | 17 | private: 18 | void topic_callback(const quadrotor_msgs::msg::PositionCommand::SharedPtr msg) const 19 | { 20 | // RCLCPP_INFO(this->get_logger(), "I heard custom_msgs::msg::TestFoxy : '%d'", msg->position.x); 21 | std::cout<position.x<::SharedPtr subscription_; 24 | }; 25 | 26 | int main(int argc, char * argv[]) 27 | { 28 | rclcpp::init(argc, argv); 29 | rclcpp::spin(std::make_shared()); 30 | rclcpp::shutdown(); 31 | return 0; 32 | } 33 | -------------------------------------------------------------------------------- /src/uav_simulator/local_sensing/src/depth_render.cuh: -------------------------------------------------------------------------------- 1 | #ifndef DEPTH_RENDER_CUH 2 | #define DEPTH_RENDER_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | //#include 11 | 12 | #include "device_image.cuh" 13 | #include "cuda_exception.cuh" 14 | #include "helper_math.h" 15 | 16 | using namespace std; 17 | //using namespace Eigen; 18 | 19 | struct Parameter 20 | { 21 | int point_number; 22 | float fx, fy, cx, cy; 23 | int width, height; 24 | float r[3][3]; 25 | float t[3]; 26 | }; 27 | 28 | class DepthRender 29 | { 30 | public: 31 | DepthRender(); 32 | void set_para(float _fx, float _fy, float _cx, float _cy, int _width, int _height); 33 | ~DepthRender(); 34 | void set_data(vector &cloud_data); 35 | //void render_pose( Matrix3d &rotation, Vector3d &translation, int *host_ptr); 36 | //void render_pose( Matrix4d &transformation, int *host_ptr); 37 | void render_pose( double * transformation, int *host_ptr); 38 | 39 | private: 40 | int cloud_size; 41 | 42 | //data 43 | float3 *host_cloud_ptr; 44 | float3 *dev_cloud_ptr; 45 | bool has_devptr; 46 | 47 | //camera 48 | Parameter parameter; 49 | Parameter* parameter_devptr; 50 | }; 51 | 52 | #endif -------------------------------------------------------------------------------- /src/uav_simulator/Utils/pose_utils/include/pose_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef POSE_UTILS_H 2 | #define POSE_UTILS_H 3 | 4 | #include 5 | #include "armadillo" 6 | 7 | #define PI 3.14159265359 8 | #define NUM_INF 999999.9 9 | 10 | using namespace arma; 11 | using namespace std; 12 | 13 | // Rotation --------------------- 14 | mat ypr_to_R(const colvec& ypr); 15 | 16 | mat yaw_to_R(double yaw); 17 | 18 | colvec R_to_ypr(const mat& R); 19 | 20 | mat quaternion_to_R(const colvec& q); 21 | 22 | colvec R_to_quaternion(const mat& R); 23 | 24 | colvec quaternion_mul(const colvec& q1, const colvec& q2); 25 | 26 | colvec quaternion_inv(const colvec& q); 27 | 28 | // General Pose Update ---------- 29 | colvec pose_update(const colvec& X1, const colvec& X2); 30 | 31 | colvec pose_inverse(const colvec& X); 32 | 33 | colvec pose_update_2d(const colvec& X1, const colvec& X2); 34 | 35 | colvec pose_inverse_2d(const colvec& X); 36 | 37 | // For Pose EKF ----------------- 38 | mat Jplus1(const colvec& X1, const colvec& X2); 39 | 40 | mat Jplus2(const colvec& X1, const colvec& X2); 41 | 42 | // For IMU EKF ------------------ 43 | colvec state_update(const colvec& X, const colvec& U, double dt); 44 | 45 | mat jacobianF(const colvec& X, const colvec& U, double dt); 46 | 47 | mat jacobianU(const colvec& X, const colvec& U, double dt); 48 | 49 | colvec state_measure(const colvec& X); 50 | 51 | mat jacobianH(); 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /src/planner/bspline_opt/include/bspline_opt/gradient_descent_optimizer.h: -------------------------------------------------------------------------------- 1 | #ifndef _GRADIENT_DESCENT_OPT_H_ 2 | #define _GRADIENT_DESCENT_OPT_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | 10 | class GradientDescentOptimizer 11 | { 12 | 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 15 | 16 | typedef double (*objfunDef)(const Eigen::VectorXd &x, Eigen::VectorXd &grad, bool &force_return, void *data); 17 | enum RESULT 18 | { 19 | FIND_MIN, 20 | FAILED, 21 | RETURN_BY_ORDER, 22 | REACH_MAX_ITERATION 23 | }; 24 | 25 | GradientDescentOptimizer(int v_num, objfunDef objf, void *f_data) 26 | { 27 | variable_num_ = v_num; 28 | objfun_ = objf; 29 | f_data_ = f_data; 30 | }; 31 | 32 | void set_maxiter(int limit) { iter_limit_ = limit; } 33 | void set_maxeval(int limit) { invoke_limit_ = limit; } 34 | void set_xtol_rel(double xtol_rel) { xtol_rel_ = xtol_rel; } 35 | void set_xtol_abs(double xtol_abs) { xtol_abs_ = xtol_abs; } 36 | void set_min_grad(double min_grad) { min_grad_ = min_grad; } 37 | 38 | RESULT optimize(Eigen::VectorXd &x_init_optimal, double &opt_f); 39 | 40 | private: 41 | int variable_num_{0}; 42 | int iter_limit_{1e10}; 43 | int invoke_limit_{1e10}; 44 | double xtol_rel_; 45 | double xtol_abs_; 46 | double min_grad_; 47 | double time_limit_; 48 | void *f_data_; 49 | objfunDef objfun_; 50 | }; 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /src/planner/plan_manage/src/ego_planner_node.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | #include 3 | 4 | #include 5 | 6 | using namespace ego_planner; 7 | 8 | int main(int argc, char **argv) 9 | { 10 | 11 | // ros::init(argc, argv, "ego_planner_node"); 12 | // ros::NodeHandle nh("~"); 13 | rclcpp::init(argc, argv); 14 | auto nh = std::make_shared("ego_planner_node");//这个只是说节点的名字叫这个 15 | EGOReplanFSM rebo_replan; 16 | 17 | rebo_replan.init(nh); 18 | 19 | // ros::Duration(1.0).sleep(); 20 | // ros::spin(); 21 | rclcpp::spin(nh); 22 | 23 | return 0; 24 | } 25 | 26 | // #include "rclcpp/rclcpp.hpp" 27 | // #include 28 | // #include 29 | 30 | // #include 31 | 32 | // using namespace ego_planner; 33 | 34 | // void SignalHandler(int signal) { 35 | // if(ros::isInitialized() && ros::isStarted() && rclcpp::ok() && !ros::isShuttingDown()){ 36 | // ros::shutdown(); 37 | // } 38 | // } 39 | 40 | // int main(int argc, char **argv) { 41 | 42 | // signal(SIGINT, SignalHandler); 43 | // signal(SIGTERM,SignalHandler); 44 | 45 | // ros::init(argc, argv, "ego_planner_node", ros::init_options::NoSigintHandler); 46 | // ros::NodeHandle nh("~"); 47 | 48 | // EGOReplanFSM rebo_replan; 49 | 50 | // rebo_replan.init(nh); 51 | 52 | // // ros::Duration(1.0).sleep(); 53 | // ros::AsyncSpinner async_spinner(4); 54 | // async_spinner.start(); 55 | // ros::waitForShutdown(); 56 | 57 | // return 0; 58 | // } -------------------------------------------------------------------------------- /src/planner/plan_env/include/plan_env/raycast.h: -------------------------------------------------------------------------------- 1 | #ifndef RAYCAST_H_ 2 | #define RAYCAST_H_ 3 | 4 | #include 5 | #include 6 | 7 | double signum(double x); 8 | 9 | double mod(double value, double modulus); 10 | 11 | double intbound(double s, double ds); 12 | 13 | void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min, 14 | const Eigen::Vector3d& max, int& output_points_cnt, Eigen::Vector3d* output); 15 | 16 | void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min, 17 | const Eigen::Vector3d& max, std::vector* output); 18 | 19 | class RayCaster { 20 | private: 21 | /* data */ 22 | Eigen::Vector3d start_; 23 | Eigen::Vector3d end_; 24 | Eigen::Vector3d direction_; 25 | Eigen::Vector3d min_; 26 | Eigen::Vector3d max_; 27 | int x_; 28 | int y_; 29 | int z_; 30 | int endX_; 31 | int endY_; 32 | int endZ_; 33 | double maxDist_; 34 | double dx_; 35 | double dy_; 36 | double dz_; 37 | int stepX_; 38 | int stepY_; 39 | int stepZ_; 40 | double tMaxX_; 41 | double tMaxY_; 42 | double tMaxZ_; 43 | double tDeltaX_; 44 | double tDeltaY_; 45 | double tDeltaZ_; 46 | double dist_; 47 | 48 | int step_num_; 49 | 50 | public: 51 | RayCaster(/* args */) { 52 | } 53 | ~RayCaster() { 54 | } 55 | 56 | bool setInput(const Eigen::Vector3d& start, 57 | const Eigen::Vector3d& end /* , const Eigen::Vector3d& min, 58 | const Eigen::Vector3d& max */); 59 | 60 | bool step(Eigen::Vector3d& ray_pt); 61 | }; 62 | 63 | #endif // RAYCAST_H_ -------------------------------------------------------------------------------- /src/planner/new/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(new) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | # uncomment the following section in order to fill in 21 | # further dependencies manually. 22 | # find_package( REQUIRED) 23 | find_package(rclcpp REQUIRED) 24 | #EXPL 1.那边编译好以后这边只需要find_package即可 25 | find_package(quadrotor_msgs REQUIRED) 26 | add_executable(testpub src/testpub.cpp) 27 | ament_target_dependencies(testpub rclcpp quadrotor_msgs) 28 | # rosidl_target_interfaces(testpub 29 | # ${PROJECT_NAME} "rosidl_typesupport_cpp") 30 | # rosidl_get_typesupport_target(cpp_typesupport_target 31 | # ${PROJECT_NAME} rosidl_typesupport_cpp) 32 | 33 | # target_link_libraries(testpub "${cpp_typesupport_target}") 34 | 35 | install(TARGETS 36 | testpub 37 | DESTINATION lib/${PROJECT_NAME}) 38 | 39 | 40 | 41 | 42 | 43 | 44 | if(BUILD_TESTING) 45 | find_package(ament_lint_auto REQUIRED) 46 | # the following line skips the linter which checks for copyrights 47 | # uncomment the line when a copyright and license is not present in all source files 48 | #set(ament_cmake_copyright_FOUND TRUE) 49 | # the following line skips cpplint (only works in a git repo) 50 | # uncomment the line when this package is not in a git repo 51 | #set(ament_cmake_cpplint_FOUND TRUE) 52 | ament_lint_auto_find_test_dependencies() 53 | endif() 54 | 55 | ament_package() 56 | -------------------------------------------------------------------------------- /src/uav_simulator/local_sensing/src/cuda_exception.cuh: -------------------------------------------------------------------------------- 1 | // This file is part of REMODE - REgularized MOnocular Depth Estimation. 2 | // 3 | // Copyright (C) 2014 Matia Pizzoli 4 | // Robotics and Perception Group, University of Zurich, Switzerland 5 | // http://rpg.ifi.uzh.ch 6 | // 7 | // REMODE is free software: you can redistribute it and/or modify it under the 8 | // terms of the GNU General Public License as published by the Free Software 9 | // Foundation, either version 3 of the License, or any later version. 10 | // 11 | // REMODE is distributed in the hope that it will be useful, but WITHOUT ANY 12 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 14 | // 15 | // You should have received a copy of the GNU General Public License 16 | // along with this program. If not, see . 17 | 18 | #ifndef RMD_CUDA_EXCEPTION_CUH_ 19 | #define RMD_CUDA_EXCEPTION_CUH_ 20 | 21 | #include 22 | #include 23 | 24 | 25 | struct CudaException : public std::exception 26 | { 27 | CudaException(const std::string& what, cudaError err) 28 | : what_(what), err_(err) {} 29 | virtual ~CudaException() throw() {} 30 | virtual const char* what() const throw() 31 | { 32 | std::stringstream description; 33 | description << "CudaException: " << what_ << std::endl; 34 | if(err_ != cudaSuccess) 35 | { 36 | description << "cudaError code: " << cudaGetErrorString(err_); 37 | description << " (" << err_ << ")" << std::endl; 38 | } 39 | return description.str().c_str(); 40 | } 41 | std::string what_; 42 | cudaError err_; 43 | }; 44 | 45 | #endif /* RMD_CUDA_EXCEPTION_CUH_ */ 46 | -------------------------------------------------------------------------------- /src/planner/new/src/testpub.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | #include "quadrotor_msgs/msg/position_command.hpp" //包含我们创建的msg文件 3 | 4 | using namespace std::chrono_literals; 5 | 6 | class AddressBookPublisher : public rclcpp::Node 7 | { 8 | public: 9 | AddressBookPublisher() 10 | : Node("address_book_publisher") 11 | { 12 | address_book_publisher_ = 13 | this->create_publisher("address_book", 10); 14 | //生成一个节点address_book_publisher以及话题发布者 AddressBookPublisher 15 | 16 | auto publish_msg = [this]() -> void { 17 | quadrotor_msgs::msg::PositionCommand message ;//创建一个消息 AddressBook稍后发送 18 | message.header.frame_id=11; 19 | message.position.x=1; 20 | // message.first_name = "John"; 21 | // message.last_name = "Doe"; 22 | // message.phone_number = "1234567890"; 23 | // message.phone_type = message.PHONE_TYPE_MOBILE; 24 | //定期发送消息 25 | // std::cout << "Publishing Contact\nFirst:" << message.first_name << 26 | // " Last:" << message.last_name << std::endl; 27 | 28 | this->address_book_publisher_->publish(message);//发布消息到话题 29 | }; 30 | timer_ = this->create_wall_timer(1s, publish_msg);//创建一个定时器，每秒调用一次publish_msg 31 | } 32 | 33 | private: 34 | rclcpp::Publisher::SharedPtr address_book_publisher_; 35 | //声明了一个指向 rclcpp::Publisher 类模板实例的共享指针 ，用于将消息发布到主题。 36 | //more_interfaces::msg::AddressBook 模板参数指定将要发布的消息类型。SharedPtr 类型是一个智能指针，提供了对象的共享所有权。 37 | rclcpp::TimerBase::SharedPtr timer_; 38 | //声明一个指向rclcpp::TimerBase类的共享指针，用于创建定时器，指定时间间隔后调用回调函数。 39 | }; 40 | 41 | 42 | int main(int argc, char * argv[]) 43 | { 44 | rclcpp::init(argc, argv);//初始化 45 | rclcpp::spin(std::make_shared());//运行 46 | rclcpp::shutdown();//清理ros2客户端库以及中间件资源 47 | 48 | return 0; 49 | } -------------------------------------------------------------------------------- /src/uav_simulator/Utils/pose_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(pose_utils) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-O3 -Wall -g") 6 | ADD_COMPILE_OPTIONS(-std=c++11 ) 7 | ADD_COMPILE_OPTIONS(-std=c++14 ) 8 | 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | 12 | # find_package(catkin REQUIRED COMPONENTS 13 | # #armadillo 14 | # roscpp 15 | # ) 16 | set(MY_DEPENDENCIES 17 | rclcpp 18 | # PCL 19 | ) 20 | # catkin_package( 21 | # INCLUDE_DIRS include 22 | # LIBRARIES pose_utils 23 | # # CATKIN_DEPENDS geometry_msgs nav_msgs 24 | # # DEPENDS system_lib 25 | # ) 26 | 27 | find_package(Armadillo REQUIRED) 28 | 29 | include_directories( 30 | # ${catkin_INCLUDE_DIRS} 31 | ${ARMADILLO_INCLUDE_DIRS} 32 | include 33 | ) 34 | 35 | add_library(pose_utils SHARED 36 | ${ARMADILLO_LIBRARIES} 37 | src/pose_utils.cpp) 38 | ament_target_dependencies(pose_utils 39 | ${MY_DEPENDENCIES} 40 | ) 41 | install(TARGETS 42 | pose_utils 43 | DESTINATION lib/${PROJECT_NAME} 44 | ) 45 | # install( 46 | # TARGETS bspline_opt 47 | # ARCHIVE DESTINATION lib 48 | # LIBRARY DESTINATION lib 49 | # RUNTIME DESTINATION bin 50 | # ) 51 | # install(DIRECTORY launch 52 | # DESTINATION share/${PROJECT_NAME} 53 | # ) 54 | # install(DIRECTORY config 55 | # DESTINATION share/${PROJECT_NAME}/#把存yaml文件的文件夹config移到share 56 | # ) 57 | if(BUILD_TESTING) 58 | find_package(ament_lint_auto REQUIRED) 59 | # the following line skips the linter which checks for copyrights 60 | # uncomment the line when a copyright and license is not present in all source files 61 | #set(ament_cmake_copyright_FOUND TRUE) 62 | # the following line skips cpplint (only works in a git repo) 63 | # uncomment the line when this package is not in a git repo 64 | #set(ament_cmake_cpplint_FOUND TRUE) 65 | ament_lint_auto_find_test_dependencies() 66 | endif() 67 | ##1.自身工程的.h库 68 | ament_package() -------------------------------------------------------------------------------- /src/uav_simulator/fake_drone/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(poscmd_2_odom) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | #set(CMAKE_CXX_FLAGS "-std=c++11") 6 | ADD_COMPILE_OPTIONS(-std=c++11 ) 7 | ADD_COMPILE_OPTIONS(-std=c++14 ) 8 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 9 | 10 | find_package(ament_cmake REQUIRED) 11 | find_package(rclcpp REQUIRED) 12 | find_package(nav_msgs REQUIRED) 13 | find_package(quadrotor_msgs REQUIRED) 14 | set(MY_DEPENDENCIES 15 | rclcpp 16 | nav_msgs 17 | quadrotor_msgs 18 | # PCL 19 | ) 20 | # find_package(nav_msgs REQUIRED) 21 | # find_package(catkin REQUIRED COMPONENTS 22 | # roscpp 23 | # rospy 24 | # nav_msgs 25 | # quadrotor_msgs 26 | # ) 27 | find_package(Eigen3 REQUIRED) 28 | 29 | # catkin_package() 30 | 31 | include_directories( 32 | # include 33 | # ${catkin_INCLUDE_DIRS} 34 | ${EIGEN3_INCLUDE_DIR} 35 | ) 36 | 37 | # add_executable (poscmd_2_odom src/poscmd_2_odom.cpp ) 38 | # target_link_libraries(poscmd_2_odom 39 | # ${catkin_LIBRARIES}) 40 | 41 | add_executable (poscmd_2_odom src/poscmd_2_odom.cpp ) 42 | ament_target_dependencies(poscmd_2_odom 43 | ${MY_DEPENDENCIES} 44 | ) 45 | 46 | install(TARGETS 47 | poscmd_2_odom 48 | DESTINATION lib/${PROJECT_NAME} 49 | ) 50 | # install(DIRECTORY launch 51 | # DESTINATION share/${PROJECT_NAME} 52 | # ) 53 | # install(DIRECTORY config 54 | # DESTINATION share/${PROJECT_NAME}/#把存yaml文件的文件夹config移到share 55 | # ) 56 | if(BUILD_TESTING) 57 | find_package(ament_lint_auto REQUIRED) 58 | # the following line skips the linter which checks for copyrights 59 | # uncomment the line when a copyright and license is not present in all source files 60 | #set(ament_cmake_copyright_FOUND TRUE) 61 | # the following line skips cpplint (only works in a git repo) 62 | # uncomment the line when this package is not in a git repo 63 | #set(ament_cmake_cpplint_FOUND TRUE) 64 | ament_lint_auto_find_test_dependencies() 65 | endif() 66 | ##1.自身工程的.h库 67 | ament_package() -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/comm_types.h: -------------------------------------------------------------------------------- 1 | #ifndef __QUADROTOR_MSGS_COMM_TYPES_H__ 2 | #define __QUADROTOR_MSGS_COMM_TYPES_H__ 3 | 4 | #define TYPE_SO3_CMD 's' 5 | struct SO3_CMD_INPUT 6 | { 7 | // Scaling factors when decoding 8 | int16_t force[3]; // /500 9 | int8_t des_qx, des_qy, des_qz, des_qw; // /125 10 | uint8_t kR[3]; // /50 11 | uint8_t kOm[3]; // /100 12 | int16_t cur_yaw; // /1e4 13 | int16_t kf_correction; // /1e11; 14 | uint8_t angle_corrections[2]; // roll,pitch /2500 15 | uint8_t enable_motors:1; 16 | uint8_t use_external_yaw:1; 17 | uint8_t seq; 18 | }; 19 | 20 | #define TYPE_STATUS_DATA 'c' 21 | struct STATUS_DATA 22 | { 23 | uint16_t loop_rate; 24 | uint16_t voltage; 25 | uint8_t seq; 26 | }; 27 | 28 | #define TYPE_OUTPUT_DATA 'd' 29 | struct OUTPUT_DATA 30 | { 31 | uint16_t loop_rate; 32 | uint16_t voltage; 33 | int16_t roll, pitch, yaw; 34 | int16_t ang_vel[3]; 35 | int16_t acc[3]; 36 | int16_t dheight; 37 | int32_t height; 38 | int16_t mag[3]; 39 | uint8_t radio[8]; 40 | //uint8_t rpm[4]; 41 | uint8_t seq; 42 | }; 43 | 44 | #define TYPE_TRPY_CMD 'p' 45 | struct TRPY_CMD 46 | { 47 | int16_t thrust; 48 | int16_t roll; 49 | int16_t pitch; 50 | int16_t yaw; 51 | int16_t current_yaw; 52 | uint8_t enable_motors:1; 53 | uint8_t use_external_yaw:1; 54 | }; 55 | 56 | #define TYPE_PPR_OUTPUT_DATA 't' 57 | struct PPR_OUTPUT_DATA 58 | { 59 | uint16_t time; 60 | int16_t des_thrust; 61 | int16_t des_roll; 62 | int16_t des_pitch; 63 | int16_t des_yaw; 64 | int16_t est_roll; 65 | int16_t est_pitch; 66 | int16_t est_yaw; 67 | int16_t est_angvel_x; 68 | int16_t est_angvel_y; 69 | int16_t est_angvel_z; 70 | int16_t est_acc_x; 71 | int16_t est_acc_y; 72 | int16_t est_acc_z; 73 | uint16_t pwm1; 74 | uint16_t pwm2; 75 | uint16_t pwm3; 76 | uint16_t pwm4; 77 | }; 78 | 79 | #define TYPE_PPR_GAINS 'g' 80 | struct PPR_GAINS 81 | { 82 | int16_t Kp; 83 | int16_t Kd; 84 | int16_t Kp_yaw; 85 | int16_t Kd_yaw; 86 | }; 87 | #endif 88 | -------------------------------------------------------------------------------- /src/uav_simulator/map_generator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(map_generator) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | #set(CMAKE_CXX_FLAGS "-std=c++11") 6 | ADD_COMPILE_OPTIONS(-std=c++11 ) 7 | ADD_COMPILE_OPTIONS(-std=c++14 ) 8 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 9 | 10 | 11 | find_package(ament_cmake REQUIRED) 12 | find_package(rclcpp REQUIRED) 13 | find_package(std_msgs REQUIRED) 14 | find_package(geometry_msgs REQUIRED) 15 | find_package(nav_msgs REQUIRED) 16 | find_package(pcl_conversions REQUIRED) 17 | 18 | set(MY_DEPENDENCIES 19 | rclcpp 20 | std_msgs 21 | geometry_msgs 22 | nav_msgs 23 | pcl_conversions 24 | # PCL 25 | ) 26 | find_package(PCL REQUIRED) 27 | find_package(Eigen3 REQUIRED) 28 | # find_package(catkin REQUIRED COMPONENTS 29 | # roscpp 30 | # rospy 31 | # std_msgs 32 | # geometry_msgs 33 | # pcl_conversions 34 | # ) 35 | 36 | 37 | # catkin_package() 38 | 39 | include_directories( 40 | # include 41 | # ${catkin_INCLUDE_DIRS} 42 | # ${EIGEN3_INCLUDE_DIR} 43 | # ${PCL_INCLUDE_DIRS} 44 | ) 45 | 46 | add_executable (random_forest src/random_forest_sensing.cpp ) 47 | ament_target_dependencies(random_forest 48 | ${MY_DEPENDENCIES} 49 | ) 50 | target_link_libraries(random_forest 51 | 52 | ${PCL_LIBRARIES}) 53 | install(TARGETS 54 | random_forest 55 | DESTINATION lib/${PROJECT_NAME} 56 | ) 57 | # install(DIRECTORY launch 58 | # DESTINATION share/${PROJECT_NAME} 59 | # ) 60 | # install(DIRECTORY config 61 | # DESTINATION share/${PROJECT_NAME}/#把存yaml文件的文件夹config移到share 62 | # ) 63 | if(BUILD_TESTING) 64 | find_package(ament_lint_auto REQUIRED) 65 | # the following line skips the linter which checks for copyrights 66 | # uncomment the line when a copyright and license is not present in all source files 67 | #set(ament_cmake_copyright_FOUND TRUE) 68 | # the following line skips cpplint (only works in a git repo) 69 | # uncomment the line when this package is not in a git repo 70 | #set(ament_cmake_cpplint_FOUND TRUE) 71 | ament_lint_auto_find_test_dependencies() 72 | endif() 73 | ##1.自身工程的.h库 74 | ament_package() -------------------------------------------------------------------------------- /src/planner/path_searching/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(path_searching) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | ADD_COMPILE_OPTIONS(-std=c++11 ) 6 | ADD_COMPILE_OPTIONS(-std=c++14 ) 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 8 | 9 | 10 | 11 | find_package(ament_cmake REQUIRED) 12 | find_package(rclcpp REQUIRED) 13 | find_package(std_msgs REQUIRED) 14 | find_package(visualization_msgs REQUIRED) 15 | # find_package(plan_env REQUIRED) 16 | find_package(cv_bridge REQUIRED) 17 | find_package(Eigen3 REQUIRED) 18 | set(MY_DEPENDENCIES 19 | rclcpp 20 | std_msgs 21 | visualization_msgs 22 | # plan_env 23 | cv_bridge 24 | Eigen3 25 | # PCL 26 | ) 27 | 28 | find_package(PCL 1.7 REQUIRED) 29 | 30 | # catkin_package( 31 | # INCLUDE_DIRS include 32 | # LIBRARIES path_searching 33 | # CATKIN_DEPENDS plan_env 34 | # # DEPENDS system_lib 35 | # ) 36 | #TAG 自制的没有main函数的几个cpp编译成库文件，然后如何被工程内的其他功能包使用，1.编译add_library编译好 2.这边引用那边的时候，在include_directories键入那边的src源文件路径即可 37 | get_filename_component(PROJECT_ROOT "${CMAKE_CURRENT_SOURCE_DIR}" DIRECTORY) 38 | include_directories( 39 | SYSTEM 40 | include 41 | # ${catkin_INCLUDE_DIRS} 42 | ${Eigen3_INCLUDE_DIRS} 43 | ${PCL_INCLUDE_DIRS} 44 | ${PROJECT_ROOT}/plan_env/include 45 | ) 46 | 47 | add_library( path_searching SHARED 48 | src/dyn_a_star.cpp 49 | ) 50 | ament_target_dependencies(path_searching 51 | ${MY_DEPENDENCIES} 52 | ) 53 | 54 | 55 | # target_include_directories(path_searching PRIVATE "/home/suxin/English_Path/NEW_WORK_PATH/MY_FINAL_PJ/ROS2_ego_planner/src/planner/plan_env/include") 56 | install(TARGETS 57 | path_searching 58 | DESTINATION lib/${PROJECT_NAME} 59 | ) 60 | # install(DIRECTORY launch 61 | # DESTINATION share/${PROJECT_NAME} 62 | # ) 63 | # install(DIRECTORY config 64 | # DESTINATION share/${PROJECT_NAME}/#把存yaml文件的文件夹config移到share 65 | # ) 66 | if(BUILD_TESTING) 67 | find_package(ament_lint_auto REQUIRED) 68 | # the following line skips the linter which checks for copyrights 69 | # uncomment the line when a copyright and license is not present in all source files 70 | #set(ament_cmake_copyright_FOUND TRUE) 71 | # the following line skips cpplint (only works in a git repo) 72 | # uncomment the line when this package is not in a git repo 73 | #set(ament_cmake_cpplint_FOUND TRUE) 74 | ament_lint_auto_find_test_dependencies() 75 | endif() 76 | ##1.自身工程的.h库 77 | ament_package() -------------------------------------------------------------------------------- /src/planner/bspline_opt/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(bspline_opt) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | ADD_COMPILE_OPTIONS(-std=c++11 ) 6 | ADD_COMPILE_OPTIONS(-std=c++14 ) 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 8 | 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(std_msgs REQUIRED) 12 | find_package(visualization_msgs REQUIRED) 13 | # find_package(plan_env REQUIRED) 14 | find_package(cv_bridge REQUIRED) 15 | # find_package(traj_utils REQUIRED) 16 | # find_package(path_searching REQUIRED) 17 | find_package(Eigen3 REQUIRED) 18 | set(MY_DEPENDENCIES 19 | rclcpp 20 | std_msgs 21 | visualization_msgs 22 | # plan_env 23 | cv_bridge 24 | # traj_utils 25 | # path_searching 26 | # PCL 27 | Eigen3 28 | ) 29 | 30 | find_package(PCL 1.7 REQUIRED) 31 | 32 | # catkin_package( 33 | # INCLUDE_DIRS include 34 | # LIBRARIES bspline_opt 35 | # CATKIN_DEPENDS plan_env 36 | # # DEPENDS system_lib 37 | # ) 38 | get_filename_component(PROJECT_ROOT "${CMAKE_CURRENT_SOURCE_DIR}" DIRECTORY) 39 | include_directories( 40 | SYSTEM 41 | include 42 | # ${catkin_INCLUDE_DIRS} 43 | ${Eigen3_INCLUDE_DIRS} 44 | ${PCL_INCLUDE_DIRS} 45 | ${PROJECT_ROOT}/path_searching/include 46 | ${PROJECT_ROOT}/plan_env/include 47 | ${PROJECT_ROOT}/traj_utils_so/include 48 | ) 49 | 50 | add_library( bspline_opt SHARED 51 | src/uniform_bspline.cpp 52 | src/bspline_optimizer.cpp 53 | src/gradient_descent_optimizer.cpp 54 | ) 55 | ament_target_dependencies(bspline_opt 56 | ${MY_DEPENDENCIES} 57 | ) 58 | # target_link_libraries(traj_server 59 | # /home/suxin/English_Path/NEW_WORK_PATH/MY_FINAL_PJ/ROS2_ego_planner/install/bspline_opt/lib/libbspline_opt.so 60 | 61 | # ) 62 | 63 | 64 | 65 | 66 | install(TARGETS 67 | bspline_opt 68 | DESTINATION lib/${PROJECT_NAME} 69 | ) 70 | # install( 71 | # TARGETS bspline_opt 72 | # ARCHIVE DESTINATION lib 73 | # LIBRARY DESTINATION lib 74 | # RUNTIME DESTINATION bin 75 | # ) 76 | # install(DIRECTORY launch 77 | # DESTINATION share/${PROJECT_NAME} 78 | # ) 79 | # install(DIRECTORY config 80 | # DESTINATION share/${PROJECT_NAME}/#把存yaml文件的文件夹config移到share 81 | # ) 82 | if(BUILD_TESTING) 83 | find_package(ament_lint_auto REQUIRED) 84 | # the following line skips the linter which checks for copyrights 85 | # uncomment the line when a copyright and license is not present in all source files 86 | #set(ament_cmake_copyright_FOUND TRUE) 87 | # the following line skips cpplint (only works in a git repo) 88 | # uncomment the line when this package is not in a git repo 89 | #set(ament_cmake_cpplint_FOUND TRUE) 90 | ament_lint_auto_find_test_dependencies() 91 | endif() 92 | ##1.自身工程的.h库 93 | ament_package() -------------------------------------------------------------------------------- /src/planner/bspline_opt/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bspline_opt 4 | 0.0.0 5 | The bspline_opt package 6 | 7 | 8 | 9 | 10 | iszhouxin 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | ament_cmake 52 | 53 | 54 | 55 | ament_lint_auto 56 | ament_lint_common 57 | 58 | 59 | ament_cmake 60 | 61 | 62 | -------------------------------------------------------------------------------- /src/planner/plan_env/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(plan_env) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | ADD_COMPILE_OPTIONS(-std=c++11 ) 6 | ADD_COMPILE_OPTIONS(-std=c++14 ) 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 8 | 9 | find_package(OpenCV REQUIRED) 10 | 11 | 12 | find_package(ament_cmake REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(std_msgs REQUIRED) 15 | find_package(visualization_msgs REQUIRED) 16 | find_package(cv_bridge REQUIRED) 17 | find_package(message_filters REQUIRED) 18 | find_package(nav_msgs REQUIRED) 19 | set(MY_DEPENDENCIES 20 | rclcpp 21 | std_msgs 22 | visualization_msgs 23 | cv_bridge 24 | message_filters 25 | nav_msgs 26 | # PCL 27 | ) 28 | # find_package(catkin REQUIRED COMPONENTS 29 | # roscpp 30 | # rospy 31 | # std_msgs 32 | # visualization_msgs 33 | # cv_bridge 34 | # message_filters 35 | # ) 36 | 37 | find_package(Eigen3 REQUIRED) 38 | find_package(PCL 1.7 REQUIRED) 39 | 40 | # catkin_package( 41 | # INCLUDE_DIRS include 42 | # LIBRARIES plan_env 43 | # CATKIN_DEPENDS roscpp std_msgs 44 | # DEPENDS OpenCV 45 | # # DEPENDS system_lib 46 | # ) 47 | 48 | include_directories( 49 | SYSTEM 50 | include 51 | ${catkin_INCLUDE_DIRS} 52 | ${Eigen3_INCLUDE_DIRS} 53 | ${PCL_INCLUDE_DIRS} 54 | ${OpenCV_INCLUDE_DIRS} 55 | ) 56 | 57 | link_directories(${PCL_LIBRARY_DIRS}) 58 | #TAG 这里非常有意思，三个没有main函数的cpp文件共同编译在一起，作为代码库（在ros2中被当做功能包如msg引入使用）使用 59 | add_library( plan_env SHARED 60 | src/grid_map.cpp 61 | src/raycast.cpp 62 | src/obj_predictor.cpp 63 | ) 64 | ament_target_dependencies(plan_env 65 | ${MY_DEPENDENCIES} 66 | ) 67 | target_link_libraries( plan_env 68 | ${PCL_LIBRARIES} 69 | ${OpenCV_LIBS} 70 | ) 71 | 72 | add_executable(obj_generator 73 | src/obj_generator.cpp 74 | ) 75 | ament_target_dependencies(obj_generator 76 | ${MY_DEPENDENCIES} 77 | ) 78 | 79 | install(TARGETS 80 | plan_env 81 | obj_generator 82 | DESTINATION lib/${PROJECT_NAME} 83 | ) 84 | # install(DIRECTORY launch 85 | # DESTINATION share/${PROJECT_NAME} 86 | # ) 87 | # install(DIRECTORY config 88 | # DESTINATION share/${PROJECT_NAME}/#把存yaml文件的文件夹config移到share 89 | # ) 90 | if(BUILD_TESTING) 91 | find_package(ament_lint_auto REQUIRED) 92 | # the following line skips the linter which checks for copyrights 93 | # uncomment the line when a copyright and license is not present in all source files 94 | #set(ament_cmake_copyright_FOUND TRUE) 95 | # the following line skips cpplint (only works in a git repo) 96 | # uncomment the line when this package is not in a git repo 97 | #set(ament_cmake_cpplint_FOUND TRUE) 98 | ament_lint_auto_find_test_dependencies() 99 | endif() 100 | ##1.自身工程的.h库 101 | ament_package() -------------------------------------------------------------------------------- /src/planner/plan_env/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | plan_env 4 | 0.0.0 5 | The plan_env package 6 | 7 | 8 | 9 | 10 | iszhouxin 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | ament_cmake 52 | 53 | ament_lint_auto 54 | ament_lint_common 55 | 56 | 57 | ament_cmake 58 | 59 | 60 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/src/encode_msgs.cpp: -------------------------------------------------------------------------------- 1 | #include "quadrotor_msgs/encode_msgs.h" 2 | #include 3 | 4 | namespace quadrotor_msgs 5 | { 6 | 7 | void encodeSO3Command(const quadrotor_msgs::SO3Command &so3_command, 8 | std::vector &output) 9 | { 10 | struct SO3_CMD_INPUT so3_cmd_input; 11 | 12 | so3_cmd_input.force[0] = so3_command.force.x*500; 13 | so3_cmd_input.force[1] = so3_command.force.y*500; 14 | so3_cmd_input.force[2] = so3_command.force.z*500; 15 | 16 | so3_cmd_input.des_qx = so3_command.orientation.x*125; 17 | so3_cmd_input.des_qy = so3_command.orientation.y*125; 18 | so3_cmd_input.des_qz = so3_command.orientation.z*125; 19 | so3_cmd_input.des_qw = so3_command.orientation.w*125; 20 | 21 | so3_cmd_input.kR[0] = so3_command.kR[0]*50; 22 | so3_cmd_input.kR[1] = so3_command.kR[1]*50; 23 | so3_cmd_input.kR[2] = so3_command.kR[2]*50; 24 | 25 | so3_cmd_input.kOm[0] = so3_command.kOm[0]*100; 26 | so3_cmd_input.kOm[1] = so3_command.kOm[1]*100; 27 | so3_cmd_input.kOm[2] = so3_command.kOm[2]*100; 28 | 29 | so3_cmd_input.cur_yaw = so3_command.aux.current_yaw*1e4; 30 | 31 | so3_cmd_input.kf_correction = so3_command.aux.kf_correction*1e11; 32 | so3_cmd_input.angle_corrections[0] = so3_command.aux.angle_corrections[0]*2500; 33 | so3_cmd_input.angle_corrections[1] = so3_command.aux.angle_corrections[1]*2500; 34 | 35 | so3_cmd_input.enable_motors = so3_command.aux.enable_motors; 36 | so3_cmd_input.use_external_yaw = so3_command.aux.use_external_yaw; 37 | 38 | so3_cmd_input.seq = so3_command.header.seq % 255; 39 | 40 | output.resize(sizeof(so3_cmd_input)); 41 | memcpy(&output[0], &so3_cmd_input, sizeof(so3_cmd_input)); 42 | } 43 | 44 | void encodeTRPYCommand(const quadrotor_msgs::TRPYCommand &trpy_command, 45 | std::vector &output) 46 | { 47 | struct TRPY_CMD trpy_cmd_input; 48 | trpy_cmd_input.thrust = trpy_command.thrust*1e4; 49 | trpy_cmd_input.roll = trpy_command.roll*1e4; 50 | trpy_cmd_input.pitch = trpy_command.pitch*1e4; 51 | trpy_cmd_input.yaw = trpy_command.yaw*1e4; 52 | trpy_cmd_input.current_yaw = trpy_command.aux.current_yaw*1e4; 53 | trpy_cmd_input.enable_motors = trpy_command.aux.enable_motors; 54 | trpy_cmd_input.use_external_yaw = trpy_command.aux.use_external_yaw; 55 | 56 | output.resize(sizeof(trpy_cmd_input)); 57 | memcpy(&output[0], &trpy_cmd_input, sizeof(trpy_cmd_input)); 58 | } 59 | 60 | void encodePPRGains(const quadrotor_msgs::Gains &gains, 61 | std::vector &output) 62 | { 63 | struct PPR_GAINS ppr_gains; 64 | ppr_gains.Kp = gains.Kp; 65 | ppr_gains.Kd = gains.Kd; 66 | ppr_gains.Kp_yaw = gains.Kp_yaw; 67 | ppr_gains.Kd_yaw = gains.Kd_yaw; 68 | 69 | output.resize(sizeof(ppr_gains)); 70 | memcpy(&output[0], &ppr_gains, sizeof(ppr_gains)); 71 | } 72 | 73 | } 74 | -------------------------------------------------------------------------------- /src/uav_simulator/map_generator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | map_generator 4 | 0.0.0 5 | The map_generator package 6 | 7 | 8 | 9 | 10 | bzhouai 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | ament_cmake 52 | 53 | ament_lint_auto 54 | ament_lint_common 55 | 56 | 57 | ament_cmake 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /src/uav_simulator/fake_drone/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | poscmd_2_odom 4 | 0.0.0 5 | The poscmd_2_odom package 6 | 7 | 8 | 9 | 10 | todo 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | ament_cmake 52 | 53 | ament_lint_auto 54 | ament_lint_common 55 | quadrotor_msgs 56 | 57 | 58 | ament_cmake 59 | 60 | 61 | -------------------------------------------------------------------------------- /src/planner/traj_utils_so/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | traj_utils_so 5 | 0.0.0 6 | The traj_utils package 7 | 8 | 9 | 10 | 11 | bzhouai 12 | 13 | 14 | 15 | 16 | 17 | TODO 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | ament_cmake 53 | 54 | rosidl_default_generators 55 | rosidl_default_runtime 56 | rosidl_interface_packages 57 | 58 | 59 | 60 | ament_lint_auto 61 | ament_lint_common 62 | 63 | 64 | ament_cmake 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /src/planner/traj_utils_z/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | traj_utils 5 | 0.0.0 6 | The traj_utils_z package 7 | 8 | 9 | 10 | 11 | bzhouai 12 | 13 | 14 | 15 | 16 | 17 | TODO 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | ament_cmake 53 | 54 | rosidl_default_generators 55 | rosidl_default_runtime 56 | rosidl_interface_packages 57 | 58 | 59 | 60 | ament_lint_auto 61 | ament_lint_common 62 | 63 | 64 | ament_cmake 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /src/planner/bspline_opt/include/bspline_opt/uniform_bspline.h: -------------------------------------------------------------------------------- 1 | #ifndef _UNIFORM_BSPLINE_H_ 2 | #define _UNIFORM_BSPLINE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | 10 | namespace ego_planner 11 | { 12 | // An implementation of non-uniform B-spline with different dimensions 13 | // It also represents uniform B-spline which is a special case of non-uniform 14 | class UniformBspline 15 | { 16 | private: 17 | // control points for B-spline with different dimensions. 18 | // Each row represents one single control point 19 | // The dimension is determined by column number 20 | // e.g. B-spline with N points in 3D space -> Nx3 matrix 21 | Eigen::MatrixXd control_points_; 22 | 23 | int p_, n_, m_; // p degree, n+1 control points, m = n+p+1 24 | Eigen::VectorXd u_; // knots vector 25 | double interval_; // knot span \delta t 26 | 27 | Eigen::MatrixXd getDerivativeControlPoints(); 28 | 29 | double limit_vel_, limit_acc_, limit_ratio_, feasibility_tolerance_; // physical limits and time adjustment ratio 30 | 31 | public: 32 | UniformBspline() {} 33 | UniformBspline(const Eigen::MatrixXd &points, const int &order, const double &interval); 34 | ~UniformBspline(); 35 | 36 | Eigen::MatrixXd get_control_points(void) { return control_points_; } 37 | 38 | // initialize as an uniform B-spline 39 | void setUniformBspline(const Eigen::MatrixXd &points, const int &order, const double &interval); 40 | 41 | // get / set basic bspline info 42 | 43 | void setKnot(const Eigen::VectorXd &knot); 44 | Eigen::VectorXd getKnot(); 45 | Eigen::MatrixXd getControlPoint(); 46 | double getInterval(); 47 | bool getTimeSpan(double &um, double &um_p); 48 | 49 | // compute position / derivative 50 | 51 | Eigen::VectorXd evaluateDeBoor(const double &u); // use u \in [up, u_mp] 52 | inline Eigen::VectorXd evaluateDeBoorT(const double &t) { return evaluateDeBoor(t + u_(p_)); } // use t \in [0, duration] 53 | UniformBspline getDerivative(); 54 | 55 | // 3D B-spline interpolation of points in point_set, with boundary vel&acc 56 | // constraints 57 | // input : (K+2) points with boundary vel/acc; ts 58 | // output: (K+6) control_pts 59 | static void parameterizeToBspline(const double &ts, const vector &point_set, 60 | const vector &start_end_derivative, 61 | Eigen::MatrixXd &ctrl_pts); 62 | 63 | /* check feasibility, adjust time */ 64 | 65 | void setPhysicalLimits(const double &vel, const double &acc, const double &tolerance); 66 | bool checkFeasibility(double &ratio, bool show = false); 67 | void lengthenTime(const double &ratio); 68 | 69 | /* for performance evaluation */ 70 | 71 | double getTimeSum(); 72 | double getLength(const double &res = 0.01); 73 | double getJerk(); 74 | void getMeanAndMaxVel(double &mean_v, double &max_v); 75 | void getMeanAndMaxAcc(double &mean_a, double &max_a); 76 | 77 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 78 | }; 79 | } // namespace ego_planner 80 | #endif -------------------------------------------------------------------------------- /src/planner/traj_utils_so/include/traj_utils/planning_visualization.h: -------------------------------------------------------------------------------- 1 | #ifndef _PLANNING_VISUALIZATION_H_ 2 | #define _PLANNING_VISUALIZATION_H_ 3 | 4 | #include 5 | #include 6 | //#include 7 | #include 8 | //#include 9 | #include "rclcpp/rclcpp.hpp" 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using std::vector; 16 | namespace ego_planner 17 | { 18 | class PlanningVisualization 19 | { 20 | private: 21 | std::shared_ptr node; 22 | 23 | rclcpp::Publisher::SharedPtr goal_point_pub; 24 | rclcpp::Publisher::SharedPtr global_list_pub; 25 | rclcpp::Publisher::SharedPtr init_list_pub; 26 | rclcpp::Publisher::SharedPtr optimal_list_pub; 27 | rclcpp::Publisher::SharedPtr a_star_list_pub; 28 | rclcpp::Publisher::SharedPtr guide_vector_pub; 29 | rclcpp::Publisher::SharedPtr intermediate_state_pub; 30 | 31 | public: 32 | PlanningVisualization(/* args */) {} 33 | ~PlanningVisualization() {} 34 | PlanningVisualization(std::shared_ptr& nh); 35 | 36 | typedef std::shared_ptr Ptr; 37 | 38 | void displayMarkerList(rclcpp::Publisher::SharedPtr &pub, const vector &list, double scale, 39 | Eigen::Vector4d color, int id, bool show_sphere = true); 40 | void generatePathDisplayArray(visualization_msgs::msg::MarkerArray &array, 41 | const vector &list, double scale, Eigen::Vector4d color, int id); 42 | void generateArrowDisplayArray(visualization_msgs::msg::MarkerArray &array, 43 | const vector &list, double scale, Eigen::Vector4d color, int id); 44 | void displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id); 45 | void displayGlobalPathList(vector global_pts, const double scale, int id); 46 | void displayInitPathList(vector init_pts, const double scale, int id); 47 | void displayMultiInitPathList(vector> init_trajs, const double scale); 48 | void displayOptimalList(Eigen::MatrixXd optimal_pts, int id); 49 | void displayAStarList(std::vector> a_star_paths, int id); 50 | void displayArrowList(rclcpp::Publisher::SharedPtr &pub, const vector &list, double scale, Eigen::Vector4d color, int id); 51 | // void displayIntermediateState(ros::Publisher& intermediate_pub, ego_planner::BsplineOptimizer::Ptr optimizer, double sleep_time, const int start_iteration); 52 | // void displayNewArrow(ros::Publisher& guide_vector_pub, ego_planner::BsplineOptimizer::Ptr optimizer); 53 | rclcpp::Clock _clock; 54 | }; 55 | } // namespace ego_planner 56 | #endif -------------------------------------------------------------------------------- /src/planner/traj_utils_z/include/traj_utils/planning_visualization.h: -------------------------------------------------------------------------------- 1 | #ifndef _PLANNING_VISUALIZATION_H_ 2 | #define _PLANNING_VISUALIZATION_H_ 3 | 4 | #include 5 | #include 6 | //#include 7 | #include 8 | //#include 9 | #include "rclcpp/rclcpp.hpp" 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using std::vector; 16 | namespace ego_planner 17 | { 18 | class PlanningVisualization 19 | { 20 | private: 21 | std::shared_ptr node; 22 | 23 | rclcpp::Publisher::SharedPtr goal_point_pub; 24 | rclcpp::Publisher::SharedPtr global_list_pub; 25 | rclcpp::Publisher::SharedPtr init_list_pub; 26 | rclcpp::Publisher::SharedPtr optimal_list_pub; 27 | rclcpp::Publisher::SharedPtr a_star_list_pub; 28 | rclcpp::Publisher::SharedPtr guide_vector_pub; 29 | rclcpp::Publisher::SharedPtr intermediate_state_pub; 30 | 31 | public: 32 | PlanningVisualization(/* args */) {} 33 | ~PlanningVisualization() {} 34 | PlanningVisualization(std::shared_ptr& nh); 35 | 36 | typedef std::shared_ptr Ptr; 37 | 38 | void displayMarkerList(rclcpp::Publisher::SharedPtr &pub, const vector &list, double scale, 39 | Eigen::Vector4d color, int id, bool show_sphere = true); 40 | void generatePathDisplayArray(visualization_msgs::msg::MarkerArray &array, 41 | const vector &list, double scale, Eigen::Vector4d color, int id); 42 | void generateArrowDisplayArray(visualization_msgs::msg::MarkerArray &array, 43 | const vector &list, double scale, Eigen::Vector4d color, int id); 44 | void displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id); 45 | void displayGlobalPathList(vector global_pts, const double scale, int id); 46 | void displayInitPathList(vector init_pts, const double scale, int id); 47 | void displayMultiInitPathList(vector> init_trajs, const double scale); 48 | void displayOptimalList(Eigen::MatrixXd optimal_pts, int id); 49 | void displayAStarList(std::vector> a_star_paths, int id); 50 | void displayArrowList(rclcpp::Publisher::SharedPtr &pub, const vector &list, double scale, Eigen::Vector4d color, int id); 51 | // void displayIntermediateState(ros::Publisher& intermediate_pub, ego_planner::BsplineOptimizer::Ptr optimizer, double sleep_time, const int start_iteration); 52 | // void displayNewArrow(ros::Publisher& guide_vector_pub, ego_planner::BsplineOptimizer::Ptr optimizer); 53 | rclcpp::Clock _clock; 54 | }; 55 | } // namespace ego_planner 56 | #endif -------------------------------------------------------------------------------- /src/uav_simulator/local_sensing/CMakeModules/FindEigen.cmake~: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # 3 | # CMake script for finding the Eigen library. 4 | # 5 | # http://eigen.tuxfamily.org/index.php?title=Main_Page 6 | # 7 | # Copyright (c) 2006, 2007 Montel Laurent, 8 | # Copyright (c) 2008, 2009 Gael Guennebaud, 9 | # Copyright (c) 2009 Benoit Jacob 10 | # Redistribution and use is allowed according to the terms of the 2-clause BSD 11 | # license. 12 | # 13 | # 14 | # Input variables: 15 | # 16 | # - Eigen_ROOT_DIR (optional): When specified, header files and libraries 17 | # will be searched for in `${Eigen_ROOT_DIR}/include` and 18 | # `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order 19 | # will be ignored. When unspecified, the default CMake search order is used. 20 | # This variable can be specified either as a CMake or environment variable. 21 | # If both are set, preference is given to the CMake variable. 22 | # Use this variable for finding packages installed in a nonstandard location, 23 | # or for enforcing that one of multiple package installations is picked up. 24 | # 25 | # Cache variables (not intended to be used in CMakeLists.txt files) 26 | # 27 | # - Eigen_INCLUDE_DIR: Absolute path to package headers. 28 | # 29 | # 30 | # Output variables: 31 | # 32 | # - Eigen_FOUND: Boolean that indicates if the package was found 33 | # - Eigen_INCLUDE_DIRS: Paths to the necessary header files 34 | # - Eigen_VERSION: Version of Eigen library found 35 | # - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen 36 | # 37 | # 38 | # Example usage: 39 | # 40 | # # Passing the version means Eigen_FOUND will only be TRUE if a 41 | # # version >= the provided version is found. 42 | # find_package(Eigen 3.1.2) 43 | # if(NOT Eigen_FOUND) 44 | # # Error handling 45 | # endif() 46 | # ... 47 | # add_definitions(${Eigen_DEFINITIONS}) 48 | # ... 49 | # include_directories(${Eigen_INCLUDE_DIRS} ...) 50 | # 51 | ############################################################################### 52 | 53 | find_package(PkgConfig) 54 | pkg_check_modules(PC_EIGEN eigen3) 55 | set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) 56 | 57 | 58 | find_path(EIGEN_INCLUDE_DIR Eigen/Core 59 | HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} 60 | "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" 61 | "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility 62 | PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" 63 | "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" 64 | PATH_SUFFIXES eigen3 include/eigen3 include) 65 | 66 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 67 | 68 | include(FindPackageHandleStandardArgs) 69 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) 70 | 71 | mark_as_advanced(EIGEN_INCLUDE_DIR) 72 | 73 | if(EIGEN_FOUND) 74 | message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") 75 | endif(EIGEN_FOUND) 76 | 77 | 78 | set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 79 | set(Eigen_FOUND ${EIGEN_FOUND}) 80 | set(Eigen_VERSION ${EIGEN_VERSION}) 81 | set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) 82 | -------------------------------------------------------------------------------- /src/planner/plan_manage/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ego_planner 4 | 0.0.0 5 | The ego_planner package 6 | 7 | 8 | 9 | 10 | iszhouxin 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | ament_cmake 52 | 53 | ament_lint_auto 54 | ament_lint_common 55 | quadrotor_msgs 56 | traj_utils 57 | plan_env 58 | 59 | traj_utils_so 60 | path_searching 61 | bspline_opt 62 | 63 | 64 | ament_cmake 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /src/uav_simulator/local_sensing/CMakeModules/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # 3 | # CMake script for finding the Eigen library. 4 | # 5 | # http://eigen.tuxfamily.org/index.php?title=Main_Page 6 | # 7 | # Copyright (c) 2006, 2007 Montel Laurent, 8 | # Copyright (c) 2008, 2009 Gael Guennebaud, 9 | # Copyright (c) 2009 Benoit Jacob 10 | # Redistribution and use is allowed according to the terms of the 2-clause BSD 11 | # license. 12 | # 13 | # 14 | # Input variables: 15 | # 16 | # - Eigen_ROOT_DIR (optional): When specified, header files and libraries 17 | # will be searched for in `${Eigen_ROOT_DIR}/include` and 18 | # `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order 19 | # will be ignored. When unspecified, the default CMake search order is used. 20 | # This variable can be specified either as a CMake or environment variable. 21 | # If both are set, preference is given to the CMake variable. 22 | # Use this variable for finding packages installed in a nonstandard location, 23 | # or for enforcing that one of multiple package installations is picked up. 24 | # 25 | # Cache variables (not intended to be used in CMakeLists.txt files) 26 | # 27 | # - Eigen_INCLUDE_DIR: Absolute path to package headers. 28 | # 29 | # 30 | # Output variables: 31 | # 32 | # - Eigen_FOUND: Boolean that indicates if the package was found 33 | # - Eigen_INCLUDE_DIRS: Paths to the necessary header files 34 | # - Eigen_VERSION: Version of Eigen library found 35 | # - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen 36 | # 37 | # 38 | # Example usage: 39 | # 40 | # # Passing the version means Eigen_FOUND will only be TRUE if a 41 | # # version >= the provided version is found. 42 | # find_package(Eigen 3.1.2) 43 | # if(NOT Eigen_FOUND) 44 | # # Error handling 45 | # endif() 46 | # ... 47 | # add_definitions(${Eigen_DEFINITIONS}) 48 | # ... 49 | # include_directories(${Eigen_INCLUDE_DIRS} ...) 50 | # 51 | ############################################################################### 52 | 53 | find_package(PkgConfig) 54 | pkg_check_modules(PC_EIGEN eigen3) 55 | set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) 56 | 57 | set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3/") 58 | find_path(EIGEN_INCLUDE_DIR Eigen/Core 59 | HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} 60 | "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" 61 | "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility 62 | PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" 63 | "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" 64 | PATH_SUFFIXES eigen3 include/eigen3 include) 65 | 66 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 67 | 68 | include(FindPackageHandleStandardArgs) 69 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) 70 | 71 | mark_as_advanced(EIGEN_INCLUDE_DIR) 72 | 73 | if(EIGEN_FOUND) 74 | message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") 75 | endif(EIGEN_FOUND) 76 | 77 | 78 | set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 79 | set(Eigen_FOUND ${EIGEN_FOUND}) 80 | set(Eigen_VERSION ${EIGEN_VERSION}) 81 | set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) 82 | -------------------------------------------------------------------------------- /src/planner/path_searching/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | path_searching 4 | 0.0.0 5 | The path_searching package 6 | 7 | 8 | 9 | 10 | iszhouxin 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /src/planner/plan_manage/include/plan_manage/planner_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef _PLANNER_MANAGER_H_ 2 | #define _PLANNER_MANAGER_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "rclcpp/rclcpp.hpp" 13 | #include 14 | 15 | namespace ego_planner 16 | { 17 | 18 | // Fast Planner Manager 19 | // Key algorithms of mapping and planning are called 20 | 21 | class EGOPlannerManager 22 | { 23 | // SECTION stable 24 | public: 25 | EGOPlannerManager(); 26 | ~EGOPlannerManager(); 27 | 28 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 29 | 30 | /* main planning interface */ 31 | bool reboundReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel, Eigen::Vector3d start_acc, 32 | Eigen::Vector3d end_pt, Eigen::Vector3d end_vel, bool flag_polyInit, bool flag_randomPolyTraj); 33 | bool EmergencyStop(Eigen::Vector3d stop_pos); 34 | bool planGlobalTraj(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc, 35 | const Eigen::Vector3d &end_pos, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc); 36 | bool planGlobalTrajWaypoints(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc, 37 | const std::vector &waypoints, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc); 38 | 39 | void initPlanModules(std::shared_ptr& nh, PlanningVisualization::Ptr vis = NULL); 40 | 41 | void deliverTrajToOptimizer(void) { bspline_optimizer_->setSwarmTrajs(&swarm_trajs_buf_); }; 42 | 43 | void setDroneIdtoOpt(void) { bspline_optimizer_->setDroneId(pp_.drone_id); } 44 | 45 | double getSwarmClearance(void) { return bspline_optimizer_->getSwarmClearance(); } 46 | 47 | bool checkCollision(int drone_id); 48 | 49 | 50 | PlanParameters pp_; 51 | LocalTrajData local_data_; 52 | GlobalTrajData global_data_; 53 | GridMap::Ptr grid_map_; 54 | fast_planner::ObjPredictor::Ptr obj_predictor_; 55 | SwarmTrajData swarm_trajs_buf_; 56 | rclcpp::Clock _clock; 57 | 58 | private: 59 | /* main planning algorithms & modules */ 60 | PlanningVisualization::Ptr visualization_; 61 | 62 | // ros::Publisher obj_pub_; //zx-todo 63 | 64 | BsplineOptimizer::Ptr bspline_optimizer_; 65 | 66 | int continous_failures_count_{0}; 67 | 68 | void updateTrajInfo(const UniformBspline &position_traj, const rclcpp::Time time_now); 69 | 70 | void reparamBspline(UniformBspline &bspline, vector &start_end_derivative, double ratio, Eigen::MatrixXd &ctrl_pts, double &dt, 71 | double &time_inc); 72 | 73 | bool refineTrajAlgo(UniformBspline &traj, vector &start_end_derivative, double ratio, double &ts, Eigen::MatrixXd &optimal_control_points); 74 | 75 | // !SECTION stable 76 | 77 | // SECTION developing 78 | 79 | public: 80 | typedef unique_ptr Ptr; 81 | 82 | // !SECTION 83 | }; 84 | } // namespace ego_planner 85 | 86 | #endif -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/cmake/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | 82 | -------------------------------------------------------------------------------- /src/uav_simulator/local_sensing/CMakeModules/FindCUDA/make2cmake.cmake: -------------------------------------------------------------------------------- 1 | # James Bigler, NVIDIA Corp (nvidia.com - jbigler) 2 | # Abe Stephens, SCI Institute -- http://www.sci.utah.edu/~abe/FindCuda.html 3 | # 4 | # Copyright (c) 2008 - 2009 NVIDIA Corporation. All rights reserved. 5 | # 6 | # Copyright (c) 2007-2009 7 | # Scientific Computing and Imaging Institute, University of Utah 8 | # 9 | # This code is licensed under the MIT License. See the FindCUDA.cmake script 10 | # for the text of the license. 11 | 12 | # The MIT License 13 | # 14 | # License for the specific language governing rights and limitations under 15 | # Permission is hereby granted, free of charge, to any person obtaining a 16 | # copy of this software and associated documentation files (the "Software"), 17 | # to deal in the Software without restriction, including without limitation 18 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 19 | # and/or sell copies of the Software, and to permit persons to whom the 20 | # Software is furnished to do so, subject to the following conditions: 21 | # 22 | # The above copyright notice and this permission notice shall be included 23 | # in all copies or substantial portions of the Software. 24 | # 25 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 26 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 27 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 28 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 29 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 30 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 31 | # DEALINGS IN THE SOFTWARE. 32 | # 33 | 34 | ####################################################################### 35 | # This converts a file written in makefile syntax into one that can be included 36 | # by CMake. 37 | 38 | file(READ ${input_file} depend_text) 39 | 40 | if (${depend_text} MATCHES ".+") 41 | 42 | # message("FOUND DEPENDS") 43 | 44 | # Remember, four backslashes is escaped to one backslash in the string. 45 | string(REGEX REPLACE "\\\\ " " " depend_text ${depend_text}) 46 | 47 | # This works for the nvcc -M generated dependency files. 48 | string(REGEX REPLACE "^.* : " "" depend_text ${depend_text}) 49 | string(REGEX REPLACE "[ \\\\]*\n" ";" depend_text ${depend_text}) 50 | 51 | set(dependency_list "") 52 | 53 | foreach(file ${depend_text}) 54 | 55 | string(REGEX REPLACE "^ +" "" file ${file}) 56 | 57 | if(NOT IS_DIRECTORY ${file}) 58 | # If softlinks start to matter, we should change this to REALPATH. For now we need 59 | # to flatten paths, because nvcc can generate stuff like /bin/../include instead of 60 | # just /include. 61 | get_filename_component(file_absolute "${file}" ABSOLUTE) 62 | list(APPEND dependency_list "${file_absolute}") 63 | endif(NOT IS_DIRECTORY ${file}) 64 | 65 | endforeach(file) 66 | 67 | else() 68 | # message("FOUND NO DEPENDS") 69 | endif() 70 | 71 | # Remove the duplicate entries and sort them. 72 | list(REMOVE_DUPLICATES dependency_list) 73 | list(SORT dependency_list) 74 | 75 | foreach(file ${dependency_list}) 76 | set(cuda_nvcc_depend "${cuda_nvcc_depend} \"${file}\"\n") 77 | endforeach() 78 | 79 | file(WRITE ${output_file} "# Generated by: make2cmake.cmake\nSET(CUDA_NVCC_DEPEND\n ${cuda_nvcc_depend})\n\n") 80 | -------------------------------------------------------------------------------- /src/planner/bspline_opt/src/gradient_descent_optimizer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define RESET "\033[0m" 4 | #define RED "\033[31m" 5 | 6 | GradientDescentOptimizer::RESULT 7 | GradientDescentOptimizer::optimize(Eigen::VectorXd &x_init_optimal, double &opt_f) 8 | { 9 | if (min_grad_ < 1e-10) 10 | { 11 | cout << RED << "min_grad_ is invalid:" << min_grad_ << RESET << endl; 12 | return FAILED; 13 | } 14 | if (iter_limit_ <= 2) 15 | { 16 | cout << RED << "iter_limit_ is invalid:" << iter_limit_ << RESET << endl; 17 | return FAILED; 18 | } 19 | 20 | void *f_data = f_data_; 21 | int iter = 2; 22 | int invoke_count = 2; 23 | bool force_return; 24 | Eigen::VectorXd x_k(x_init_optimal), x_kp1(x_init_optimal.rows()); 25 | double cost_k, cost_kp1, cost_min; 26 | Eigen::VectorXd grad_k(x_init_optimal.rows()), grad_kp1(x_init_optimal.rows()); 27 | 28 | cost_k = objfun_(x_k, grad_k, force_return, f_data); 29 | if (force_return) 30 | return RETURN_BY_ORDER; 31 | cost_min = cost_k; 32 | double max_grad = max(abs(grad_k.maxCoeff()), abs(grad_k.minCoeff())); 33 | constexpr double MAX_MOVEMENT_AT_FIRST_ITERATION = 0.1; // meter 34 | double alpha0 = max_grad < MAX_MOVEMENT_AT_FIRST_ITERATION ? 1.0 : (MAX_MOVEMENT_AT_FIRST_ITERATION / max_grad); 35 | x_kp1 = x_k - alpha0 * grad_k; 36 | cost_kp1 = objfun_(x_kp1, grad_kp1, force_return, f_data); 37 | if (force_return) 38 | return RETURN_BY_ORDER; 39 | if (cost_min > cost_kp1) 40 | cost_min = cost_kp1; 41 | 42 | /*** start iteration ***/ 43 | while (++iter <= iter_limit_ && invoke_count <= invoke_limit_) 44 | { 45 | Eigen::VectorXd s = x_kp1 - x_k; 46 | Eigen::VectorXd y = grad_kp1 - grad_k; 47 | double alpha = s.dot(y) / y.dot(y); 48 | if (isnan(alpha) || isinf(alpha)) 49 | { 50 | cout << RED << "step size invalid! alpha=" << alpha << RESET << endl; 51 | return FAILED; 52 | } 53 | 54 | if (iter % 2) // to aviod copying operations 55 | { 56 | do 57 | { 58 | x_k = x_kp1 - alpha * grad_kp1; 59 | cost_k = objfun_(x_k, grad_k, force_return, f_data); 60 | invoke_count++; 61 | if (force_return) 62 | return RETURN_BY_ORDER; 63 | alpha *= 0.5; 64 | } while (cost_k > cost_kp1 - 1e-4 * alpha * grad_kp1.transpose() * grad_kp1); // Armijo condition 65 | 66 | if (grad_k.norm() < min_grad_) 67 | { 68 | opt_f = cost_k; 69 | return FIND_MIN; 70 | } 71 | } 72 | else 73 | { 74 | do 75 | { 76 | x_kp1 = x_k - alpha * grad_k; 77 | cost_kp1 = objfun_(x_kp1, grad_kp1, force_return, f_data); 78 | invoke_count++; 79 | if (force_return) 80 | return RETURN_BY_ORDER; 81 | alpha *= 0.5; 82 | } while (cost_kp1 > cost_k - 1e-4 * alpha * grad_k.transpose() * grad_k); // Armijo condition 83 | 84 | if (grad_kp1.norm() < min_grad_) 85 | { 86 | opt_f = cost_kp1; 87 | return FIND_MIN; 88 | } 89 | } 90 | } 91 | 92 | opt_f = iter_limit_ % 2 ? cost_k : cost_kp1; 93 | return REACH_MAX_ITERATION; 94 | } 95 | -------------------------------------------------------------------------------- /src/uav_simulator/local_sensing/src/AlignError.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | struct AlignError { 5 | AlignError( const Eigen::Quaterniond camera_pose, const Eigen::Vector3d camera_trans, 6 | const Eigen::Quaterniond velodyne_pose, const Eigen::Vector3d velodyne_trans) 7 | { 8 | camera_q[0] = camera_pose.x(); 9 | camera_q[1] = camera_pose.y(); 10 | camera_q[2] = camera_pose.z(); 11 | camera_q[3] = camera_pose.w(); 12 | camera_t[0] = camera_trans.x(); 13 | camera_t[1] = camera_trans.y(); 14 | camera_t[2] = camera_trans.z(); 15 | 16 | velodyne_q[0] = velodyne_pose.x(); 17 | velodyne_q[1] = velodyne_pose.y(); 18 | velodyne_q[2] = velodyne_pose.z(); 19 | velodyne_q[3] = velodyne_pose.w(); 20 | velodyne_t[0] = velodyne_trans.x(); 21 | velodyne_t[1] = velodyne_trans.y(); 22 | velodyne_t[2] = velodyne_trans.z(); 23 | } 24 | 25 | // Factory to hide the construction of the CostFunction object from the client code. 26 | static ceres::CostFunction* Create( const Eigen::Quaterniond camera_pose, const Eigen::Vector3d camera_trans, 27 | const Eigen::Quaterniond velodyne_pose, const Eigen::Vector3d velodyne_trans) 28 | { 29 | return (new ceres::AutoDiffCostFunction( 30 | new AlignError(camera_pose, camera_trans, velodyne_pose, velodyne_trans))); 31 | } 32 | 33 | template 34 | bool operator()(const T* const world_rotation, const T* const world_translation, 35 | const T* const v2c_rotation, const T* const v2c_translation, 36 | T* residuals) const 37 | { 38 | Eigen::Quaternion q_world = Eigen::Map< const Eigen::Quaternion >(world_rotation); 39 | Eigen::Matrix t_world = Eigen::Map< const Eigen::Matrix >(world_translation); 40 | 41 | Eigen::Quaternion q_v2c = Eigen::Map< const Eigen::Quaternion >(v2c_rotation); 42 | Eigen::Matrix t_v2c = Eigen::Map< const Eigen::Matrix >(v2c_translation); 43 | 44 | Eigen::Quaternion q_c; 45 | Eigen::Matrix t_c; 46 | q_c.x() = T(camera_q[0]); 47 | q_c.y() = T(camera_q[1]); 48 | q_c.z() = T(camera_q[2]); 49 | q_c.w() = T(camera_q[3]); 50 | t_c << T(camera_t[0]), T(camera_t[1]), T(camera_t[2]); 51 | 52 | Eigen::Quaternion q_v; 53 | Eigen::Matrix t_v; 54 | q_v.x() = T(velodyne_q[0]); 55 | q_v.y() = T(velodyne_q[1]); 56 | q_v.z() = T(velodyne_q[2]); 57 | q_v.w() = T(velodyne_q[3]); 58 | t_v << T(velodyne_t[0]), T(velodyne_t[1]), T(velodyne_t[2]); 59 | 60 | Eigen::Quaternion q_left; 61 | Eigen::Matrix t_left; 62 | 63 | q_left = q_world * q_c * q_v2c; 64 | t_left = q_world * q_c * t_v2c + q_world * t_c + t_world; 65 | 66 | Eigen::Quaternion q_diff; 67 | Eigen::Matrix t_diff; 68 | q_diff = q_left * q_v.inverse(); 69 | // t_diff = t_left - q_left * q_v.inverse() * t_v; 70 | t_diff = t_left - t_v; 71 | 72 | residuals[0] = q_diff.x(); 73 | residuals[1] = q_diff.y(); 74 | residuals[2] = q_diff.z(); 75 | residuals[3] = t_diff(0); 76 | residuals[4] = t_diff(1); 77 | residuals[5] = t_diff(2); 78 | 79 | return true; 80 | } 81 | 82 | double camera_q[4]; 83 | double camera_t[3]; 84 | double velodyne_q[4]; 85 | double velodyne_t[3]; 86 | }; 87 | -------------------------------------------------------------------------------- /src/planner/path_searching/include/path_searching/dyn_a_star.h: -------------------------------------------------------------------------------- 1 | #ifndef _DYN_A_STAR_H_ 2 | #define _DYN_A_STAR_H_ 3 | 4 | #include 5 | #include "rclcpp/rclcpp.hpp" 6 | // #include 7 | #include "rclcpp/logging.hpp" 8 | #include 9 | #include 10 | #include 11 | 12 | constexpr double inf = 1 >> 20; 13 | struct GridNode; 14 | typedef GridNode *GridNodePtr; 15 | 16 | struct GridNode 17 | { 18 | enum enum_state 19 | { 20 | OPENSET = 1, 21 | CLOSEDSET = 2, 22 | UNDEFINED = 3 23 | }; 24 | 25 | int rounds{0}; // Distinguish every call 26 | enum enum_state state 27 | { 28 | UNDEFINED 29 | }; 30 | Eigen::Vector3i index; 31 | 32 | double gScore{inf}, fScore{inf}; 33 | GridNodePtr cameFrom{NULL}; 34 | }; 35 | 36 | class NodeComparator 37 | { 38 | public: 39 | bool operator()(GridNodePtr node1, GridNodePtr node2) 40 | { 41 | return node1->fScore > node2->fScore; 42 | } 43 | }; 44 | 45 | class AStar 46 | { 47 | private: 48 | GridMap::Ptr grid_map_; 49 | 50 | inline void coord2gridIndexFast(const double x, const double y, const double z, int &id_x, int &id_y, int &id_z); 51 | 52 | double getDiagHeu(GridNodePtr node1, GridNodePtr node2); 53 | double getManhHeu(GridNodePtr node1, GridNodePtr node2); 54 | double getEuclHeu(GridNodePtr node1, GridNodePtr node2); 55 | inline double getHeu(GridNodePtr node1, GridNodePtr node2); 56 | 57 | bool ConvertToIndexAndAdjustStartEndPoints(const Eigen::Vector3d start_pt, const Eigen::Vector3d end_pt, Eigen::Vector3i &start_idx, Eigen::Vector3i &end_idx); 58 | 59 | inline Eigen::Vector3d Index2Coord(const Eigen::Vector3i &index) const; 60 | inline bool Coord2Index(const Eigen::Vector3d &pt, Eigen::Vector3i &idx) const; 61 | 62 | //bool (*checkOccupancyPtr)( const Eigen::Vector3d &pos ); 63 | 64 | inline bool checkOccupancy(const Eigen::Vector3d &pos) { return (bool)grid_map_->getInflateOccupancy(pos); } 65 | 66 | std::vector retrievePath(GridNodePtr current); 67 | 68 | double step_size_, inv_step_size_; 69 | Eigen::Vector3d center_; 70 | Eigen::Vector3i CENTER_IDX_, POOL_SIZE_; 71 | const double tie_breaker_ = 1.0 + 1.0 / 10000; 72 | 73 | std::vector gridPath_; 74 | 75 | GridNodePtr ***GridNodeMap_; 76 | std::priority_queue, NodeComparator> openSet_; 77 | 78 | int rounds_{0}; 79 | 80 | public: 81 | typedef std::shared_ptr Ptr; 82 | 83 | AStar(){}; 84 | ~AStar(); 85 | 86 | void initGridMap(GridMap::Ptr occ_map, const Eigen::Vector3i pool_size); 87 | 88 | bool AstarSearch(const double step_size, Eigen::Vector3d start_pt, Eigen::Vector3d end_pt); 89 | 90 | std::vector getPath(); 91 | rclcpp::Clock _clock; 92 | }; 93 | 94 | inline double AStar::getHeu(GridNodePtr node1, GridNodePtr node2) 95 | { 96 | return tie_breaker_ * getDiagHeu(node1, node2); 97 | } 98 | 99 | inline Eigen::Vector3d AStar::Index2Coord(const Eigen::Vector3i &index) const 100 | { 101 | return ((index - CENTER_IDX_).cast() * step_size_) + center_; 102 | }; 103 | 104 | inline bool AStar::Coord2Index(const Eigen::Vector3d &pt, Eigen::Vector3i &idx) const 105 | { 106 | idx = ((pt - center_) * inv_step_size_ + Eigen::Vector3d(0.5, 0.5, 0.5)).cast() + CENTER_IDX_; 107 | 108 | if (idx(0) < 0 || idx(0) >= POOL_SIZE_(0) || idx(1) < 0 || idx(1) >= POOL_SIZE_(1) || idx(2) < 0 || idx(2) >= POOL_SIZE_(2)) 109 | { 110 | // ROS_ERROR("Ran out of pool, index=%d %d %d", idx(0), idx(1), idx(2)); 111 | RCLCPP_ERROR(rclcpp::get_logger("dyn_a_star"), "Ran out of pool, index=%d %d %d", idx(0), idx(1), idx(2)); 112 | return false; 113 | } 114 | 115 | return true; 116 | }; 117 | 118 | #endif 119 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/src/decode_msgs.cpp: -------------------------------------------------------------------------------- 1 | #include "quadrotor_msgs/decode_msgs.h" 2 | #include 3 | #include 4 | 5 | namespace quadrotor_msgs 6 | { 7 | 8 | bool decodeOutputData(const std::vector &data, 9 | quadrotor_msgs::OutputData &output) 10 | { 11 | struct OUTPUT_DATA output_data; 12 | if(data.size() != sizeof(output_data)) 13 | return false; 14 | 15 | memcpy(&output_data, &data[0], sizeof(output_data)); 16 | output.loop_rate = output_data.loop_rate; 17 | output.voltage = output_data.voltage/1e3; 18 | 19 | const double roll = output_data.roll/1e2 * M_PI/180; 20 | const double pitch = output_data.pitch/1e2 * M_PI/180; 21 | const double yaw = output_data.yaw/1e2 * M_PI/180; 22 | // Asctec (2012 firmware) uses Z-Y-X convention 23 | Eigen::Quaterniond q = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * 24 | Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * 25 | Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()); 26 | output.orientation.w = q.w(); 27 | output.orientation.x = q.x(); 28 | output.orientation.y = q.y(); 29 | output.orientation.z = q.z(); 30 | 31 | output.angular_velocity.x = output_data.ang_vel[0]*0.0154*M_PI/180; 32 | output.angular_velocity.y = output_data.ang_vel[1]*0.0154*M_PI/180; 33 | output.angular_velocity.z = output_data.ang_vel[2]*0.0154*M_PI/180; 34 | 35 | output.linear_acceleration.x = output_data.acc[0]/1e3 * 9.81; 36 | output.linear_acceleration.y = output_data.acc[1]/1e3 * 9.81; 37 | output.linear_acceleration.z = output_data.acc[2]/1e3 * 9.81; 38 | 39 | output.pressure_dheight = output_data.dheight/1e3; 40 | output.pressure_height = output_data.height/1e3; 41 | 42 | output.magnetic_field.x = output_data.mag[0]/2500.0; 43 | output.magnetic_field.y = output_data.mag[1]/2500.0; 44 | output.magnetic_field.z = output_data.mag[2]/2500.0; 45 | 46 | for(int i = 0; i < 8; i++) 47 | { 48 | output.radio_channel[i] = output_data.radio[i]; 49 | } 50 | //for(int i = 0; i < 4; i++) 51 | // output.motor_rpm[i] = output_data.rpm[i]; 52 | 53 | output.seq = output_data.seq; 54 | 55 | return true; 56 | } 57 | 58 | bool decodeStatusData(const std::vector &data, 59 | quadrotor_msgs::StatusData &status) 60 | { 61 | struct STATUS_DATA status_data; 62 | if(data.size() != sizeof(status_data)) 63 | return false; 64 | memcpy(&status_data, &data[0], sizeof(status_data)); 65 | 66 | status.loop_rate = status_data.loop_rate; 67 | status.voltage = status_data.voltage/1e3; 68 | status.seq = status_data.seq; 69 | 70 | return true; 71 | } 72 | 73 | bool decodePPROutputData(const std::vector &data, 74 | quadrotor_msgs::PPROutputData &output) 75 | { 76 | struct PPR_OUTPUT_DATA output_data; 77 | if(data.size() != sizeof(output_data)) 78 | return false; 79 | memcpy(&output_data, &data[0], sizeof(output_data)); 80 | 81 | output.quad_time = output_data.time; 82 | output.des_thrust = output_data.des_thrust*1e-4; 83 | output.des_roll = output_data.des_roll*1e-4; 84 | output.des_pitch = output_data.des_pitch*1e-4; 85 | output.des_yaw = output_data.des_yaw*1e-4; 86 | output.est_roll = output_data.est_roll*1e-4; 87 | output.est_pitch = output_data.est_pitch*1e-4; 88 | output.est_yaw = output_data.est_yaw*1e-4; 89 | output.est_angvel_x = output_data.est_angvel_x*1e-3; 90 | output.est_angvel_y = output_data.est_angvel_y*1e-3; 91 | output.est_angvel_z = output_data.est_angvel_z*1e-3; 92 | output.est_acc_x = output_data.est_acc_x*1e-4; 93 | output.est_acc_y = output_data.est_acc_y*1e-4; 94 | output.est_acc_z = output_data.est_acc_z*1e-4; 95 | output.pwm[0] = output_data.pwm1; 96 | output.pwm[1] = output_data.pwm2; 97 | output.pwm[2] = output_data.pwm3; 98 | output.pwm[3] = output_data.pwm4; 99 | 100 | return true; 101 | } 102 | 103 | } 104 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Gains.py: -------------------------------------------------------------------------------- 1 | """autogenerated by genpy from quadrotor_msgs/Gains.msg. Do not edit.""" 2 | import sys 3 | python3 = True if sys.hexversion > 0x03000000 else False 4 | import genpy 5 | import struct 6 | 7 | 8 | class Gains(genpy.Message): 9 | _md5sum = "b82763b9f24e5595e2a816aa779c9461" 10 | _type = "quadrotor_msgs/Gains" 11 | _has_header = False #flag to mark the presence of a Header object 12 | _full_text = """float64 Kp 13 | float64 Kd 14 | float64 Kp_yaw 15 | float64 Kd_yaw 16 | 17 | """ 18 | __slots__ = ['Kp','Kd','Kp_yaw','Kd_yaw'] 19 | _slot_types = ['float64','float64','float64','float64'] 20 | 21 | def __init__(self, *args, **kwds): 22 | """ 23 | Constructor. Any message fields that are implicitly/explicitly 24 | set to None will be assigned a default value. The recommend 25 | use is keyword arguments as this is more robust to future message 26 | changes. You cannot mix in-order arguments and keyword arguments. 27 | 28 | The available fields are: 29 | Kp,Kd,Kp_yaw,Kd_yaw 30 | 31 | :param args: complete set of field values, in .msg order 32 | :param kwds: use keyword arguments corresponding to message field names 33 | to set specific fields. 34 | """ 35 | if args or kwds: 36 | super(Gains, self).__init__(*args, **kwds) 37 | #message fields cannot be None, assign default values for those that are 38 | if self.Kp is None: 39 | self.Kp = 0. 40 | if self.Kd is None: 41 | self.Kd = 0. 42 | if self.Kp_yaw is None: 43 | self.Kp_yaw = 0. 44 | if self.Kd_yaw is None: 45 | self.Kd_yaw = 0. 46 | else: 47 | self.Kp = 0. 48 | self.Kd = 0. 49 | self.Kp_yaw = 0. 50 | self.Kd_yaw = 0. 51 | 52 | def _get_types(self): 53 | """ 54 | internal API method 55 | """ 56 | return self._slot_types 57 | 58 | def serialize(self, buff): 59 | """ 60 | serialize message into buffer 61 | :param buff: buffer, ``StringIO`` 62 | """ 63 | try: 64 | _x = self 65 | buff.write(_struct_4d.pack(_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw)) 66 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x)))) 67 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x)))) 68 | 69 | def deserialize(self, str): 70 | """ 71 | unpack serialized message in str into this message instance 72 | :param str: byte array of serialized message, ``str`` 73 | """ 74 | try: 75 | end = 0 76 | _x = self 77 | start = end 78 | end += 32 79 | (_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw,) = _struct_4d.unpack(str[start:end]) 80 | return self 81 | except struct.error as e: 82 | raise genpy.DeserializationError(e) #most likely buffer underfill 83 | 84 | 85 | def serialize_numpy(self, buff, numpy): 86 | """ 87 | serialize message with numpy array types into buffer 88 | :param buff: buffer, ``StringIO`` 89 | :param numpy: numpy python module 90 | """ 91 | try: 92 | _x = self 93 | buff.write(_struct_4d.pack(_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw)) 94 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x)))) 95 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x)))) 96 | 97 | def deserialize_numpy(self, str, numpy): 98 | """ 99 | unpack serialized message in str into this message instance using numpy for array types 100 | :param str: byte array of serialized message, ``str`` 101 | :param numpy: numpy python module 102 | """ 103 | try: 104 | end = 0 105 | _x = self 106 | start = end 107 | end += 32 108 | (_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw,) = _struct_4d.unpack(str[start:end]) 109 | return self 110 | except struct.error as e: 111 | raise genpy.DeserializationError(e) #most likely buffer underfill 112 | 113 | _struct_I = genpy.struct_I 114 | _struct_4d = struct.Struct("<4d") 115 | -------------------------------------------------------------------------------- /src/planner/plan_manage/launch/simulator.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from ament_index_python.packages import get_package_share_directory 3 | from launch import LaunchDescription 4 | from launch_ros.actions import Node 5 | from ament_index_python.packages import get_package_prefix 6 | from launch.substitutions import LaunchConfiguration 7 | from launch.actions import ExecuteProcess 8 | def generate_launch_description(): 9 | 10 | poscmd_2_odom_node = Node( 11 | package='poscmd_2_odom', 12 | executable='poscmd_2_odom', 13 | output='screen', 14 | name=LaunchConfiguration('name_of_poscmd_2_odom'), 15 | # parameters=[local_planner_para_dir,] 16 | remappings=[ 17 | # 重映射 position_cmd 话题 18 | ('command', LaunchConfiguration('command')), 19 | ('odometry', LaunchConfiguration('odometry')), 20 | ], 21 | parameters=[ 22 | # local_planner_para_dir,#9 23 | {'init_x': LaunchConfiguration('init_x_')}, 24 | {'init_y': LaunchConfiguration('init_y_')}, 25 | {'init_z': LaunchConfiguration('init_z_')}, 26 | ] 27 | ) 28 | odom_visualization_node = Node( 29 | package='odom_visualization', 30 | executable='odom_visualization', 31 | output='screen', 32 | name=LaunchConfiguration('name_of_odom_visualization'), 33 | # parameters=[local_planner_para_dir,] 34 | remappings=[ 35 | # 重映射 position_cmd 话题 36 | ('odom', LaunchConfiguration('odom')), 37 | ('robot', LaunchConfiguration('robot')), 38 | 39 | ('optimal_list', LaunchConfiguration('optimal_list')), 40 | ('path', LaunchConfiguration('path')), 41 | # ('odometry', LaunchConfiguration('odometry')), 42 | ], 43 | parameters=[ 44 | # local_planner_para_dir,#9 45 | {'color/a': 1.0}, 46 | {'color/r': 0.0}, 47 | {'color/g': 0.0}, 48 | {'color/b': 0.0}, 49 | {'covariance_scale': 100.0}, 50 | {'robot_scale': 1.0}, 51 | {'tf45': False}, 52 | {'drone_id': LaunchConfiguration('drone_id')}, 53 | # {'robot_scale': 1.0}, 54 | ] 55 | ) 56 | 57 | local_sensing_node = Node( 58 | package='local_sensing_node', 59 | executable='pcl_render_node', 60 | output='screen', 61 | name=LaunchConfiguration('name_of_local_sensing_node'), 62 | # parameters=[local_planner_para_dir,] 63 | remappings=[ 64 | # 重映射 position_cmd 话题 65 | ('global_map', "/map_generator/global_cloud"), 66 | ('odometry', LaunchConfiguration('odometry')), 67 | ('pcl_render_node/cloud', LaunchConfiguration('pcl_render_node/cloud')), 68 | # ('path', LaunchConfiguration('path')), 69 | # ('odometry', LaunchConfiguration('odometry')), 70 | ], 71 | parameters=[ 72 | # local_planner_para_dir,#9 73 | {'sensing_horizon': 5.0}, 74 | {'sensing_rate': 30.0}, 75 | {'estimation_rate': 30.0}, 76 | {'map/x_size': LaunchConfiguration('map_size_x_')}, 77 | {'map/y_size': LaunchConfiguration('map_size_y_')}, 78 | {'map/z_size': LaunchConfiguration('map_size_z_')}, 79 | 80 | {'cam_width': 640}, 81 | {'cam_height': 480}, 82 | {'cam_fx': 387.229248046875}, 83 | {'cam_fy': 321.04638671875}, 84 | {'cam_cx': 321.04638671875}, 85 | {'cam_cy': 243.44969177246094}, 86 | # {'tf45': False}, 87 | # {'drone_id': LaunchConfiguration('drone_id')}, 88 | # {'robot_scale': 1.0}, 89 | ] 90 | ) 91 | # print("hhhhhhhhhhhhhhhhhhhhhhhhhhhsimulator------------") 92 | ld = LaunchDescription() 93 | ld.add_action(poscmd_2_odom_node) 94 | ld.add_action(odom_visualization_node) 95 | ld.add_action(local_sensing_node) 96 | 97 | 98 | return ld 99 | -------------------------------------------------------------------------------- /src/uav_simulator/fake_drone/src/poscmd_2_odom.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "rclcpp/rclcpp.hpp" 6 | #include 7 | #include "quadrotor_msgs/msg/position_command.hpp" 8 | 9 | rclcpp::Subscription::SharedPtr _cmd_sub; 10 | rclcpp::Publisher::SharedPtr _odom_pub; 11 | rclcpp::Clock _clock; 12 | quadrotor_msgs::msg::PositionCommand::SharedPtr _cmd; 13 | double _init_x, _init_y, _init_z; 14 | 15 | bool rcv_cmd = false; 16 | void rcvPosCmdCallBack(const quadrotor_msgs::msg::PositionCommand::SharedPtr cmd) 17 | { 18 | rcv_cmd = true; 19 | _cmd = cmd; 20 | } 21 | 22 | void pubOdom() 23 | { 24 | nav_msgs::msg::Odometry odom; 25 | odom.header.stamp = _clock.now(); 26 | odom.header.frame_id = "world"; 27 | 28 | if(rcv_cmd) 29 | { 30 | odom.pose.pose.position.x = _cmd->position.x; 31 | odom.pose.pose.position.y = _cmd->position.y; 32 | odom.pose.pose.position.z = _cmd->position.z; 33 | 34 | Eigen::Vector3d alpha = Eigen::Vector3d(_cmd->acceleration.x, _cmd->acceleration.y, _cmd->acceleration.z) + 9.8*Eigen::Vector3d(0,0,1); 35 | Eigen::Vector3d xC(cos(_cmd->yaw), sin(_cmd->yaw), 0); 36 | Eigen::Vector3d yC(-sin(_cmd->yaw), cos(_cmd->yaw), 0); 37 | Eigen::Vector3d xB = (yC.cross(alpha)).normalized(); 38 | Eigen::Vector3d yB = (alpha.cross(xB)).normalized(); 39 | Eigen::Vector3d zB = xB.cross(yB); 40 | Eigen::Matrix3d R; 41 | R.col(0) = xB; 42 | R.col(1) = yB; 43 | R.col(2) = zB; 44 | Eigen::Quaterniond q(R); 45 | odom.pose.pose.orientation.w = q.w(); 46 | odom.pose.pose.orientation.x = q.x(); 47 | odom.pose.pose.orientation.y = q.y(); 48 | odom.pose.pose.orientation.z = q.z(); 49 | 50 | odom.twist.twist.linear.x = _cmd->velocity.x; 51 | odom.twist.twist.linear.y = _cmd->velocity.y; 52 | odom.twist.twist.linear.z = _cmd->velocity.z; 53 | 54 | odom.twist.twist.angular.x = _cmd->acceleration.x; 55 | odom.twist.twist.angular.y = _cmd->acceleration.y; 56 | odom.twist.twist.angular.z = _cmd->acceleration.z; 57 | } 58 | else 59 | { 60 | odom.pose.pose.position.x = _init_x; 61 | odom.pose.pose.position.y = _init_y; 62 | odom.pose.pose.position.z = _init_z; 63 | 64 | odom.pose.pose.orientation.w = 1; 65 | odom.pose.pose.orientation.x = 0; 66 | odom.pose.pose.orientation.y = 0; 67 | odom.pose.pose.orientation.z = 0; 68 | 69 | odom.twist.twist.linear.x = 0.0; 70 | odom.twist.twist.linear.y = 0.0; 71 | odom.twist.twist.linear.z = 0.0; 72 | 73 | odom.twist.twist.angular.x = 0.0; 74 | odom.twist.twist.angular.y = 0.0; 75 | odom.twist.twist.angular.z = 0.0; 76 | } 77 | 78 | _odom_pub->publish(odom); 79 | } 80 | 81 | int main (int argc, char** argv) 82 | { 83 | // ros::init (argc, argv, "odom_generator"); 84 | // ros::NodeHandle nh( "~" ); 85 | rclcpp::init(argc, argv); 86 | auto nh = std::make_shared("odom_generator");//这个只是说节点的名字叫这个 87 | 88 | nh->declare_parameter("init_x",0.0); 89 | nh->declare_parameter("init_y",0.0); 90 | nh->declare_parameter("init_z",0.0); 91 | nh->get_parameter("init_x",_init_x); 92 | nh->get_parameter("init_y",_init_y); 93 | nh->get_parameter("init_z",_init_z); 94 | 95 | // nh.param("init_x", _init_x, 0.0); 96 | // nh.param("init_y", _init_y, 0.0); 97 | // nh.param("init_z", _init_z, 0.0); 98 | 99 | // _cmd_sub = nh.subscribe( "command", 1, rcvPosCmdCallBack ); 100 | _cmd_sub = nh->create_subscription("command",1,rcvPosCmdCallBack); 101 | 102 | // _odom_pub = nh.advertise("odometry", 1); 103 | _odom_pub = nh->create_publisher("odometry", 1); 104 | 105 | rclcpp::Rate rate(100); 106 | bool status = rclcpp::ok(); 107 | while(status) 108 | { 109 | pubOdom(); 110 | // ros::spinOnce(); 111 | rclcpp::spin_some(nh); 112 | status = rclcpp::ok(); 113 | rate.sleep(); 114 | } 115 | 116 | return 0; 117 | } -------------------------------------------------------------------------------- /src/uav_simulator/local_sensing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | PROJECT(local_sensing_node) 2 | cmake_minimum_required(VERSION 3.5) 3 | SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo 4 | #SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 5 | ADD_COMPILE_OPTIONS(-std=c++11 ) 6 | ADD_COMPILE_OPTIONS(-std=c++14 ) 7 | 8 | #set(ENABLE_CUDA false) 9 | set(ENABLE_CUDA false) 10 | 11 | if(ENABLE_CUDA) 12 | find_package(CUDA REQUIRED) 13 | SET(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS};-O3 -use_fast_math) 14 | set(CUDA_NVCC_FLAGS 15 | # -gencode arch=compute_20,code=sm_20; 16 | # -gencode arch=compute_20,code=sm_21; 17 | # -gencode arch=compute_30,code=sm_30; 18 | # -gencode arch=compute_35,code=sm_35; 19 | # -gencode arch=compute_50,code=sm_50; 20 | # -gencode arch=compute_52,code=sm_52; 21 | # -gencode arch=compute_60,code=sm_60; 22 | # -gencode arch=compute_61,code=sm_61; 23 | -gencode arch=compute_75,code=sm_75; 24 | ) 25 | 26 | SET(CUDA_PROPAGATE_HOST_FLAGS OFF) 27 | 28 | find_package(OpenCV REQUIRED) 29 | find_package(Eigen3 REQUIRED) 30 | find_package(Boost REQUIRED COMPONENTS system filesystem) 31 | 32 | find_package(catkin REQUIRED COMPONENTS 33 | roscpp roslib cmake_modules cv_bridge image_transport pcl_ros sensor_msgs geometry_msgs nav_msgs quadrotor_msgs dynamic_reconfigure) 34 | generate_dynamic_reconfigure_options( 35 | cfg/local_sensing_node.cfg 36 | ) 37 | catkin_package( 38 | DEPENDS OpenCV Eigen Boost 39 | CATKIN_DEPENDS roscpp roslib image_transport pcl_ros 40 | #INCLUDE_DIRS include 41 | LIBRARIES depth_render_cuda 42 | ) 43 | 44 | include_directories( 45 | SYSTEM 46 | #include 47 | ${catkin_INCLUDE_DIRS} 48 | ${OpenCV_INCLUDE_DIRS} 49 | ${Eigen_INCLUDE_DIRS} 50 | ${Boost_INCLUDE_DIRS} 51 | ) 52 | 53 | CUDA_ADD_LIBRARY( depth_render_cuda 54 | src/depth_render.cu 55 | ) 56 | 57 | add_executable( 58 | pcl_render_node 59 | src/pcl_render_node.cpp 60 | ) 61 | target_link_libraries( pcl_render_node 62 | depth_render_cuda 63 | ${OpenCV_LIBS} 64 | ${Boost_LIBRARIES} 65 | ${catkin_LIBRARIES} 66 | ) 67 | else(ENABLE_CUDA) 68 | 69 | # find_package(catkin REQUIRED COMPONENTS 70 | # roscpp roslib cmake_modules pcl_ros sensor_msgs geometry_msgs nav_msgs quadrotor_msgs) 71 | 72 | # catkin_package( 73 | # DEPENDS Eigen 74 | # CATKIN_DEPENDS roscpp roslib pcl_ros 75 | # ) 76 | find_package(ament_cmake REQUIRED) 77 | find_package(rclcpp REQUIRED) 78 | find_package(pcl_ros REQUIRED) 79 | find_package(sensor_msgs REQUIRED) 80 | find_package(geometry_msgs REQUIRED) 81 | find_package(nav_msgs REQUIRED) 82 | find_package(quadrotor_msgs REQUIRED) 83 | set(MY_DEPENDENCIES 84 | rclcpp 85 | pcl_ros 86 | sensor_msgs 87 | geometry_msgs 88 | nav_msgs 89 | quadrotor_msgs 90 | # PCL 91 | ) 92 | find_package(Eigen3 REQUIRED) 93 | 94 | 95 | include_directories( 96 | SYSTEM 97 | # ${catkin_INCLUDE_DIRS} 98 | ${Eigen_INCLUDE_DIRS}. 99 | ) 100 | 101 | add_executable( 102 | pcl_render_node 103 | src/pointcloud_render_node.cpp 104 | ) 105 | ament_target_dependencies(pcl_render_node 106 | ${MY_DEPENDENCIES} 107 | ) 108 | target_link_libraries( pcl_render_node 109 | # ${catkin_LIBRARIES} 110 | ${PCL_LIBRARIES} 111 | ) 112 | 113 | install(TARGETS 114 | pcl_render_node 115 | DESTINATION lib/${PROJECT_NAME} 116 | ) 117 | # install(DIRECTORY launch 118 | # DESTINATION share/${PROJECT_NAME} 119 | # ) 120 | # install(DIRECTORY config 121 | # DESTINATION share/${PROJECT_NAME}/#把存yaml文件的文件夹config移到share 122 | # ) 123 | if(BUILD_TESTING) 124 | find_package(ament_lint_auto REQUIRED) 125 | # the following line skips the linter which checks for copyrights 126 | # uncomment the line when a copyright and license is not present in all source files 127 | #set(ament_cmake_copyright_FOUND TRUE) 128 | # the following line skips cpplint (only works in a git repo) 129 | # uncomment the line when this package is not in a git repo 130 | #set(ament_cmake_cpplint_FOUND TRUE) 131 | ament_lint_auto_find_test_dependencies() 132 | endif() 133 | ##1.自身工程的.h库 134 | ament_package() 135 | endif(ENABLE_CUDA) 136 | -------------------------------------------------------------------------------- /src/uav_simulator/local_sensing/CMakeModules/FindCUDA/parse_cubin.cmake: -------------------------------------------------------------------------------- 1 | # James Bigler, NVIDIA Corp (nvidia.com - jbigler) 2 | # Abe Stephens, SCI Institute -- http://www.sci.utah.edu/~abe/FindCuda.html 3 | # 4 | # Copyright (c) 2008 - 2009 NVIDIA Corporation. All rights reserved. 5 | # 6 | # Copyright (c) 2007-2009 7 | # Scientific Computing and Imaging Institute, University of Utah 8 | # 9 | # This code is licensed under the MIT License. See the FindCUDA.cmake script 10 | # for the text of the license. 11 | 12 | # The MIT License 13 | # 14 | # License for the specific language governing rights and limitations under 15 | # Permission is hereby granted, free of charge, to any person obtaining a 16 | # copy of this software and associated documentation files (the "Software"), 17 | # to deal in the Software without restriction, including without limitation 18 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 19 | # and/or sell copies of the Software, and to permit persons to whom the 20 | # Software is furnished to do so, subject to the following conditions: 21 | # 22 | # The above copyright notice and this permission notice shall be included 23 | # in all copies or substantial portions of the Software. 24 | # 25 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 26 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 27 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 28 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 29 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 30 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 31 | # DEALINGS IN THE SOFTWARE. 32 | # 33 | 34 | ####################################################################### 35 | # Parses a .cubin file produced by nvcc and reports statistics about the file. 36 | 37 | 38 | file(READ ${input_file} file_text) 39 | 40 | if (${file_text} MATCHES ".+") 41 | 42 | # Remember, four backslashes is escaped to one backslash in the string. 43 | string(REGEX REPLACE ";" "\\\\;" file_text ${file_text}) 44 | string(REGEX REPLACE "\ncode" ";code" file_text ${file_text}) 45 | 46 | list(LENGTH file_text len) 47 | 48 | foreach(line ${file_text}) 49 | 50 | # Only look at "code { }" blocks. 51 | if(line MATCHES "^code") 52 | 53 | # Break into individual lines. 54 | string(REGEX REPLACE "\n" ";" line ${line}) 55 | 56 | foreach(entry ${line}) 57 | 58 | # Extract kernel names. 59 | if (${entry} MATCHES "[^g]name = ([^ ]+)") 60 | string(REGEX REPLACE ".* = ([^ ]+)" "\\1" entry ${entry}) 61 | 62 | # Check to see if the kernel name starts with "_" 63 | set(skip FALSE) 64 | # if (${entry} MATCHES "^_") 65 | # Skip the rest of this block. 66 | # message("Skipping ${entry}") 67 | # set(skip TRUE) 68 | # else (${entry} MATCHES "^_") 69 | message("Kernel: ${entry}") 70 | # endif (${entry} MATCHES "^_") 71 | 72 | endif(${entry} MATCHES "[^g]name = ([^ ]+)") 73 | 74 | # Skip the rest of the block if necessary 75 | if(NOT skip) 76 | 77 | # Registers 78 | if (${entry} MATCHES "reg([ ]+)=([ ]+)([^ ]+)") 79 | string(REGEX REPLACE ".*([ ]+)=([ ]+)([^ ]+)" "\\3" entry ${entry}) 80 | message("Registers: ${entry}") 81 | endif() 82 | 83 | # Local memory 84 | if (${entry} MATCHES "lmem([ ]+)=([ ]+)([^ ]+)") 85 | string(REGEX REPLACE ".*([ ]+)=([ ]+)([^ ]+)" "\\3" entry ${entry}) 86 | message("Local: ${entry}") 87 | endif() 88 | 89 | # Shared memory 90 | if (${entry} MATCHES "smem([ ]+)=([ ]+)([^ ]+)") 91 | string(REGEX REPLACE ".*([ ]+)=([ ]+)([^ ]+)" "\\3" entry ${entry}) 92 | message("Shared: ${entry}") 93 | endif() 94 | 95 | if (${entry} MATCHES "^}") 96 | message("") 97 | endif() 98 | 99 | endif(NOT skip) 100 | 101 | 102 | endforeach(entry) 103 | 104 | endif(line MATCHES "^code") 105 | 106 | endforeach(line) 107 | 108 | else() 109 | # message("FOUND NO DEPENDS") 110 | endif() 111 | 112 | 113 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Corrections.py: -------------------------------------------------------------------------------- 1 | """autogenerated by genpy from quadrotor_msgs/Corrections.msg. Do not edit.""" 2 | import sys 3 | python3 = True if sys.hexversion > 0x03000000 else False 4 | import genpy 5 | import struct 6 | 7 | 8 | class Corrections(genpy.Message): 9 | _md5sum = "61e86887a75fe520847d3256306360f5" 10 | _type = "quadrotor_msgs/Corrections" 11 | _has_header = False #flag to mark the presence of a Header object 12 | _full_text = """float64 kf_correction 13 | float64[2] angle_corrections 14 | 15 | """ 16 | __slots__ = ['kf_correction','angle_corrections'] 17 | _slot_types = ['float64','float64[2]'] 18 | 19 | def __init__(self, *args, **kwds): 20 | """ 21 | Constructor. Any message fields that are implicitly/explicitly 22 | set to None will be assigned a default value. The recommend 23 | use is keyword arguments as this is more robust to future message 24 | changes. You cannot mix in-order arguments and keyword arguments. 25 | 26 | The available fields are: 27 | kf_correction,angle_corrections 28 | 29 | :param args: complete set of field values, in .msg order 30 | :param kwds: use keyword arguments corresponding to message field names 31 | to set specific fields. 32 | """ 33 | if args or kwds: 34 | super(Corrections, self).__init__(*args, **kwds) 35 | #message fields cannot be None, assign default values for those that are 36 | if self.kf_correction is None: 37 | self.kf_correction = 0. 38 | if self.angle_corrections is None: 39 | self.angle_corrections = [0.,0.] 40 | else: 41 | self.kf_correction = 0. 42 | self.angle_corrections = [0.,0.] 43 | 44 | def _get_types(self): 45 | """ 46 | internal API method 47 | """ 48 | return self._slot_types 49 | 50 | def serialize(self, buff): 51 | """ 52 | serialize message into buffer 53 | :param buff: buffer, ``StringIO`` 54 | """ 55 | try: 56 | buff.write(_struct_d.pack(self.kf_correction)) 57 | buff.write(_struct_2d.pack(*self.angle_corrections)) 58 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x)))) 59 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x)))) 60 | 61 | def deserialize(self, str): 62 | """ 63 | unpack serialized message in str into this message instance 64 | :param str: byte array of serialized message, ``str`` 65 | """ 66 | try: 67 | end = 0 68 | start = end 69 | end += 8 70 | (self.kf_correction,) = _struct_d.unpack(str[start:end]) 71 | start = end 72 | end += 16 73 | self.angle_corrections = _struct_2d.unpack(str[start:end]) 74 | return self 75 | except struct.error as e: 76 | raise genpy.DeserializationError(e) #most likely buffer underfill 77 | 78 | 79 | def serialize_numpy(self, buff, numpy): 80 | """ 81 | serialize message with numpy array types into buffer 82 | :param buff: buffer, ``StringIO`` 83 | :param numpy: numpy python module 84 | """ 85 | try: 86 | buff.write(_struct_d.pack(self.kf_correction)) 87 | buff.write(self.angle_corrections.tostring()) 88 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x)))) 89 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x)))) 90 | 91 | def deserialize_numpy(self, str, numpy): 92 | """ 93 | unpack serialized message in str into this message instance using numpy for array types 94 | :param str: byte array of serialized message, ``str`` 95 | :param numpy: numpy python module 96 | """ 97 | try: 98 | end = 0 99 | start = end 100 | end += 8 101 | (self.kf_correction,) = _struct_d.unpack(str[start:end]) 102 | start = end 103 | end += 16 104 | self.angle_corrections = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=2) 105 | return self 106 | except struct.error as e: 107 | raise genpy.DeserializationError(e) #most likely buffer underfill 108 | 109 | _struct_I = genpy.struct_I 110 | _struct_2d = struct.Struct("<2d") 111 | _struct_d = struct.Struct(" 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | -------------------------------------------------------------------------------- /src/planner/plan_manage/include/plan_manage/ego_replan_fsm.h: -------------------------------------------------------------------------------- 1 | #ifndef _REBO_REPLAN_FSM_H_ 2 | #define _REBO_REPLAN_FSM_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include "rclcpp/rclcpp.hpp" 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | using std::vector; 31 | 32 | namespace ego_planner 33 | { 34 | 35 | class EGOReplanFSM 36 | { 37 | 38 | private: 39 | /* ---------- flag ---------- */ 40 | enum FSM_EXEC_STATE 41 | { 42 | INIT, 43 | WAIT_TARGET, 44 | GEN_NEW_TRAJ, 45 | REPLAN_TRAJ, 46 | EXEC_TRAJ, 47 | EMERGENCY_STOP, 48 | SEQUENTIAL_START 49 | }; 50 | enum TARGET_TYPE 51 | { 52 | MANUAL_TARGET = 1, 53 | PRESET_TARGET = 2, 54 | REFENCE_PATH = 3 55 | }; 56 | 57 | /* planning utils */ 58 | EGOPlannerManager::Ptr planner_manager_; 59 | PlanningVisualization::Ptr visualization_; 60 | traj_utils::msg::DataDisp data_disp_; 61 | traj_utils::msg::MultiBsplines multi_bspline_msgs_buf_; 62 | 63 | /* parameters */ 64 | int target_type_; // 1 mannual select, 2 hard code 65 | double no_replan_thresh_, replan_thresh_; 66 | double waypoints_[50][3]; 67 | int waypoint_num_, wp_id_; 68 | double planning_horizen_, planning_horizen_time_; 69 | double emergency_time_; 70 | bool flag_realworld_experiment_; 71 | bool enable_fail_safe_; 72 | 73 | /* planning data */ 74 | bool have_trigger_, have_target_, have_odom_, have_new_target_, have_recv_pre_agent_; 75 | FSM_EXEC_STATE exec_state_; 76 | int continously_called_times_{0}; 77 | 78 | Eigen::Vector3d odom_pos_, odom_vel_, odom_acc_; // odometry state 79 | Eigen::Quaterniond odom_orient_; 80 | 81 | Eigen::Vector3d init_pt_, start_pt_, start_vel_, start_acc_, start_yaw_; // start state 82 | Eigen::Vector3d end_pt_, end_vel_; // goal state 83 | Eigen::Vector3d local_target_pt_, local_target_vel_; // local target state 84 | std::vector wps_; 85 | int current_wp_; 86 | 87 | bool flag_escape_emergency_; 88 | 89 | /* ROS utils */ 90 | std::shared_ptr node_; 91 | rclcpp::TimerBase::SharedPtr exec_timer_, safety_timer_; 92 | // ros::Subscriber waypoint_sub_, , trigger_sub_; 93 | // ros::Publisher replan_pub_, new_pub_, bspline_pub_, data_disp_pub_, , ; 94 | rclcpp::Subscription::SharedPtr odom_sub_; 95 | rclcpp::Subscription::SharedPtr swarm_trajs_sub_; 96 | rclcpp::Subscription::SharedPtr broadcast_bspline_sub_; 97 | rclcpp::Subscription::SharedPtr waypoint_sub_; 98 | rclcpp::Subscription::SharedPtr trigger_sub_; 99 | 100 | rclcpp::Publisher::SharedPtr swarm_trajs_pub_; 101 | rclcpp::Publisher::SharedPtr broadcast_bspline_pub_; 102 | rclcpp::Publisher::SharedPtr bspline_pub_; 103 | rclcpp::Publisher::SharedPtr data_disp_pub_; 104 | 105 | /* helper functions */ 106 | bool callReboundReplan(bool flag_use_poly_init, bool flag_randomPolyTraj); // front-end and back-end method 107 | bool callEmergencyStop(Eigen::Vector3d stop_pos); // front-end and back-end method 108 | bool planFromGlobalTraj(const int trial_times = 1); 109 | bool planFromCurrentTraj(const int trial_times = 1); 110 | 111 | /* return value: std::pair< Times of the same state be continuously called, current continuously called state > */ 112 | void changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call); 113 | std::pair timesOfConsecutiveStateCalls(); 114 | void printFSMExecState(); 115 | 116 | void readGivenWps(); 117 | void planNextWaypoint(const Eigen::Vector3d next_wp); 118 | void getLocalTarget(); 119 | 120 | /* ROS functions */ 121 | void execFSMCallback(); 122 | void checkCollisionCallback(); 123 | void waypointCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); 124 | void triggerCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); 125 | void odometryCallback(const nav_msgs::msg::Odometry::SharedPtr msg); 126 | void swarmTrajsCallback(const traj_utils::msg::MultiBsplines::SharedPtr msg); 127 | void BroadcastBsplineCallback(const traj_utils::msg::Bspline::SharedPtr msg); 128 | 129 | bool checkCollision(); 130 | void publishSwarmTrajs(bool startup_pub); 131 | 132 | public: 133 | EGOReplanFSM(/* args */) 134 | { 135 | } 136 | ~EGOReplanFSM() 137 | { 138 | } 139 | 140 | void init(std::shared_ptr &nh); 141 | 142 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 143 | rclcpp::Clock _clock; 144 | }; 145 | 146 | } // namespace ego_planner 147 | 148 | #endif -------------------------------------------------------------------------------- /src/planner/plan_env/include/plan_env/obj_predictor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of Fast-Planner. 3 | * 4 | * Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, 5 | * Developed by Boyu Zhou , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * Fast-Planner is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU Lesser General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * Fast-Planner is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU Lesser General Public License 21 | * along with Fast-Planner. If not, see . 22 | */ 23 | 24 | 25 | 26 | #ifndef _OBJ_PREDICTOR_H_ 27 | #define _OBJ_PREDICTOR_H_ 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include "rclcpp/rclcpp.hpp" 35 | #include 36 | 37 | using std::cout; 38 | using std::endl; 39 | using std::list; 40 | using std::shared_ptr; 41 | using std::unique_ptr; 42 | using std::vector; 43 | 44 | namespace fast_planner { 45 | class PolynomialPrediction; 46 | typedef shared_ptr> ObjPrediction; 47 | typedef shared_ptr> ObjScale; 48 | 49 | /* ========== prediction polynomial ========== */ 50 | class PolynomialPrediction { 51 | private: 52 | vector> polys; 53 | double t1, t2; // start / end 54 | rclcpp::Time global_start_time_; 55 | 56 | public: 57 | PolynomialPrediction(/* args */) { 58 | } 59 | ~PolynomialPrediction() { 60 | } 61 | 62 | void setPolynomial(vector>& pls) { 63 | polys = pls; 64 | } 65 | void setTime(double t1, double t2) { 66 | this->t1 = t1; 67 | this->t2 = t2; 68 | } 69 | void setGlobalStartTime(rclcpp::Time global_start_time) { 70 | global_start_time_ = global_start_time; 71 | } 72 | 73 | bool valid() { 74 | return polys.size() == 3; 75 | } 76 | 77 | /* note that t should be in [t1, t2] */ 78 | Eigen::Vector3d evaluate(double t) { 79 | Eigen::Matrix tv; 80 | tv << 1.0, pow(t, 1), pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5); 81 | 82 | Eigen::Vector3d pt; 83 | pt(0) = tv.dot(polys[0]), pt(1) = tv.dot(polys[1]), pt(2) = tv.dot(polys[2]); 84 | 85 | return pt; 86 | } 87 | 88 | Eigen::Vector3d evaluateConstVel(double t) { 89 | Eigen::Matrix tv; 90 | tv << 1.0, pow(t-global_start_time_.seconds(), 1); 91 | 92 | // cout << t-global_start_time_.toSec() << endl; 93 | 94 | Eigen::Vector3d pt; 95 | pt(0) = tv.dot(polys[0].head(2)), pt(1) = tv.dot(polys[1].head(2)), pt(2) = tv.dot(polys[2].head(2)); 96 | 97 | return pt; 98 | } 99 | }; 100 | 101 | /* ========== subscribe and record object history ========== */ 102 | class ObjHistory { 103 | public: 104 | int skip_num_; 105 | int queue_size_; 106 | rclcpp::Time global_start_time_; 107 | 108 | ObjHistory() { 109 | } 110 | ~ObjHistory() { 111 | } 112 | 113 | void init(int id, int skip_num, int queue_size, rclcpp::Time global_start_time); 114 | 115 | void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); 116 | 117 | void clear() { 118 | history_.clear(); 119 | } 120 | 121 | void getHistory(list& his) { 122 | his = history_; 123 | } 124 | rclcpp::Clock _clock; 125 | private: 126 | list history_; // x,y,z;t 127 | int skip_; 128 | int obj_idx_; 129 | Eigen::Vector3d scale_; 130 | }; 131 | 132 | /* ========== predict future trajectory using history ========== */ 133 | class ObjPredictor { 134 | private: 135 | std::shared_ptr node_handle_; 136 | 137 | int obj_num_; 138 | double lambda_; 139 | double predict_rate_; 140 | 141 | vector::SharedPtr> pose_subs_; 142 | rclcpp::Subscription::SharedPtr marker_sub_; 143 | rclcpp::TimerBase::SharedPtr predict_timer_; 144 | vector> obj_histories_; 145 | 146 | /* share data with planner */ 147 | ObjPrediction predict_trajs_; 148 | ObjScale obj_scale_; 149 | vector scale_init_; 150 | 151 | void markerCallback(const visualization_msgs::msg::Marker::SharedPtr msg); 152 | 153 | void predictCallback(); 154 | void predictPolyFit(); 155 | void predictConstVel(); 156 | 157 | public: 158 | ObjPredictor(/* args */); 159 | ObjPredictor(std::shared_ptr& node); 160 | ~ObjPredictor(); 161 | 162 | void init(); 163 | 164 | ObjPrediction getPredictionTraj(); 165 | ObjScale getObjScale(); 166 | int getObjNums() {return obj_num_;} 167 | 168 | Eigen::Vector3d evaluatePoly(int obs_id, double time); 169 | Eigen::Vector3d evaluateConstVel(int obs_id, double time); 170 | 171 | typedef shared_ptr Ptr; 172 | rclcpp::Clock _clock; 173 | }; 174 | 175 | } // namespace fast_planner 176 | 177 | #endif -------------------------------------------------------------------------------- /src/planner/plan_manage/launch/run_in_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | > 133 | 134 | 135 | 136 | -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_AuxCommand.py: -------------------------------------------------------------------------------- 1 | """autogenerated by genpy from quadrotor_msgs/AuxCommand.msg. Do not edit.""" 2 | import sys 3 | python3 = True if sys.hexversion > 0x03000000 else False 4 | import genpy 5 | import struct 6 | 7 | 8 | class AuxCommand(genpy.Message): 9 | _md5sum = "94f75840e4b1e03675da764692f2c839" 10 | _type = "quadrotor_msgs/AuxCommand" 11 | _has_header = False #flag to mark the presence of a Header object 12 | _full_text = """float64 current_yaw 13 | float64 kf_correction 14 | float64[2] angle_corrections# Trims for roll, pitch 15 | bool enable_motors 16 | bool use_external_yaw 17 | 18 | """ 19 | __slots__ = ['current_yaw','kf_correction','angle_corrections','enable_motors','use_external_yaw'] 20 | _slot_types = ['float64','float64','float64[2]','bool','bool'] 21 | 22 | def __init__(self, *args, **kwds): 23 | """ 24 | Constructor. Any message fields that are implicitly/explicitly 25 | set to None will be assigned a default value. The recommend 26 | use is keyword arguments as this is more robust to future message 27 | changes. You cannot mix in-order arguments and keyword arguments. 28 | 29 | The available fields are: 30 | current_yaw,kf_correction,angle_corrections,enable_motors,use_external_yaw 31 | 32 | :param args: complete set of field values, in .msg order 33 | :param kwds: use keyword arguments corresponding to message field names 34 | to set specific fields. 35 | """ 36 | if args or kwds: 37 | super(AuxCommand, self).__init__(*args, **kwds) 38 | #message fields cannot be None, assign default values for those that are 39 | if self.current_yaw is None: 40 | self.current_yaw = 0. 41 | if self.kf_correction is None: 42 | self.kf_correction = 0. 43 | if self.angle_corrections is None: 44 | self.angle_corrections = [0.,0.] 45 | if self.enable_motors is None: 46 | self.enable_motors = False 47 | if self.use_external_yaw is None: 48 | self.use_external_yaw = False 49 | else: 50 | self.current_yaw = 0. 51 | self.kf_correction = 0. 52 | self.angle_corrections = [0.,0.] 53 | self.enable_motors = False 54 | self.use_external_yaw = False 55 | 56 | def _get_types(self): 57 | """ 58 | internal API method 59 | """ 60 | return self._slot_types 61 | 62 | def serialize(self, buff): 63 | """ 64 | serialize message into buffer 65 | :param buff: buffer, ``StringIO`` 66 | """ 67 | try: 68 | _x = self 69 | buff.write(_struct_2d.pack(_x.current_yaw, _x.kf_correction)) 70 | buff.write(_struct_2d.pack(*self.angle_corrections)) 71 | _x = self 72 | buff.write(_struct_2B.pack(_x.enable_motors, _x.use_external_yaw)) 73 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x)))) 74 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x)))) 75 | 76 | def deserialize(self, str): 77 | """ 78 | unpack serialized message in str into this message instance 79 | :param str: byte array of serialized message, ``str`` 80 | """ 81 | try: 82 | end = 0 83 | _x = self 84 | start = end 85 | end += 16 86 | (_x.current_yaw, _x.kf_correction,) = _struct_2d.unpack(str[start:end]) 87 | start = end 88 | end += 16 89 | self.angle_corrections = _struct_2d.unpack(str[start:end]) 90 | _x = self 91 | start = end 92 | end += 2 93 | (_x.enable_motors, _x.use_external_yaw,) = _struct_2B.unpack(str[start:end]) 94 | self.enable_motors = bool(self.enable_motors) 95 | self.use_external_yaw = bool(self.use_external_yaw) 96 | return self 97 | except struct.error as e: 98 | raise genpy.DeserializationError(e) #most likely buffer underfill 99 | 100 | 101 | def serialize_numpy(self, buff, numpy): 102 | """ 103 | serialize message with numpy array types into buffer 104 | :param buff: buffer, ``StringIO`` 105 | :param numpy: numpy python module 106 | """ 107 | try: 108 | _x = self 109 | buff.write(_struct_2d.pack(_x.current_yaw, _x.kf_correction)) 110 | buff.write(self.angle_corrections.tostring()) 111 | _x = self 112 | buff.write(_struct_2B.pack(_x.enable_motors, _x.use_external_yaw)) 113 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x)))) 114 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x)))) 115 | 116 | def deserialize_numpy(self, str, numpy): 117 | """ 118 | unpack serialized message in str into this message instance using numpy for array types 119 | :param str: byte array of serialized message, ``str`` 120 | :param numpy: numpy python module 121 | """ 122 | try: 123 | end = 0 124 | _x = self 125 | start = end 126 | end += 16 127 | (_x.current_yaw, _x.kf_correction,) = _struct_2d.unpack(str[start:end]) 128 | start = end 129 | end += 16 130 | self.angle_corrections = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=2) 131 | _x = self 132 | start = end 133 | end += 2 134 | (_x.enable_motors, _x.use_external_yaw,) = _struct_2B.unpack(str[start:end]) 135 | self.enable_motors = bool(self.enable_motors) 136 | self.use_external_yaw = bool(self.use_external_yaw) 137 | return self 138 | except struct.error as e: 139 | raise genpy.DeserializationError(e) #most likely buffer underfill 140 | 141 | _struct_I = genpy.struct_I 142 | _struct_2d = struct.Struct("<2d") 143 | _struct_2B = struct.Struct("<2B") 144 | -------------------------------------------------------------------------------- /src/uav_simulator/local_sensing/src/device_image.cuh: -------------------------------------------------------------------------------- 1 | // This file is part of REMODE - REgularized MOnocular Depth Estimation. 2 | // 3 | // Copyright (C) 2014 Matia Pizzoli 4 | // Robotics and Perception Group, University of Zurich, Switzerland 5 | // http://rpg.ifi.uzh.ch 6 | // 7 | // REMODE is free software: you can redistribute it and/or modify it under the 8 | // terms of the GNU General Public License as published by the Free Software 9 | // Foundation, either version 3 of the License, or any later version. 10 | // 11 | // REMODE is distributed in the hope that it will be useful, but WITHOUT ANY 12 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 14 | // 15 | // You should have received a copy of the GNU General Public License 16 | // along with this program. If not, see . 17 | 18 | #ifndef DEVICE_IMAGE_CUH 19 | #define DEVICE_IMAGE_CUH 20 | 21 | #include 22 | #include 23 | #include "cuda_exception.cuh" 24 | 25 | struct Size 26 | { 27 | int width; 28 | int height; 29 | }; 30 | 31 | template 32 | struct DeviceImage 33 | { 34 | __host__ 35 | DeviceImage(size_t width, size_t height) 36 | : width(width), 37 | height(height) 38 | { 39 | cudaError err = cudaMallocPitch( 40 | &data, 41 | &pitch, 42 | width*sizeof(ElementType), 43 | height); 44 | if(err != cudaSuccess) 45 | throw CudaException("Image: unable to allocate pitched memory.", err); 46 | 47 | stride = pitch / sizeof(ElementType); 48 | 49 | err = cudaMalloc( 50 | &dev_ptr, 51 | sizeof(*this)); 52 | if(err != cudaSuccess) 53 | throw CudaException("DeviceData, cannot allocate device memory to store image parameters.", err); 54 | 55 | err = cudaMemcpy( 56 | dev_ptr, 57 | this, 58 | sizeof(*this), 59 | cudaMemcpyHostToDevice); 60 | if(err != cudaSuccess) 61 | throw CudaException("DeviceData, cannot copy image parameters to device memory.", err); 62 | } 63 | 64 | __device__ 65 | ElementType & operator()(size_t x, size_t y) 66 | { 67 | return atXY(x, y); 68 | } 69 | 70 | __device__ 71 | const ElementType & operator()(size_t x, size_t y) const 72 | { 73 | return atXY(x, y); 74 | } 75 | 76 | __device__ 77 | ElementType & atXY(size_t x, size_t y) 78 | { 79 | return data[y*stride+x]; 80 | } 81 | 82 | __device__ 83 | const ElementType & atXY(size_t x, size_t y) const 84 | { 85 | return data[y*stride+x]; 86 | } 87 | 88 | /// Upload aligned_data_row_major to device memory 89 | __host__ 90 | void setDevData(const ElementType * aligned_data_row_major) 91 | { 92 | const cudaError err = cudaMemcpy2D( 93 | data, 94 | pitch, 95 | aligned_data_row_major, 96 | width*sizeof(ElementType), 97 | width*sizeof(ElementType), 98 | height, 99 | cudaMemcpyHostToDevice); 100 | if(err != cudaSuccess) 101 | throw CudaException("Image: unable to copy data from host to device.", err); 102 | } 103 | 104 | /// Download the data from the device memory to aligned_data_row_major, a preallocated array in host memory 105 | __host__ 106 | void getDevData(ElementType* aligned_data_row_major) const 107 | { 108 | const cudaError err = cudaMemcpy2D( 109 | aligned_data_row_major, // destination memory address 110 | width*sizeof(ElementType), // pitch of destination memory 111 | data, // source memory address 112 | pitch, // pitch of source memory 113 | width*sizeof(ElementType), // width of matrix transfer (columns in bytes) 114 | height, // height of matrix transfer 115 | cudaMemcpyDeviceToHost); 116 | if(err != cudaSuccess) 117 | throw CudaException("Image: unable to copy data from device to host.", err); 118 | } 119 | 120 | __host__ 121 | ~DeviceImage() 122 | { 123 | cudaError err = cudaFree(data); 124 | if(err != cudaSuccess) 125 | throw CudaException("Image: unable to free allocated memory.", err); 126 | err = cudaFree(dev_ptr); 127 | if(err != cudaSuccess) 128 | throw CudaException("Image: unable to free allocated memory.", err); 129 | } 130 | 131 | __host__ 132 | cudaChannelFormatDesc getCudaChannelFormatDesc() const 133 | { 134 | return cudaCreateChannelDesc(); 135 | } 136 | 137 | __host__ 138 | void zero() 139 | { 140 | const cudaError err = cudaMemset2D( 141 | data, 142 | pitch, 143 | 0, 144 | width*sizeof(ElementType), 145 | height); 146 | if(err != cudaSuccess) 147 | throw CudaException("Image: unable to zero.", err); 148 | } 149 | 150 | __host__ 151 | DeviceImage & operator=(const DeviceImage &other_image) 152 | { 153 | if(this != &other_image) 154 | { 155 | assert(width == other_image.width && 156 | height == other_image.height); 157 | const cudaError err = cudaMemcpy2D(data, 158 | pitch, 159 | other_image.data, 160 | other_image.pitch, 161 | width*sizeof(ElementType), 162 | height, 163 | cudaMemcpyDeviceToDevice); 164 | if(err != cudaSuccess) 165 | throw CudaException("Image, operator '=': unable to copy data from another image.", err); 166 | } 167 | return *this; 168 | } 169 | 170 | // fields 171 | size_t width; 172 | size_t height; 173 | size_t pitch; 174 | size_t stride; 175 | ElementType * data; 176 | DeviceImage *dev_ptr; 177 | }; 178 | 179 | #endif // DEVICE_IMAGE_CUH 180 | -------------------------------------------------------------------------------- /src/uav_simulator/local_sensing/src/ceres_extensions.h: -------------------------------------------------------------------------------- 1 | // 2 | // ceres_extensions.h 3 | // Bundle_Adjust_Test 4 | // 5 | // Created by Lloyd Hughes on 2014/04/11. 6 | // Copyright (c) 2014 Lloyd Hughes. All rights reserved. 7 | // hughes.lloyd@gmail.com 8 | // 9 | 10 | #ifndef CERES_EXTENSIONS_H 11 | #define CERES_EXTENSIONS_H 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace ceres_ext { 18 | 19 | // Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x 20 | // with * being the quaternion multiplication operator. Here we assume 21 | // that the first element of the quaternion vector is the real (cos 22 | // theta) part. 23 | class EigenQuaternionParameterization : public ceres::LocalParameterization 24 | { 25 | public: 26 | virtual ~EigenQuaternionParameterization() {} 27 | 28 | virtual bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const 29 | { 30 | const Eigen::Map x(x_raw); 31 | const Eigen::Map delta(delta_raw); 32 | 33 | Eigen::Map x_plus_delta(x_plus_delta_raw); 34 | 35 | const double delta_norm = delta.norm(); 36 | if ( delta_norm > 0.0 ) 37 | { 38 | const double sin_delta_by_delta = sin(delta_norm) / delta_norm; 39 | Eigen::Quaterniond tmp( cos(delta_norm), sin_delta_by_delta*delta[0], sin_delta_by_delta*delta[1], sin_delta_by_delta*delta[2] ); 40 | 41 | x_plus_delta = tmp*x; 42 | } 43 | else 44 | { 45 | x_plus_delta = x; 46 | } 47 | return true; 48 | } 49 | 50 | virtual bool ComputeJacobian(const double* x, double* jacobian) const 51 | { 52 | jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT x 53 | jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT y 54 | jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT z 55 | jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT w 56 | return true; 57 | } 58 | 59 | virtual int GlobalSize() const { return 4; } 60 | virtual int LocalSize() const { return 3; } 61 | 62 | }; 63 | 64 | template inline 65 | void EigenQuaternionToScaledRotation(const T q[4], T R[3 * 3]) { 66 | EigenQuaternionToScaledRotation(q, RowMajorAdapter3x3(R)); 67 | } 68 | 69 | template inline 70 | void EigenQuaternionToScaledRotation(const T q[4], 71 | const ceres::MatrixAdapter& R) { 72 | // Make convenient names for elements of q. 73 | T a = q[3]; 74 | T b = q[0]; 75 | T c = q[1]; 76 | T d = q[2]; 77 | // This is not to eliminate common sub-expression, but to 78 | // make the lines shorter so that they fit in 80 columns! 79 | T aa = a * a; 80 | T ab = a * b; 81 | T ac = a * c; 82 | T ad = a * d; 83 | T bb = b * b; 84 | T bc = b * c; 85 | T bd = b * d; 86 | T cc = c * c; 87 | T cd = c * d; 88 | T dd = d * d; 89 | 90 | R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); // NOLINT 91 | R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); // NOLINT 92 | R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; // NOLINT 93 | } 94 | 95 | template inline 96 | void EigenQuaternionToRotation(const T q[4], T R[3 * 3]) { 97 | EigenQuaternionToRotation(q, RowMajorAdapter3x3(R)); 98 | } 99 | 100 | template inline 101 | void EigenQuaternionToRotation(const T q[4], 102 | const ceres::MatrixAdapter& R) { 103 | EigenQuaternionToScaledRotation(q, R); 104 | 105 | T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; 106 | CHECK_NE(normalizer, T(0)); 107 | normalizer = T(1) / normalizer; 108 | 109 | for (int i = 0; i < 3; ++i) { 110 | for (int j = 0; j < 3; ++j) { 111 | R(i, j) *= normalizer; 112 | } 113 | } 114 | } 115 | 116 | template inline 117 | void EigenUnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { 118 | const T t2 = q[3] * q[0]; 119 | const T t3 = q[3] * q[1]; 120 | const T t4 = q[3] * q[2]; 121 | const T t5 = -q[0] * q[0]; 122 | const T t6 = q[0] * q[1]; 123 | const T t7 = q[0] * q[2]; 124 | const T t8 = -q[1] * q[1]; 125 | const T t9 = q[1] * q[2]; 126 | const T t1 = -q[2] * q[2]; 127 | result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT 128 | result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT 129 | result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT 130 | } 131 | 132 | template inline 133 | void EigenQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { 134 | // 'scale' is 1 / norm(q). 135 | const T scale = T(1) / sqrt(q[0] * q[0] + 136 | q[1] * q[1] + 137 | q[2] * q[2] + 138 | q[3] * q[3]); 139 | 140 | // Make unit-norm version of q. 141 | const T unit[4] = { 142 | scale * q[0], 143 | scale * q[1], 144 | scale * q[2], 145 | scale * q[3], 146 | }; 147 | 148 | EigenUnitQuaternionRotatePoint(unit, pt, result); 149 | } 150 | 151 | template inline 152 | void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) { 153 | zw[0] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; 154 | zw[1] = - z[0] * w[2] + z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; 155 | zw[2] = z[0] * w[1] - z[1] * w[0] + z[2] * w[3] + z[3] * w[2]; 156 | zw[3] = - z[0] * w[0] - z[1] * w[1] - z[2] * w[2] + z[3] * w[3]; 157 | } 158 | 159 | } 160 | 161 | #endif -------------------------------------------------------------------------------- /src/uav_simulator/local_sensing/src/depth_render.cu: -------------------------------------------------------------------------------- 1 | #include "depth_render.cuh" 2 | __global__ void render(float3 *data_devptr, Parameter *para_devptr, DeviceImage *depth_devptr) 3 | { 4 | const int index = threadIdx.x + blockIdx.x * blockDim.x; 5 | const Parameter para = *para_devptr; 6 | if(index >= para.point_number) 7 | return; 8 | float3 my_point = data_devptr[index]; 9 | 10 | //transform 11 | float3 trans_point; 12 | 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]; 13 | 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]; 14 | 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]; 15 | 16 | if(trans_point.z <= 0.0f) 17 | return; 18 | 19 | //project 20 | int2 projected; 21 | projected.x = trans_point.x / trans_point.z * para.fx + para.cx + 0.5; 22 | projected.y = trans_point.y / trans_point.z * para.fy + para.cy + 0.5; 23 | if(projected.x < 0 || projected.x >= para.width || projected.y < 0 || projected.y >= para.height) 24 | return; 25 | 26 | // float dist = length(trans_point); 27 | float dist = trans_point.z; 28 | int dist_mm = dist * 1000.0f + 0.5f; 29 | 30 | //int r = 0.0173 * para.fx / dist + 0.5f; 31 | // int r = 0.0473 * para.fx / dist + 0.5f; 32 | int r = 0.0573 * para.fx / dist + 0.5f; 33 | for(int i = -r; i <= r; i++) 34 | for(int j = -r; j <= r; j++) 35 | { 36 | int to_x = projected.x + j; 37 | int to_y = projected.y + i; 38 | if(to_x < 0 || to_x >= para.width || to_y < 0 || to_y >= para.height) 39 | continue; 40 | int *dist_ptr = &(depth_devptr->atXY(to_x, to_y)); 41 | atomicMin(dist_ptr, dist_mm); 42 | } 43 | } 44 | 45 | __global__ void depth_initial(DeviceImage *depth_devptr) 46 | { 47 | const int x = threadIdx.x + blockIdx.x * blockDim.x; 48 | const int y = threadIdx.y + blockIdx.y * blockDim.y; 49 | int width = depth_devptr->width; 50 | int height = depth_devptr->height; 51 | 52 | if(x >= width || y >= height) 53 | return; 54 | 55 | depth_devptr->atXY(x,y) = 999999; 56 | } 57 | 58 | DepthRender::DepthRender(): 59 | cloud_size(0), 60 | host_cloud_ptr(NULL), 61 | dev_cloud_ptr(NULL), 62 | has_devptr(false) 63 | { 64 | } 65 | 66 | DepthRender::~DepthRender() 67 | { 68 | if(has_devptr) 69 | { 70 | free(host_cloud_ptr); 71 | cudaFree(dev_cloud_ptr); 72 | cudaFree(parameter_devptr); 73 | } 74 | } 75 | 76 | void DepthRender::set_para(float _fx, float _fy, float _cx, float _cy, int _width, int _height) 77 | { 78 | parameter.fx = _fx; 79 | parameter.fy = _fy; 80 | parameter.cx = _cx; 81 | parameter.cy = _cy; 82 | parameter.width = _width; 83 | parameter.height = _height; 84 | } 85 | 86 | void DepthRender::set_data(vector &cloud_data) 87 | { 88 | cloud_size = cloud_data.size() / 3; 89 | parameter.point_number = cloud_size; 90 | 91 | host_cloud_ptr = (float3*) malloc(cloud_size * sizeof(float3)); 92 | for(int i = 0; i < cloud_size; i++) 93 | host_cloud_ptr[i] = make_float3(cloud_data[3*i], cloud_data[3*i+1], cloud_data[3*i+2]); 94 | 95 | cudaError err = cudaMalloc(&dev_cloud_ptr, cloud_size * sizeof(float3)); 96 | if(err != cudaSuccess) 97 | throw CudaException("DeviceLinear: unable to allocate linear memory.", err); 98 | err = cudaMemcpy( 99 | dev_cloud_ptr, 100 | host_cloud_ptr, 101 | cloud_size * sizeof(float3), 102 | cudaMemcpyHostToDevice); 103 | if(err != cudaSuccess) 104 | throw CudaException("DeviceLinear: unable to copy data from host to device.", err); 105 | 106 | err = cudaMalloc(¶meter_devptr, sizeof(Parameter)); 107 | if(err != cudaSuccess) 108 | throw CudaException("DeviceLinear: unable to allocate linear memory.", err); 109 | err = cudaMemcpy( 110 | parameter_devptr, 111 | ¶meter, 112 | sizeof(Parameter), 113 | cudaMemcpyHostToDevice); 114 | if(err != cudaSuccess) 115 | throw CudaException("DeviceLinear: unable to copy data from host to device.", err); 116 | 117 | has_devptr = true; 118 | 119 | //printf("load points done!\n"); 120 | } 121 | 122 | /*void DepthRender::render_pose( Matrix3d &rotation, Vector3d &translation, int *host_ptr) 123 | { 124 | for(int i = 0; i < 3; i++) 125 | { 126 | parameter.t[i] = translation(i); 127 | for(int j = 0; j < 3; j++) 128 | { 129 | parameter.r[i][j] = rotation(i,j); 130 | } 131 | } 132 | cudaError err = cudaMemcpy( 133 | parameter_devptr, 134 | ¶meter, 135 | sizeof(Parameter), 136 | cudaMemcpyHostToDevice); 137 | if(err != cudaSuccess) 138 | throw CudaException("DeviceLinear: unable to copy data from host to device.", err); 139 | 140 | DeviceImage depth_output(parameter.width, parameter.height); 141 | depth_output.zero(); 142 | 143 | dim3 depth_block; 144 | dim3 depth_grid; 145 | depth_block.x = 16; 146 | depth_block.y = 16; 147 | depth_grid.x = (parameter.width + depth_block.x - 1 ) / depth_block.x; 148 | depth_grid.y = (parameter.height + depth_block.y - 1 ) / depth_block.y; 149 | depth_initial<<>>(depth_output.dev_ptr); 150 | 151 | dim3 render_block; 152 | dim3 render_grid; 153 | render_block.x = 64; 154 | render_grid.x = (cloud_size + render_block.x - 1) / render_block.x; 155 | render<<>>(dev_cloud_ptr, parameter_devptr, depth_output.dev_ptr); 156 | 157 | depth_output.getDevData(host_ptr); 158 | } 159 | */ 160 | //void DepthRender::render_pose( Matrix4d &transformation, int *host_ptr) 161 | void DepthRender::render_pose( double * transformation, int *host_ptr) 162 | { 163 | for(int i = 0; i < 3; i++) 164 | { 165 | parameter.t[i] = transformation[4 * i + 3];//transformation(i,3); 166 | for(int j = 0; j < 3; j++) 167 | { 168 | parameter.r[i][j] = transformation[4 * i + j];//transformation(i,j); 169 | } 170 | } 171 | cudaError err = cudaMemcpy( 172 | parameter_devptr, 173 | ¶meter, 174 | sizeof(Parameter), 175 | cudaMemcpyHostToDevice); 176 | if(err != cudaSuccess) 177 | throw CudaException("DeviceLinear: unable to copy data from host to device.", err); 178 | 179 | DeviceImage depth_output(parameter.width, parameter.height); 180 | depth_output.zero(); 181 | 182 | dim3 depth_block; 183 | dim3 depth_grid; 184 | depth_block.x = 16; 185 | depth_block.y = 16; 186 | depth_grid.x = (parameter.width + depth_block.x - 1 ) / depth_block.x; 187 | depth_grid.y = (parameter.height + depth_block.y - 1 ) / depth_block.y; 188 | depth_initial<<>>(depth_output.dev_ptr); 189 | 190 | dim3 render_block; 191 | dim3 render_grid; 192 | render_block.x = 64; 193 | render_grid.x = (cloud_size + render_block.x - 1) / render_block.x; 194 | render<<>>(dev_cloud_ptr, parameter_devptr, depth_output.dev_ptr); 195 | 196 | depth_output.getDevData(host_ptr); 197 | } -------------------------------------------------------------------------------- /src/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_StatusData.py: -------------------------------------------------------------------------------- 1 | """autogenerated by genpy from quadrotor_msgs/StatusData.msg. Do not edit.""" 2 | import sys 3 | python3 = True if sys.hexversion > 0x03000000 else False 4 | import genpy 5 | import struct 6 | 7 | import std_msgs.msg 8 | 9 | class StatusData(genpy.Message): 10 | _md5sum = "c70a4ec4725273263ae176ad30f89553" 11 | _type = "quadrotor_msgs/StatusData" 12 | _has_header = True #flag to mark the presence of a Header object 13 | _full_text = """Header header 14 | uint16 loop_rate 15 | float64 voltage 16 | uint8 seq 17 | 18 | ================================================================================ 19 | MSG: std_msgs/Header 20 | # Standard metadata for higher-level stamped data types. 21 | # This is generally used to communicate timestamped data 22 | # in a particular coordinate frame. 23 | # 24 | # sequence ID: consecutively increasing ID 25 | uint32 seq 26 | #Two-integer timestamp that is expressed as: 27 | # * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') 28 | # * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') 29 | # time-handling sugar is provided by the client library 30 | time stamp 31 | #Frame this data is associated with 32 | # 0: no frame 33 | # 1: global frame 34 | string frame_id 35 | 36 | """ 37 | __slots__ = ['header','loop_rate','voltage','seq'] 38 | _slot_types = ['std_msgs/Header','uint16','float64','uint8'] 39 | 40 | def __init__(self, *args, **kwds): 41 | """ 42 | Constructor. Any message fields that are implicitly/explicitly 43 | set to None will be assigned a default value. The recommend 44 | use is keyword arguments as this is more robust to future message 45 | changes. You cannot mix in-order arguments and keyword arguments. 46 | 47 | The available fields are: 48 | header,loop_rate,voltage,seq 49 | 50 | :param args: complete set of field values, in .msg order 51 | :param kwds: use keyword arguments corresponding to message field names 52 | to set specific fields. 53 | """ 54 | if args or kwds: 55 | super(StatusData, self).__init__(*args, **kwds) 56 | #message fields cannot be None, assign default values for those that are 57 | if self.header is None: 58 | self.header = std_msgs.msg.Header() 59 | if self.loop_rate is None: 60 | self.loop_rate = 0 61 | if self.voltage is None: 62 | self.voltage = 0. 63 | if self.seq is None: 64 | self.seq = 0 65 | else: 66 | self.header = std_msgs.msg.Header() 67 | self.loop_rate = 0 68 | self.voltage = 0. 69 | self.seq = 0 70 | 71 | def _get_types(self): 72 | """ 73 | internal API method 74 | """ 75 | return self._slot_types 76 | 77 | def serialize(self, buff): 78 | """ 79 | serialize message into buffer 80 | :param buff: buffer, ``StringIO`` 81 | """ 82 | try: 83 | _x = self 84 | buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 85 | _x = self.header.frame_id 86 | length = len(_x) 87 | if python3 or type(_x) == unicode: 88 | _x = _x.encode('utf-8') 89 | length = len(_x) 90 | if python3: 91 | buff.write(struct.pack('