diff --git a/src/无人机端代码/.gitignore b/src/无人机端代码/.gitignore deleted file mode 100644 index 56481e16..00000000 --- a/src/无人机端代码/.gitignore +++ /dev/null @@ -1,44 +0,0 @@ -# compilation and distribution -__pycache__ -_ext -*.pyc -nano.save -.catkin_workspace - -build/ -devel/ -dist/ -.vscode/ -Modules/object_detection_landing/ -Modules/object_detection_persistence/ -Modules/object_detection_yolov5openvino/ -Modules/object_detection_yolov5tensorrt/ -Modules/object_detection_circlex/ -Modules/data/ -Modules/object_detection_oneshot/ - - -# pytorch/python/numpy formats -# *.pth -*.pkl -*.npy - -# ipython/jupyter notebooks -*.ipynb -**/.ipynb_checkpoints/ - -# Editor temporaries -*.swn -*.swo -*.swp -*~ - -# Pycharm editor settings -.idea - -# project dirs -/datasets -/models - -ORBvoc.txt - diff --git a/src/无人机端代码/.gitmodules b/src/无人机端代码/.gitmodules deleted file mode 100644 index 36ec78b3..00000000 --- a/src/无人机端代码/.gitmodules +++ /dev/null @@ -1,6 +0,0 @@ -[submodule "Modules/swarm_control"] - path = Modules/swarm_control - url = https://gitee.com/amovlab/swarm_control.git -[submodule "Modules/matlab_bridge"] - path = Modules/matlab_bridge - url = https://gitee.com/amovlab1/matlab_bridge.git diff --git a/src/无人机端代码/Experiment/CMakeLists.txt b/src/无人机端代码/Experiment/CMakeLists.txt deleted file mode 100644 index bd32d374..00000000 --- a/src/无人机端代码/Experiment/CMakeLists.txt +++ /dev/null @@ -1,118 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(prometheus_experiment) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - message_generation - roscpp - geometry_msgs - nav_msgs - sensor_msgs - mavros - std_msgs - std_srvs - tf2_ros - tf2_eigen - mavros_msgs - prometheus_msgs -) - -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED COMPONENTS system) - - - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - geometry_msgs - nav_msgs - sensor_msgs - std_msgs -) - - - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - - -############################### -## 声明可执行cpp文件 ## -############################### - -###### Main File ########## - -##px4_pos_controller.cpp -#add_executable(px4_pos_controller src/px4_pos_controller.cpp) -#add_dependencies(px4_pos_controller prometheus_control_gencpp) -#target_link_libraries(px4_pos_controller ${catkin_LIBRARIES}) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" -) - -## Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch -) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_prometheus_control.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/src/无人机端代码/Experiment/config/px4_config.yaml b/src/无人机端代码/Experiment/config/px4_config.yaml deleted file mode 100644 index d9fd0eee..00000000 --- a/src/无人机端代码/Experiment/config/px4_config.yaml +++ /dev/null @@ -1,260 +0,0 @@ -# Common configuration for PX4 autopilot -# -# node: -startup_px4_usb_quirk: true - -# --- system plugins --- - -# sys_status & sys_time connection options -conn: - heartbeat_rate: 1.0 # send hertbeat rate in Hertz - timeout: 10.0 # hertbeat timeout in seconds - timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0) - system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0) - -# sys_status -sys: - min_voltage: 10.0 # diagnostics min voltage - disable_diag: false # disable all sys_status diagnostics, except heartbeat - -# sys_time -time: - time_ref_source: "fcu" # time_reference source - timesync_mode: MAVLINK - timesync_avg_alpha: 0.6 # timesync averaging factor - -# --- mavros plugins (alphabetical order) --- - -# 3dr_radio -tdr_radio: - low_rssi: 40 # raw rssi lower level for diagnostics - -# actuator_control -# None - -# command -cmd: - use_comp_id_system_control: false # quirk for some old FCUs - -# dummy -# None - -# ftp -# None - -# global_position -global_position: - frame_id: "map" # origin frame - child_frame_id: "base_link" # body-fixed frame - rot_covariance: 99999.0 # covariance for attitude? - gps_uere: 1.0 # User Equivalent Range Error (UERE) of GPS sensor (m) - use_relative_alt: true # use relative altitude for local coordinates - tf: - send: false # send TF? - frame_id: "map" # TF frame_id - global_frame_id: "earth" # TF earth frame_id - child_frame_id: "base_link" # TF child_frame_id - -# imu_pub -imu: - frame_id: "base_link" - # need find actual values - linear_acceleration_stdev: 0.0003 - angular_velocity_stdev: 0.0003490659 // 0.02 degrees - orientation_stdev: 1.0 - magnetic_stdev: 0.0 - -# local_position -local_position: - frame_id: "world" - tf: - send: true - frame_id: "world" - child_frame_id: "base_link" - send_fcu: false - -# param -# None, used for FCU params - -# rc_io -# None - -# safety_area -safety_area: - p1: {x: 1.0, y: 1.0, z: 1.0} - p2: {x: -1.0, y: -1.0, z: -1.0} - -# setpoint_accel -setpoint_accel: - send_force: false - -# setpoint_attitude -setpoint_attitude: - reverse_thrust: false # allow reversed thrust - use_quaternion: false # enable PoseStamped topic subscriber - tf: - listen: false # enable tf listener (disable topic subscribers) - frame_id: "map" - child_frame_id: "target_attitude" - rate_limit: 50.0 - -setpoint_raw: - thrust_scaling: 1.0 # used in setpoint_raw attitude callback. - # Note: PX4 expects normalized thrust values between 0 and 1, which means that - # the scaling needs to be unitary and the inputs should be 0..1 as well. - -# setpoint_position -setpoint_position: - tf: - listen: false # enable tf listener (disable topic subscribers) - frame_id: "map" - child_frame_id: "target_position" - rate_limit: 50.0 - mav_frame: LOCAL_NED - -# setpoint_velocity -setpoint_velocity: - mav_frame: LOCAL_NED - -# vfr_hud -# None - -# waypoint -mission: - pull_after_gcs: true # update mission if gcs updates - -# --- mavros extras plugins (same order) --- - -# adsb -# None - -# debug_value -# None - -# distance_sensor -## Currently available orientations: -# Check http://wiki.ros.org/mavros/Enumerations -## -distance_sensor: - hrlv_ez4_pub: - id: 0 - frame_id: "hrlv_ez4_sonar" - orientation: PITCH_270 # RPY:{0.0, 270.0, 0.0} - downward-facing - field_of_view: 0.0 # XXX TODO - send_tf: true - sensor_position: {x: 0.0, y: 0.0, z: -0.1} - lidarlite_pub: - id: 1 - frame_id: "lidarlite_laser" - orientation: PITCH_270 - field_of_view: 0.0 # XXX TODO - send_tf: true - sensor_position: {x: 0.0, y: 0.0, z: -0.1} - sonar_1_sub: - subscriber: true - id: 2 - orientation: PITCH_270 - laser_1_sub: - subscriber: true - id: 3 - orientation: PITCH_270 - -# image_pub -image: - frame_id: "px4flow" - -# fake_gps -fake_gps: - # select data source - use_mocap: true # ~mocap/pose - mocap_transform: true # ~mocap/tf instead of pose - use_vision: false # ~vision (pose) - # origin (default: Zürich) - geo_origin: - lat: 47.3667 # latitude [degrees] - lon: 8.5500 # longitude [degrees] - alt: 408.0 # altitude (height over the WGS-84 ellipsoid) [meters] - eph: 2.0 - epv: 2.0 - satellites_visible: 5 # virtual number of visible satellites - fix_type: 3 # type of GPS fix (default: 3D) - tf: - listen: false - send: false # send TF? - frame_id: "map" # TF frame_id - child_frame_id: "fix" # TF child_frame_id - rate_limit: 10.0 # TF rate - gps_rate: 5.0 # GPS data publishing rate - -# landing_target -landing_target: - listen_lt: false - mav_frame: "LOCAL_NED" - land_target_type: "VISION_FIDUCIAL" - image: - width: 640 # [pixels] - height: 480 - camera: - fov_x: 2.0071286398 # default: 115 [degrees] - fov_y: 2.0071286398 - tf: - send: true - listen: false - frame_id: "landing_target" - child_frame_id: "camera_center" - rate_limit: 10.0 - target_size: {x: 0.3, y: 0.3} - -# mocap_pose_estimate -mocap: - # select mocap source - use_tf: false # ~mocap/tf - use_pose: true # ~mocap/pose - -# odom -odometry: - fcu: - odom_parent_id_des: "map" # desired parent frame rotation of the FCU's odometry - odom_child_id_des: "base_link" # desired child frame rotation of the FCU's odometry - -# px4flow -px4flow: - frame_id: "px4flow" - ranger_fov: 0.118682 # 6.8 degrees at 5 meters, 31 degrees at 1 meter - ranger_min_range: 0.3 # meters - ranger_max_range: 5.0 # meters - -# vision_pose_estimate -vision_pose: - tf: - listen: false # enable tf listener (disable topic subscribers) - frame_id: "map" - child_frame_id: "vision_estimate" - rate_limit: 10.0 - -# vision_speed_estimate -vision_speed: - listen_twist: true # enable listen to twist topic, else listen to vec3d topic - twist_cov: true # enable listen to twist with covariance topic - -# vibration -vibration: - frame_id: "base_link" - -# wheel_odometry -wheel_odometry: - count: 2 # number of wheels to compute odometry - use_rpm: false # use wheel's RPM instead of cumulative distance to compute odometry - wheel0: {x: 0.0, y: -0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) - wheel1: {x: 0.0, y: 0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) - send_raw: true # send wheel's RPM and cumulative distance (~/wheel_odometry/rpm, ~/wheel_odometry/distance) - send_twist: false # send geometry_msgs/TwistWithCovarianceStamped instead of nav_msgs/Odometry - frame_id: "map" # origin frame - child_frame_id: "base_link" # body-fixed frame - vel_error: 0.1 # wheel velocity measurement error 1-std (m/s) - tf: - send: false - frame_id: "map" - child_frame_id: "base_link" - -# vim:set ts=2 sw=2 et: diff --git a/src/无人机端代码/Experiment/config/px4_pluginlists.yaml b/src/无人机端代码/Experiment/config/px4_pluginlists.yaml deleted file mode 100644 index 145d2629..00000000 --- a/src/无人机端代码/Experiment/config/px4_pluginlists.yaml +++ /dev/null @@ -1,33 +0,0 @@ -plugin_blacklist: -- actuator_control -- adsb -- safety_area -- 3dr_radio -- cam_imu_sync -- altitude -- distance_sensor -- fake_gps -- gps_rtk -- hil -- home_position -- landing_target -- mocap_pose_estimate -- mount_control -- obstacle_distance -- rc_io -- vfr_hud -- waypoint -- wind_estimation -- px4flow -- global_position -- companion_process_status -- debug_value -- wheel_odometry -- vibration -- odom -- setpoint_attitude -- setpoint_position -- setpoint_accel -- setpoint_velocity -plugin_whitelist: [] -#- 'sys_*' \ No newline at end of file diff --git a/src/无人机端代码/Experiment/config/uav_control_indoor.yaml b/src/无人机端代码/Experiment/config/uav_control_indoor.yaml deleted file mode 100644 index 4e751727..00000000 --- a/src/无人机端代码/Experiment/config/uav_control_indoor.yaml +++ /dev/null @@ -1,29 +0,0 @@ -## parameter for uav_control - -control: - ## 控制器编号：0 for PX4_ORIGIN, 1 for PID, 2 for UDE, 3 for NE - pos_controller : 0 - # 是否支持外部控制 - enable_external_control : false - ## 起飞高度 - Takeoff_height : 1.5 - ## 降落速度 - Land_speed : 0.2 - ##　上锁高度 - Disarm_height : 0.1 - ## 定位源: 0 for MOCAP, 1 for T265, 2 for GAZEBO, 3 for FAKE_ODOM, 4 for GPS, 5 for RTK, 6 for UWB - location_source : 2 - ## 最大安全速度 - maximum_safe_vel_xy : 2.0 - maximum_safe_vel_z : 1.5 - ## 最大vision/px4速度误差 - maximum_vel_error_for_vision : 1.0 - -##　地理围栏 -geo_fence: - x_min: -100.0 - x_max: 100.0 - y_min: -100.0 - y_max: 100.0 - z_min: -0.5 - z_max: 100.0 diff --git a/src/无人机端代码/Experiment/launch/prometheus_experiment.launch b/src/无人机端代码/Experiment/launch/prometheus_experiment.launch deleted file mode 100644 index 831b026a..00000000 --- a/src/无人机端代码/Experiment/launch/prometheus_experiment.launch +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Experiment/package.xml b/src/无人机端代码/Experiment/package.xml deleted file mode 100644 index de76d1d5..00000000 --- a/src/无人机端代码/Experiment/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - prometheus_experiment - 0.0.0 - The prometheus_experiment package - - - Yuhua Qi - Yuhua Qi - TODO - - https://www.amovlab.com - https://github.com/potato77/prometheus_experiment.git - - message_generation - message_runtime - - catkin - std_msgs - std_msgs - std_msgs - - - - - - - - diff --git a/src/无人机端代码/LICENSE b/src/无人机端代码/LICENSE deleted file mode 100644 index 7a61a724..00000000 --- a/src/无人机端代码/LICENSE +++ /dev/null @@ -1,60 +0,0 @@ -Apache License -Version 2.0, January 2004 -http://www.apache.org/licenses/ - -TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - -1. Definitions. - -"License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. - -"Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. - -"Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. - -"You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. - -"Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. - -"Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. - -"Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). - -"Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. - -"Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." - -"Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. - -2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. - -3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. - -4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: - - a.You must give any other recipients of the Work or Derivative Works a copy of this License; and - b.You must cause any modified files to carry prominent notices stating that You changed the files; and - c.You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and - d.If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. - - You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. - -5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. - -6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. - -7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. - -8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. - -9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. - -END OF TERMS AND CONDITIONS - -Copyright 2022 AMOVLAB - -Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at - -http://www.apache.org/licenses/LICENSE-2.0 - -Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. \ No newline at end of file diff --git a/src/无人机端代码/Modules/future_aircraft/CMakeLists.txt b/src/无人机端代码/Modules/future_aircraft/CMakeLists.txt deleted file mode 100644 index 4898dbc5..00000000 --- a/src/无人机端代码/Modules/future_aircraft/CMakeLists.txt +++ /dev/null @@ -1,145 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(prometheus_future_aircraft) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) -add_compile_options(-std=c++17) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(OpenCV REQUIRED) -find_package(catkin REQUIRED COMPONENTS - message_generation - roscpp - rospy - geometry_msgs - sensor_msgs - mavros - nav_msgs - std_msgs - std_srvs - tf2_ros - tf2_eigen - mavros_msgs - prometheus_msgs - cv_bridge - image_transport -) - -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED COMPONENTS system) - - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - geometry_msgs - sensor_msgs - std_msgs -) - -catkin_package( - CATKIN_DEPENDS message_runtime -) - - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include ../tutorial_demo/advanced/autonomous_landing/include - ${catkin_INCLUDE_DIRS} - ../common/include -) - - -############################### -## 声明可执行cpp文件 ## -############################### -# add_executable(autonomous_landing autonomous_landing/autonomous_landing.cpp) -# target_link_libraries(autonomous_landing ${catkin_LIBRARIES}) - -#add_executable(waypoint_tracking waypoint_tracking/waypoint_tracking.cpp) -#target_link_libraries(waypoint_tracking ${catkin_LIBRARIES}) - -#add_executable(object_tracking object_tracking/object_tracking.cpp) -#target_link_libraries(object_tracking ${catkin_LIBRARIES}) - -#add_executable(takeoff takeoff/takeoff.cpp) -#target_link_libraries(takeoff ${catkin_LIBRARIES}) - -#add_executable(formation_control formation_control/formation_control.cpp) -#target_link_libraries(formation_control ${catkin_LIBRARIES}) -add_executable(future_aircraft src/future_aircraft.cpp) -target_link_libraries(future_aircraft ${catkin_LIBRARIES}) - -add_executable(ellipse_det - src/ellipse_det.cpp - src/ellipse_detector/ellipse_detector.cpp -) -target_include_directories(ellipse_det PUBLIC - src/ellipse_detector/ -) -target_link_libraries(ellipse_det ${catkin_LIBRARIES} ${OpenCV_LIBS} yaml-cpp) - - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -install(PROGRAMS - scripts/locus.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" -) - -## Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch -) - -############# -## Testing ## -############# - - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_prometheus_demo.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/src/无人机端代码/Modules/future_aircraft/conf/calib.yaml b/src/无人机端代码/Modules/future_aircraft/conf/calib.yaml deleted file mode 100644 index eb2442d5..00000000 --- a/src/无人机端代码/Modules/future_aircraft/conf/calib.yaml +++ /dev/null @@ -1,20 +0,0 @@ -%YAML:1.0 ---- -calibration_time: "Thu 14 Jul 2022 11:46:13 AM CST" -image_width: 640 -image_height: 480 -flags: 0 -camera_matrix: !!opencv-matrix - rows: 3 - cols: 3 - dt: d - data: [ 6.7570491275428003e+02, 0., 3.4702214961185257e+02, 0., - 6.7652907115648509e+02, 2.5917548194106814e+02, 0., 0., 1. ] -distortion_coefficients: !!opencv-matrix - rows: 1 - cols: 5 - dt: d - data: [ -4.3154114726977300e-01, 2.7199122166782413e-01, - -1.5471282947090615e-03, -6.6196646287719843e-04, - -4.9889322933567892e-01 ] -avg_reprojection_error: 4.7592643246496424e-01 diff --git a/src/无人机端代码/Modules/future_aircraft/conf/calib_sitl.yaml b/src/无人机端代码/Modules/future_aircraft/conf/calib_sitl.yaml deleted file mode 100644 index 58c2ff5a..00000000 --- a/src/无人机端代码/Modules/future_aircraft/conf/calib_sitl.yaml +++ /dev/null @@ -1,17 +0,0 @@ -%YAML:1.0 ---- -calibration_time: "Thu 14 Jul 2022 11:46:13 AM CST" -image_width: 640 -image_height: 480 -flags: 0 -camera_matrix: !!opencv-matrix - rows: 3 - cols: 3 - dt: d - data: [369.502083, 0.0, 640.0, 0.0, 369.502083, 360.0, 0.0, 0.0, 1.0] -distortion_coefficients: !!opencv-matrix - rows: 1 - cols: 5 - dt: d - data: [0,0,0,0,0] -avg_reprojection_error: 4.7592643246496424e-01 diff --git a/src/无人机端代码/Modules/future_aircraft/launch/future_aircraft.launch b/src/无人机端代码/Modules/future_aircraft/launch/future_aircraft.launch deleted file mode 100644 index ede28625..00000000 --- a/src/无人机端代码/Modules/future_aircraft/launch/future_aircraft.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/无人机端代码/Modules/future_aircraft/launch/test.launch b/src/无人机端代码/Modules/future_aircraft/launch/test.launch deleted file mode 100644 index ef4d0877..00000000 --- a/src/无人机端代码/Modules/future_aircraft/launch/test.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - \ No newline at end of file diff --git a/src/无人机端代码/Modules/future_aircraft/package.xml b/src/无人机端代码/Modules/future_aircraft/package.xml deleted file mode 100644 index e7fa468f..00000000 --- a/src/无人机端代码/Modules/future_aircraft/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - prometheus_future_aircraft - 0.0.0 - The prometheus_demo package - - Yuhua Qi - - TODO - - message_generation - message_runtime - catkin - std_msgs - std_msgs - std_msgs - - - - - diff --git a/src/无人机端代码/Modules/future_aircraft/readme.md b/src/无人机端代码/Modules/future_aircraft/readme.md deleted file mode 100644 index 34e96243..00000000 --- a/src/无人机端代码/Modules/future_aircraft/readme.md +++ /dev/null @@ -1,286 +0,0 @@ -# future_aircraft 功能包使用介绍 - -## 一、介绍及使用 - -- 本功能包为第二届大学生未来飞行器挑战赛的实践类仿真demo - - 设置比赛区域（10 * 20 * 6 m，暂定）场地内随机模拟圆形标靶 3 个；其中， 2 个随 - 机固定在场地位置， 1 个固定于移动的无人车上面，无人车以≤ 1m/s 的速度在 - 8 字形轮廓进行移动。 - - 无人机由指定的位置一键起飞后，立即转换为自动模式，开始通过机载传感器 - 自主搜索这些目标标靶，并向目标标靶投掷模拟子弹；完成所有的投掷任务后， - 自主回到起飞点降落。以投中目标的数量和完成时间来综合计分。 - - 模拟子弹选用与标靶粘接的子弹，以减少因弹性或是风力影响。 - - 主办方在赛场内提供 UWB 基站信号覆盖，参赛队伍可以自主选择目标识别和 - 定位方式。 - -![image.png](https://qiniu.md.amovlab.com/img/m/202207/20220722/1153184932903574280503296.png) - -- 编译必要的文件（在Prometheus跟目录下运行） - - - ``` - ./Modules/future_aircraft/compile_aircraft_sitle.sh - ``` - -- 仿真运行：（需要连接遥控器） - - - 启动仿真环境： - - - ``` - roslaunch prometheus_future_aircraft future_aircraft.launch - ``` - - - 启动控制demo：（后续融合到一个launch文件中） - - - ``` - rosrun prometheus_future_aircraft future_aircraft - ``` - - - - - -## 二、任务控制demo说明 - -- 控制需要有一定基础，可以熟悉了解 [控制demo模块](https://wiki.amovlab.com/public/prometheus-wiki/%E6%97%A0%E4%BA%BA%E6%9C%BA%E6%8E%A7%E5%88%B6%E6%A8%A1%E5%9D%97-uav_control/%E6%97%A0%E4%BA%BA%E6%9C%BA%E6%8E%A7%E5%88%B6%E6%A8%A1%E5%9D%97-uav_control.html) -- 针对比赛以及实际情况，选择合适的控制接口进行控制。本demo中会使用惯性系与机体系下的位置和速度控制 - -### 任务状态机 - -- 状态机： - - - TAKEOFF 起飞状态机 - - - 直接调用起飞函数 - - - ``` - uav_command.header.stamp = ros::Time::now(); - uav_command.header.frame_id = "ENU"; - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Init_Pos_Hover; - ``` - - SEARCH 搜寻状态机 - - - 使用惯性系或者机体系下的位置控制 - - - ``` - uav_command.header.stamp = ros::Time::now(); - uav_command.header.frame_id = "BODY"; - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS_BODY; - uav_command.position_ref[0] = waypoint1[0]; - uav_command.position_ref[1] = waypoint1[1]; - uav_command.position_ref[2] = waypoint1[2]; - uav_command.yaw_ref = 0.0; - ``` - - - ``` - uav_command.header.stamp = ros::Time::now(); - uav_command.header.frame_id = "ENU"; - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS; - uav_command.position_ref[0] = waypoint4[0]; - uav_command.position_ref[1] = waypoint4[1]; - uav_command.position_ref[2] = waypoint4[2]; - uav_command.yaw_ref = 0.0; - ``` - - - TRACKING 跟踪状态机 - - - 使用机体系下的水平方向速度控制，垂直方向高度控制 - - - ``` - //坐标系 - uav_command.header.frame_id = "BODY"; - // Move模式 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - // 机体系下的速度控制 - uav_command.Move_mode = prometheus_msgs::UAVCommand::XY_VEL_Z_POS_BODY; - uav_command.velocity_ref[0] = 0.5 * now_arucos_info.position[0]; - uav_command.velocity_ref[1] = 0.5 * now_arucos_info.position[1]; - uav_command.position_ref[2] = 1.0; - uav_command.yaw_ref = 0.0; - ``` - - - LOST 目标丢失状态机 - - - RETURN 返航状态机（返航以及降落状态机） - -### 视觉端处理 - -- 坐标系变换说明（Prometheus\Modules\tutorial_demo\advanced\autonomous_landing\include\mission_utils.h） - - - 接受图像话题 - - ``` - /prometheus/ellipse_det - ``` - - - 识别算法发布的目标位置位于**相机坐标系**（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正） - - - 首先，从相机坐标系转换至**机体坐标系**（从机体往前看，物体在相机前方x为正，左方y为正，上方z为正）：`camera_offset`为相机安装偏移量，此处为下置摄像头，参看 `p450_future_aircraft.sdf`可知，相机安装于机体质心下方0.05米，因此，`camera_offset[0] = 0.0`，`camera_offset[1] = 0.0`，`camera_offset[2] = -0.05` 。 - - - ``` - ellipse_det.pos_body_frame[0] = -ellipse_det.Detection_info.position[1] + camera_offset[0]; - ellipse_det.pos_body_frame[1] = -ellipse_det.Detection_info.position[0] + camera_offset[1]; - ellipse_det.pos_body_frame[2] = -ellipse_det.Detection_info.position[2] + camera_offset[2]; - ``` - - - 从机体坐标系转换至**与机体固连的ENU系**（原点位于质心，x轴指向yaw=0的方向，y轴指向yaw=90的方向，z轴指向上的坐标系）：直接乘上机体系到惯性系的旋转矩阵即可 R_Body_to_ENU = get_rotation_matrix(_DroneState.attitude[0], _DroneState.attitude[1], _DroneState.attitude[2]); - - - ``` - ellipse_det.pos_body_enu_frame = R_Body_to_ENU * ellipse_det.pos_body_frame; - ``` - - - 机体惯性系 再变化为 惯性系 - - - ``` - ellipse_det.pos_enu_frame[0] = uav_state.position[0] + ellipse_det.pos_body_enu_frame[0]; - ellipse_det.pos_enu_frame[1] = uav_state.position[1] + ellipse_det.pos_body_enu_frame[1]; - ellipse_det.pos_enu_frame[2] = uav_state.position[2] + ellipse_det.pos_body_enu_frame[2]; - ``` - - - -## 三、无人机视觉 - -- 需要有一定基础 -- 完成比赛中不一定会用到所有讲解的知识 -- 才疏学浅，如果有大佬发现错误，欢迎留言指正 - -### Prometheus视觉模块简介 - -1. [概览](https://wiki.amovlab.com/public/prometheus-wiki/%E7%9B%AE%E6%A0%87%E6%A3%80%E6%B5%8B%E6%A8%A1%E5%9D%97-object_detection/%E7%9B%AE%E6%A0%87%E6%A3%80%E6%B5%8B%E6%A8%A1%E5%9D%97%E4%BB%8B%E7%BB%8D/%E7%9B%AE%E6%A0%87%E6%A3%80%E6%B5%8B%E6%A8%A1%E5%9D%97%E4%BB%8B%E7%BB%8D.html) - -2. 椭圆识别演示 - -电脑插上摄像头，输入一下命令运行，默认读取ID为0的摄像 -```bash -roslaunch prometheus_detection ellipse_det.launch -``` -![Peek 2022-07-22 10-33.gif](https://qiniu.md.amovlab.com/img/m/202207/20220722/1156295734737485863223296.gif) - - - -### 椭圆检测流程简介 - -OPENCV版本: 霍夫椭圆检测,更慢 - - -1. 图像去噪声, 去除图像中到椒盐噪声 -2. 弧检测,挑选出可能为弧的对象 -3. 弧分类,判定弧属于四个象限中的那个一个 - - - -4. 弧过滤,运用两段弧约束, CNC约束(三段弧约束),过滤不满足要求的弧 - -5. 椭圆估计,在剩下的四个象限的弧中进行排列组合,使用优化算法,通过4个弧线估计一个椭圆 - -6. 椭圆打分,使用特定打分算法,计算椭圆与4个弧线的拟合程度,给椭圆打分,最后选出得分较高的椭圆 - -### 相机模型简介 - -世界平面到图像平面(根据小孔成像原理) - - - -图像平面到像素平面, 将图像平面原点映射到像素平面原点 -- $x,y$图像尺寸, $u,v$像素尺寸 -- $u_0,v_0$是图像坐标系原点到像素坐标系原点的偏移 -- $dx, dy$ 每个像素在图像平面$x$和$y$方向上的物理尺寸 - - - -世界坐标系到像素坐标系变换(_下图中,图像坐标系到像素坐标系转换矩阵添加畸变矫正参数矫正_) - - - -这里我们假设已知目标在相机坐标系下的位置$(X_c, Y_c, Z_c)$，将前两个变换矩阵相乘得到`camera_matrix`(也叫相机内参)，最后得到相机坐标系到像素坐标系的关系: - -$$ -\begin{bmatrix} - u\\ - v\\ - 1 -\end{bmatrix}= -\begin{bmatrix} -\frac{f}{dx} & 0 & u_{0} \\ - 0 & \frac{f}{dy} & v_{0}\\ - 0 & 0 & 1 -\end{bmatrix}\begin{bmatrix} - \frac{X_{c}}{Z_{c}} \\ - \frac{Y_{c}}{Z_{c}} \\ - 1 -\end{bmatrix} -$$ - -实际中，一般像素坐标$u,v$以及$Z_c$已知，通过目标的像素位置、相机内参和深度信息，就可以反解目标世界坐标，通过上述公式，变换可得: -$$ -\begin{array}{c} -X_c = Z_c * (u - u_0) /(\frac{f}{dx}) \\ -Y_c = Z_c * (v - v_0) /(\frac{f}{dy}) \\ -\end{array} -$$ - -其中$Z_c$一般可以通过传感器直接获得，或者通过事先已知目标真实尺寸，像素尺寸通过相似三角形比值关系获得，具体可以看[配置目标的实际长宽](https://wiki.amovlab.com/public/prometheus-wiki/%E7%9B%AE%E6%A0%87%E6%A3%80%E6%B5%8B%E6%A8%A1%E5%9D%97-object_detection/%E6%89%A9%E5%B1%95%E9%98%85%E8%AF%BB/%E9%85%8D%E7%BD%AE%E7%9B%AE%E6%A0%87%E7%9A%84%E5%AE%9E%E9%99%85%E9%95%BF%E5%AE%BD.html)进行学习。 - -### 相机标定 -> [相机标定程序](https://wiki.amovlab.com/public/prometheus-wiki/%E7%9B%AE%E6%A0%87%E6%A3%80%E6%B5%8B%E6%A8%A1%E5%9D%97-object_detection/%E6%89%A9%E5%B1%95%E9%98%85%E8%AF%BB/%E7%9B%B8%E6%9C%BA%E6%A0%87%E5%AE%9A.html) -1. 获取相机内参 -2. 去除相机畸变 - - - - - -某个相机的标定文件 -```yaml -%YAML:1.0 ---- -calibration_time: "Thu 14 Jul 2022 11:46:13 AM CST" -image_width: 640 -image_height: 480 -flags: 0 -camera_matrix: !!opencv-matrix - rows: 3 - cols: 3 - dt: d - data: [ 6.7570491275428003e+02, 0., 3.4702214961185257e+02, 0., - 6.7652907115648509e+02, 2.5917548194106814e+02, 0., 0., 1. ] -distortion_coefficients: !!opencv-matrix - rows: 1 - cols: 5 - dt: d - data: [ -4.3154114726977300e-01, 2.7199122166782413e-01, - -1.5471282947090615e-03, -6.6196646287719843e-04, - -4.9889322933567892e-01 ] -avg_reprojection_error: 4.7592643246496424e-01 -``` - -### 代码解析 - -[ellipse_det.cpp](../object_detection/cpp_nodes/ellipse_det.cpp) - -- 整个代码逻辑 -- 可调节参数 -- 去除图像畸变 -- 目标位置估计 -- 区分起飞点和靶标 - -### messge定义 - -[DetectionInfo](../common/prometheus_msgs/msg/DetectionInfo.msg) - -[MultiDetectionInfo](../common/prometheus_msgs/msg/MultiDetectionInfo.msg) - -### 椭圆检测launch文件 -``` - - - - - -``` - -### 问题 - -- 飞行过程中无人机姿态变化，会导致估计的目标位置误差加大，该怎么解决？ diff --git a/src/无人机端代码/Modules/future_aircraft/scripts/locus.py b/src/无人机端代码/Modules/future_aircraft/scripts/locus.py deleted file mode 100644 index cd9f918e..00000000 --- a/src/无人机端代码/Modules/future_aircraft/scripts/locus.py +++ /dev/null @@ -1,64 +0,0 @@ -#!/usr/bin/env python3 -import rospy -import math -import time as t -from geometry_msgs.msg import Twist -from gazebo_msgs.msg import ModelState, ModelStates -from gazebo_msgs.srv import SetModelState, SetModelStateRequest, SetModelStateResponse - - -client = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState) -client.wait_for_service() - -def init_pose(): - req = SetModelStateRequest() - req.model_state.model_name = "future_aircraft_car" - req.model_state.pose.position.x = 0 - req.model_state.pose.position.y = 0 - req.model_state.pose.position.z = 0 - client.call(req) - -landing_pad = None -def get_pos(info: ModelStates): - global landing_pad - for i , name in enumerate(info.name): - if name == "future_aircraft_car": - landing_pad = info.pose[i].position - # print(landing_pad) - return - - -def pose_publisher_circle(): - pub = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=10) - pose_msg = ModelState() - pose_msg.model_name = 'landing_pad' - pose_msg.pose.position.x = 0 - pose_msg.pose.position.y = 0 - pose_msg.pose.position.z = 0 - - rate = rospy.Rate(30) - pub = rospy.Publisher('/wheeltec/cmd_vel', Twist, queue_size=10) - pose_msg = Twist() - rate = rospy.Rate(60) - circle_radius = 1.5 - linear_vel = 1 - omega = math.fabs(linear_vel / circle_radius) - # cycle = math.pi * 2 / omega - past_time = t.time() - while not rospy.is_shutdown(): - pose_msg.linear.x = linear_vel - pose_msg.angular.z = omega - if landing_pad != None and math.sqrt(landing_pad.x ** 2 + landing_pad.y**2) < 0.3 and t.time() - past_time > 1: - pub.publish(pose_msg) - init_pose() - past_time = t.time() - omega = -omega - pub.publish(pose_msg) - rate.sleep() - - -if __name__ == '__main__': - rospy.init_node('locus_8') - rospy.Subscriber("/gazebo/model_states", ModelStates, get_pos) - init_pose() - pose_publisher_circle() diff --git a/src/无人机端代码/Modules/future_aircraft/src/ellipse_det.cpp b/src/无人机端代码/Modules/future_aircraft/src/ellipse_det.cpp deleted file mode 100644 index e9e1b788..00000000 --- a/src/无人机端代码/Modules/future_aircraft/src/ellipse_det.cpp +++ /dev/null @@ -1,168 +0,0 @@ -#include "ellipse_detector.h" -#include -#include "printf_utils.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -std::shared_mutex g_mutex; -int frame_width = 640, frame_height = 480; - -cv::Mat g_camera_matrix, g_dist_coeffs; -cv::Mat g_frame; -void imageCb(const sensor_msgs::ImageConstPtr &msg) -{ - cv_bridge::CvImagePtr cam_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); - - if (cam_image) - { - std::unique_lock lock(g_mutex); - cv::Mat tmp = cam_image->image.clone(); - // NOTE: 图像去畸变 - // g_frame = tmp - cv::undistort(tmp, g_frame, g_camera_matrix, g_dist_coeffs); - frame_width = cam_image->image.size().width; - frame_height = cam_image->image.size().height; - } -} - -float g_camera_height = 0; -void heightCb(const std_msgs::Float32ConstPtr &msg) -{ - g_camera_height = msg->data; -} - -bool unique_ell(const std::pair &lhs, const std::pair &rhs) -{ - float x = lhs.first - rhs.first; - float y = lhs.second - rhs.second; - if (std::sqrt(std::pow(x, 2) + std::pow(y, 2)) < 20) - { - return false; - }; - return true; -} -// bool (*unique_ell_func)(const std::pair, const std::) = unique_ell; - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "ellipse_det"); - ros::NodeHandle nh("~"); - image_transport::ImageTransport it(nh); - std::string input_image_topic, output_image_topic, camera_params, det_info_topic, camera_height_topic; - nh.param("input_image_topic", input_image_topic, "/prometheus/camera/rgb/image_raw"); - nh.param("output_image_topic", output_image_topic, "/prometheus/detection/image_raw"); - nh.param("camera_height_topic", camera_height_topic, "/camera/height"); - nh.param("camera_params", camera_params, ""); - nh.param("det_info_topic", det_info_topic, "/prometheus/ellipse_det"); - - if (camera_params.empty() || access(camera_params.c_str(), 0x00) != 0) - { - throw camera_params + " path does not exist or not provided"; - return -1; - } - cv::FileStorage fs(camera_params, cv::FileStorage::READ); - fs["camera_matrix"] >> g_camera_matrix; - fs["distortion_coefficients"] >> g_dist_coeffs; - - // 订阅图像话题 - image_transport::Subscriber image_subscriber = it.subscribe(input_image_topic, 1, imageCb); - // 发布检测可视化图像话题 - image_transport::Publisher image_pub = it.advertise(output_image_topic, 1); - - // 发布MultiDetectionInfo话题 - ros::Publisher det_info_pub = nh.advertise(det_info_topic, 1); - - // 圆检测参数调整 - EllipseDetector ellipse_detector; - // float fMaxCenterDistance = sqrt(float(width * width + height * height)) * 0.05f; - // ellipse_detector.SetParameters(cv::Size(5, 5), 1.306, 1.f, fMaxCenterDistance, 9, 2.984, 0.111, 0.511, 0.470, 22, 0.946, 0.121, 0.468, 0.560, 0.202); - - ros::Rate rate(60); - while (ros::ok()) - { - prometheus_msgs::MultiDetectionInfo det_info; - det_info.detect_or_track = 0; - ros::spinOnce(); - if (g_frame.empty()) - { - PCOUT(-1, YELLOW, "wait for get image data"); - continue; - } - cv::Mat frame; - { - std::unique_lock lock(g_mutex); - frame = g_frame.clone(); - } - std::vector ellsCned; - double t0 = (double)cv::getTickCount(); - ellipse_detector.Detect(frame, ellsCned); - - sensor_msgs::ImagePtr det_output_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg(); - - // cv::cvtColor(frame, frame, cv::COLOR_BGR2HSV); - std::set, decltype(unique_ell) *> ellipse_set(unique_ell); - for (int i = 0; i < ellsCned.size(); i++) - { - const Ellipse &ellipse = ellsCned[i]; - auto insert_re = ellipse_set.insert(std::make_pair(ellipse.xc_, ellipse.yc_)); - if (insert_re.first == ellipse_set.end()) - continue; - prometheus_msgs::DetectionInfo info; - // NOTE: 如果区分无人机起点和靶标? - cv::Mat mask = cv::Mat::zeros(frame.size(), CV_8U); - cv::ellipse(mask, cv::Point(cvRound(ellipse.xc_), cvRound(ellipse.yc_)), cv::Size(cvRound(ellipse.a_), cvRound(ellipse.b_)), ellipse.rad_ * 180 / CV_PI, 0, 360, cv::Scalar(255, 255, 255), -1); - cv::Mat tmp; - frame.copyTo(tmp, mask); - // cv::bitwise_and(frame, cv::noArray(), tmp, mask); - cv::inRange(tmp, cv::Scalar(150, 150, 150), cv::Scalar(255, 255, 255), tmp); - - int *tmpi = nullptr; - if (static_cast(cv::countNonZero(tmp)) / cv::countNonZero(mask) > 0.1) - { - info.object_name = "S"; - cv::Size sz = cv::getTextSize(info.object_name, 0, 0.8, 1, tmpi); - cv::putText(frame, info.object_name, cv::Point(ellipse.xc_ - sz.width / 2, ellipse.yc_ + sz.height / 2), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 0), 1); - } - else - { - info.object_name = "T"; - cv::Size sz = cv::getTextSize(info.object_name, 0, 0.8, 1, tmpi); - cv::putText(frame, info.object_name, cv::Point(ellipse.xc_ - sz.width / 2, ellipse.yc_ + sz.height / 2), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 0), 1); - } - info.frame = 0; - info.detected = true; - // NOTE: 目标位置估计 - info.position[0] = g_camera_height * (ellipse.yc_ - frame_height / 2) / g_camera_matrix.at(1, 1); - info.position[1] = g_camera_height * (ellipse.xc_ - frame_width / 2) / g_camera_matrix.at(0, 0); - info.position[2] = g_camera_height; - info.sight_angle[0] = (ellipse.xc_ - frame_width / 2) / (frame_width / 2) * std::atan(frame_width / (2 * g_camera_matrix.at(0, 0))); - info.sight_angle[1] = (ellipse.yc_ - frame_height / 2) / (frame_height / 2) * std::atan(frame_height / (2 * g_camera_matrix.at(1, 1))); - info.pixel_position[0] = ellipse.xc_; - info.pixel_position[1] = ellipse.yc_; - det_info.detection_infos.push_back(info); - } - det_info.num_objs = ellipse_set.size(); - - double dt = ((double)cv::getTickCount() - t0) / cv::getTickFrequency(); - char msg[256]; - sprintf(msg, "FPS: %f", 1.f / dt); - cv::putText(frame, msg, cv::Point(10, 20), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 0, 255), 1, 4, 0); - - ellipse_detector.DrawDetectedEllipses(frame, ellsCned, 1, 6); - image_pub.publish(det_output_msg); - cv::imshow("show", frame); - cv::waitKey(1); - - det_info_pub.publish(det_info); - rate.sleep(); - } - return 0; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/future_aircraft/src/ellipse_detector/ellipse_detector.cpp b/src/无人机端代码/Modules/future_aircraft/src/ellipse_detector/ellipse_detector.cpp deleted file mode 100644 index f256e57b..00000000 --- a/src/无人机端代码/Modules/future_aircraft/src/ellipse_detector/ellipse_detector.cpp +++ /dev/null @@ -1,3855 +0,0 @@ -#include "ellipse_detector.h" -#include -#include - - -using namespace std; -using namespace cv; - - - -#ifndef USE_OMP -int omp_get_max_threads() { return 1; } -int omp_get_thread_num() { return 0; } -// int omp_set_num_threads(int){ return 0; } -#endif - - -void _list_dir(std::string dir, std::vector& files, std::string suffixs, bool r) { - // assert(_endswith(dir, "/") || _endswith(dir, "\\")); - - DIR *pdir; - struct dirent *ent; - string childpath; - string absolutepath; - pdir = opendir(dir.c_str()); - assert(pdir != NULL); - - vector suffixd(0); - if (!suffixs.empty() && suffixs != "") { - suffixd = _split(suffixs, "|"); - } - - while ((ent = readdir(pdir)) != NULL) { - if (ent->d_type & DT_DIR) { - if (strcmp(ent->d_name, ".") == 0 || strcmp(ent->d_name, "..") == 0) { - continue; - } - if (r) { // If need to traverse subdirectories - childpath = dir + ent->d_name; - _list_dir(childpath, files); - } - } - else { - if (suffixd.size() > 0) { - bool can_push = false; - for (int i = 0; i < (int)suffixd.size(); i++) { - if (_endswith(ent->d_name, suffixd[i])) - can_push = true; - } - if (can_push) { - absolutepath = dir + ent->d_name; - files.push_back(ent->d_name); // filepath - } - } - else { - absolutepath = dir + ent->d_name; - files.push_back(ent->d_name); // filepath - } - } - } - sort(files.begin(), files.end()); //sort names -} - -vector _split(const string& srcstr, const string& delimeter) -{ - vector ret(0); //use ret save the spilted reault - if (srcstr.empty()) //judge the arguments - { - return ret; - } - string::size_type pos_begin = srcstr.find_first_not_of(delimeter); //find first element of srcstr - - string::size_type dlm_pos; //the delimeter postion - string temp; //use third-party temp to save splited element - while (pos_begin != string::npos) //if not a next of end, continue spliting - { - dlm_pos = srcstr.find(delimeter, pos_begin); //find the delimeter symbol - if (dlm_pos != string::npos) - { - temp = srcstr.substr(pos_begin, dlm_pos - pos_begin); - pos_begin = dlm_pos + delimeter.length(); - } - else - { - temp = srcstr.substr(pos_begin); - pos_begin = dlm_pos; - } - if (!temp.empty()) - ret.push_back(temp); - } - return ret; -} - -bool _startswith(const std::string& str, const std::string& start) -{ - size_t srclen = str.size(); - size_t startlen = start.size(); - if (srclen >= startlen) - { - string temp = str.substr(0, startlen); - if (temp == start) - return true; - } - - return false; -} - -bool _endswith(const std::string& str, const std::string& end) -{ - size_t srclen = str.size(); - size_t endlen = end.size(); - if (srclen >= endlen) - { - string temp = str.substr(srclen - endlen, endlen); - if (temp == end) - return true; - } - - return false; -} - -int inline randint(int l, int u) -{ - return l + rand() % (u - l + 1); - // rand() % (u-l+1) -> [0, u-l] - // [0, u-l] -> [l, u] -} - -void _randperm(int n, int m, int arr[], bool sort_) -{ - int* x = (int*)malloc(sizeof(int)*n); - for (int i = 0; i < n; ++i) - x[i] = i; - for (int i = 0; i < m; ++i) - { - int j = randint(i, n - 1); - int t = x[i]; x[i] = x[j]; x[j] = t; // swap(x[i], x[j]); - } - if (sort_) - sort(x, x + m); - for (int i = 0; i < m; ++i) - arr[i] = x[i]; - free(x); -} - -#define FL_PI 3.14159265358979323846f -#define FL_1_2_PI 1.57079632679f -#define FL_2__PI 6.28318530718 -float _atan2(float y, float x) -{ - float ang(0); - if (x > 0) - ang = atanf(y / x); - else if (y >= 0 && x < 0) - ang = atanf(y / x) + FL_PI; - else if (y < 0 && x < 0) - ang = atanf(y / x) - FL_PI; - else if (y > 0 && x == 0) - ang = FL_1_2_PI; - else if (y < 0 && x == 0) - ang = -FL_1_2_PI; - else // (y == 0 && x == 0) - ang = INFINITY; - // if (ang < 0) ang += FL_2__PI; - return ang; -} - -void _mean_std(std::vector& vec, float& mean, float& std) -{ - float sum = std::accumulate(std::begin(vec), std::end(vec), 0.0); - mean = sum / vec.size(); - - float accum = 0.0; - std::for_each(std::begin(vec), std::end(vec), [&](const double d) { - accum += (d - mean)*(d - mean); - }); - - std = sqrt(accum / (vec.size() - 1)); -} - -float _get_min_angle_PI(float alpha, float beta) -{ - float pi = float(CV_PI); - float pi2 = float(2.0 * CV_PI); - - //normalize data in [0, 2*pi] - float a = fmod(alpha + pi2, pi2); - float b = fmod(beta + pi2, pi2); - - //normalize data in [0, pi] - if (a > pi) - a -= pi; - if (b > pi) - b -= pi; - - if (a > b) - { - swap(a, b); - } - - float diff1 = b - a; - float diff2 = pi - diff1; - return min(diff1, diff2); -} - - -void _load_ellipse_GT(const string& gt_file_name, vector& gt_ellipses, bool is_angle_radians) -{ - ifstream in(gt_file_name); - if (!in.good()) - { - cout << "Error opening: " << gt_file_name << endl; - return; - } - - unsigned n; - in >> n; - - gt_ellipses.clear(); - gt_ellipses.reserve(n); - - while (in.good() && n--) - { - Ellipse e; - in >> e.xc_ >> e.yc_ >> e.a_ >> e.b_ >> e.rad_; - - if (!is_angle_radians) - { - // convert to radians - e.rad_ = float(e.rad_ * CV_PI / 180.0); - } - - if (e.a_ < e.b_) - { - float temp = e.a_; - e.a_ = e.b_; - e.b_ = temp; - - e.rad_ = e.rad_ + float(0.5*CV_PI); - } - - e.rad_ = fmod(float(e.rad_ + 2.f*CV_PI), float(CV_PI)); - e.score_ = 1.f; - gt_ellipses.push_back(e); - } - in.close(); -} - -void _load_ellipse_DT(const string& dt_file_name, vector& dt_ellipses, bool is_angle_radians) -{ - ifstream in(dt_file_name); - if (!in.good()) - { - cout << "Error opening: " << dt_file_name << endl; - return; - } - - unsigned n; - in >> n; - - dt_ellipses.clear(); - dt_ellipses.reserve(n); - - while (in.good() && n--) - { - Ellipse e; - in >> e.xc_ >> e.yc_ >> e.a_ >> e.b_ >> e.rad_ >> e.score_; - - if (!is_angle_radians) - { - // convert to radians - e.rad_ = float(e.rad_ * CV_PI / 180.0); - } - - if (e.a_ < e.b_) - { - float temp = e.a_; - e.a_ = e.b_; - e.b_ = temp; - - e.rad_ = e.rad_ + float(0.5*CV_PI); - } - - e.rad_ = fmod(float(e.rad_ + 2.f*CV_PI), float(CV_PI)); - e.score_ = 1.f; - dt_ellipses.push_back(e); - } - in.close(); -} - -bool _ellipse_overlap(const Mat1b& gt, const Mat1b& dt, float th) -{ - float f_and = float(countNonZero(gt & dt)); - float f_or = float(countNonZero(gt | dt)); - float f_sim = f_and / f_or; - - return (f_sim >= th); -} - -float _ellipse_overlap_real(const Mat1b& gt, const Mat1b& dt) -{ - float f_and = float(countNonZero(gt & dt)); - float f_or = float(countNonZero(gt | dt)); - float f_sim = f_and / f_or; - - return f_sim; -} - -int _bool_count(const std::vector vb) -{ - int counter = 0; - for (unsigned i = 0; i& ell_gt, const vector& ell_dt, const Mat3b& img) -{ - float threshold_overlap = 0.8f; - // float threshold = 0.95f; - - unsigned sz_gt = ell_gt.size(); - unsigned sz_dt = ell_dt.size(); - - unsigned sz_dt_min = unsigned(min(1000, int(sz_dt))); - - vector mat_gts(sz_gt); - vector mat_dts(sz_dt_min); - - // Draw each ground-Truth ellipse - for (unsigned i = 0; i vec_gt(sz_gt, false); - vector vec_dt(sz_dt_min, false); - - // Each row in the matrix has one means the ellipse be found - for (unsigned int i = 0; i < sz_dt_min; ++i) - { - for (unsigned int j = 0; j < sz_gt; ++j) - { - bool b_found = overlap(j, i) != 0; - if (b_found) - { - vec_gt[j] = true; - vec_dt[i] = true; - } - } - } - - float tp = _bool_count(vec_gt); - float fn = int(sz_gt) - tp; - float fp = sz_dt - _bool_count(vec_dt); // !!!! sz_dt - _bool_count(vec_dt); // - - float pr(0.f); - float re(0.f); - float fmeasure(0.f); - - if (tp == 0) { - if (fp == 0) { - pr = 1.f; - re = 0.f; - fmeasure = (2.f * pr * re) / (pr + re); - } - else { - pr = 0.f; - re = 0.f; - fmeasure = 0.f; - } - } - else { - pr = float(tp) / float(tp + fp); - re = float(tp) / float(tp + fn); - fmeasure = (2.f * pr * re) / (pr + re); - } - - return fmeasure; -} - -float _ellipse_evaluate(vector& image_fns, vector& gt_fns, vector& dt_fns, bool gt_angle_radians) -{ - float fmeasure(0.f); - for (int i = 0; i < image_fns.size(); i++) { - Mat3b image = imread(image_fns[i]); - - vector ell_gt, ell_dt; - _load_ellipse_GT(gt_fns[i], ell_gt, gt_angle_radians); - _load_ellipse_DT(dt_fns[i], ell_dt); - - int tp, fn, fp; - fmeasure += _ellipse_evaluate_one(ell_gt, ell_dt, image); - - } - - fmeasure /= image_fns.size(); - return fmeasure; -} - -Point2f inline _lineCrossPoint(Point2f l1p1, Point2f l1p2, Point2f l2p1, Point2f l2p2) -{ - Point2f crossPoint; - float k1, k2, b1, b2; - if (l1p1.x == l1p2.x&&l2p1.x == l2p2.x) { - crossPoint = Point2f(0, 0); // invalid point - return crossPoint; - } - if (l1p1.x == l1p2.x) - { - crossPoint.x = l1p1.x; - k2 = (l2p2.y - l2p1.y) / (l2p2.x - l2p1.x); - b2 = l2p1.y - k2*l2p1.x; - crossPoint.y = k2*crossPoint.x + b2; - return crossPoint; - } - if (l2p1.x == l2p2.x) - { - crossPoint.x = l2p1.x; - k2 = (l1p2.y - l1p1.y) / (l1p2.x - l1p1.x); - b2 = l1p1.y - k2*l1p1.x; - crossPoint.y = k2*crossPoint.x + b2; - return crossPoint; - } - - k1 = (l1p2.y - l1p1.y) / (l1p2.x - l1p1.x); - k2 = (l2p2.y - l2p1.y) / (l2p2.x - l2p1.x); - b1 = l1p1.y - k1*l1p1.x; - b2 = l2p1.y - k2*l2p1.x; - if (k1 == k2) - { - crossPoint = Point2f(0, 0); // invalid point - } - else - { - crossPoint.x = (b2 - b1) / (k1 - k2); - crossPoint.y = k1*crossPoint.x + b1; - } - return crossPoint; -} - -void inline _point2Mat(Point2f p1, Point2f p2, float mat[2][2]) -{ - mat[0][0] = p1.x; - mat[0][1] = p1.y; - mat[1][0] = p2.x; - mat[1][1] = p2.y; -} - -float _value4SixPoints(cv::Point2f p3, cv::Point2f p2, cv::Point2f p1, cv::Point2f p4, cv::Point2f p5, cv::Point2f p6) -{ - float result = 1; - Mat A, B, C; - float matB[2][2], matC[2][2]; - Point2f v, w, u; - v = _lineCrossPoint(p1, p2, p3, p4); - w = _lineCrossPoint(p5, p6, p3, p4); - u = _lineCrossPoint(p5, p6, p1, p2); - - _point2Mat(u, v, matB); - _point2Mat(p1, p2, matC); - B = Mat(2, 2, CV_32F, matB); - C = Mat(2, 2, CV_32F, matC); - A = C*B.inv(); - - // cout<<"u:\t"<(0, 0)*A.at(1, 0) / (A.at(0, 1)*A.at(1, 1)); - - _point2Mat(p3, p4, matC); - _point2Mat(v, w, matB); - B = Mat(2, 2, CV_32F, matB); - C = Mat(2, 2, CV_32F, matC); - A = C*B.inv(); - result *= A.at(0, 0)*A.at(1, 0) / (A.at(0, 1)*A.at(1, 1)); - - _point2Mat(p5, p6, matC); - _point2Mat(w, u, matB); - B = Mat(2, 2, CV_32F, matB); - C = Mat(2, 2, CV_32F, matC); - A = C*B.inv(); - result *= A.at(0, 0)*A.at(1, 0) / (A.at(0, 1)*A.at(1, 1)); - return result; -} - -/*----------------------------------------------------------------------------*/ -/** Compute ellipse foci, given ellipse params. -*/ -void _ellipse_foci(float *param, float *foci) -{ - float f; - /* check parameters */ - if (param == NULL) fprintf(stderr, "ellipse_foci: invalid input ellipse."); - if (foci == NULL) fprintf(stderr, "ellipse_foci: 'foci' must be non null."); - - f = sqrt(param[2] * param[2] - param[3] * param[3]); - foci[0] = param[0] + f * cos(param[4]); - foci[1] = param[1] + f * sin(param[4]); - foci[2] = param[0] - f * cos(param[4]); - foci[3] = param[1] - f * sin(param[4]); -} - -/*----------------------------------------------------------------------------*/ -/** Signed angle difference. -*/ -float angle_diff_signed(float a, float b) -{ - a -= b; - while (a <= -M_PI) a += M_2__PI; - while (a > M_PI) a -= M_2__PI; - return a; -} - -/*----------------------------------------------------------------------------*/ -/** Absolute value angle difference. -*/ -float _angle_diff(float a, float b) -{ - a -= b; - while (a <= -M_PI) a += M_2__PI; - while (a > M_PI) a -= M_2__PI; - if (a < 0.0) a = -a; - return a; -} - -/*----------------------------------------------------------------------------*/ -/** Compute the angle of the normal to a point belonging to an ellipse -using the focal property. -*/ -float _ellipse_normal_angle(float x, float y, float *foci) -{ - float tmp1, tmp2, tmp3, theta; - /* check parameters */ - if (foci == NULL) fprintf(stderr, "ellipse_normal_angle: 'foci' must be non null"); - - tmp1 = atan2(y - foci[1], x - foci[0]); - tmp2 = atan2(y - foci[3], x - foci[2]); - tmp3 = angle_diff_signed(tmp1, tmp2); - - theta = tmp1 - tmp3 / 2.0; - while (theta <= -M_PI) theta += M_2__PI; - while (theta > M_PI) theta -= M_2__PI; - return theta; -} - - -void cv_canny(const void* srcarr, void* dstarr, - void* dxarr, void* dyarr, - int aperture_size, bool L2gradient, double percent_ne) { - - cv::AutoBuffer buffer; - std::vector stack; - uchar **stack_top = 0, **stack_bottom = 0; - - CvMat srcstub, *src = cvGetMat(srcarr, &srcstub); // IplImage to cvMat - CvMat dststub, *dst = cvGetMat(dstarr, &dststub); - - CvMat dxstub, *dx = cvGetMat(dxarr, &dxstub); - CvMat dystub, *dy = cvGetMat(dyarr, &dystub); - - if (CV_MAT_TYPE(src->type) != CV_8UC1 || - CV_MAT_TYPE(dst->type) != CV_8UC1 || - CV_MAT_TYPE(dx->type) != CV_16SC1 || - CV_MAT_TYPE(dy->type) != CV_16SC1) - CV_Error(CV_StsUnsupportedFormat, ""); - - if (!CV_ARE_SIZES_EQ(src, dst)) - CV_Error(CV_StsUnmatchedSizes, ""); - - aperture_size &= INT_MAX; - if ((aperture_size & 1) == 0 || aperture_size < 3 || aperture_size > 7) - CV_Error(CV_StsBadFlag, ""); - - int i, j; - CvSize size; - size.width = src->cols; - size.height = src->rows; - - cvSobel(src, dx, 1, 0, aperture_size); - cvSobel(src, dy, 0, 1, aperture_size); - - // double min, max; - // cv::minMaxLoc(Mat(dx->rows, dx->cols, CV_16SC1, dx->data.fl), &min, &max); - // cout << "min: " << min << ", max: " << max << endl; - - Mat1f magGrad(size.height, size.width, 0.f); - float maxGrad(0); - float val(0); - for (i = 0; i < size.height; ++i) - { - float* _pmag = magGrad.ptr(i); - const short* _dx = (short*)(dx->data.ptr + dx->step*i); - const short* _dy = (short*)(dy->data.ptr + dy->step*i); - for (j = 0; j < size.width; ++j) - { - val = float(abs(_dx[j]) + abs(_dy[j])); - _pmag[j] = val; - maxGrad = (val > maxGrad) ? val : maxGrad; - } - } - // cout << "maxGrad: " << maxGrad << endl; - - // set magic numbers - const int NUM_BINS = 64; - const double percent_of_pixels_not_edges = percent_ne; - const double threshold_ratio = 0.3; - - // compute histogram - int bin_size = cvFloor(maxGrad / float(NUM_BINS) + 0.5f) + 1; - if (bin_size < 1) bin_size = 1; - int bins[NUM_BINS] = { 0 }; - for (i = 0; i < size.height; ++i) - { - float *_pmag = magGrad.ptr(i); - for (j = 0; j < size.width; ++j) - { - int hgf = int(_pmag[j]); - bins[int(_pmag[j]) / bin_size]++; - } - } - // for (int i = 0; i < NUM_BINS; i++) - // cout << "BIN " << i << " :" << bins[i] << endl; - - // Select the thresholds - float total(0.f); - float target = float(size.height * size.width * percent_of_pixels_not_edges); - int low_thresh, high_thresh(0); - - while (total < target) - { - total += bins[high_thresh]; - high_thresh++; - } - high_thresh *= bin_size; - low_thresh = cvFloor(threshold_ratio * float(high_thresh)); - // cout << "low_thresh: " << low_thresh << ", high_thresh: " << high_thresh << endl; - - int low, high, maxsize; - int* mag_buf[3]; - uchar* map; - ptrdiff_t mapstep; - - if (L2gradient) { - Cv32suf ul, uh; - ul.f = (float)low_thresh; - uh.f = (float)high_thresh; - - low = ul.i; - high = uh.i; - } - else { - low = cvFloor(low_thresh); - high = cvFloor(high_thresh); - } - - buffer.allocate((size.width + 2)*(size.height + 2) + (size.width + 2) * 3 * sizeof(int)); - // cout << sizeof(int) << ", " << (size.width + 2)*(size.height + 2) + (size.width + 2) * 3 * sizeof(int) << endl; - mag_buf[0] = (int*)(char*)buffer; - mag_buf[1] = mag_buf[0] + size.width + 2; - mag_buf[2] = mag_buf[1] + size.width + 2; - map = (uchar*)(mag_buf[2] + size.width + 2); - mapstep = size.width + 2; - - maxsize = MAX(1 << 10, size.width*size.height / 10); - stack.resize(maxsize); - stack_top = stack_bottom = &stack[0]; - - memset(mag_buf[0], 0, (size.width + 2) * sizeof(int)); - memset(map, 1, mapstep); - memset(map + mapstep*(size.height + 1), 1, mapstep); - - /* sector numbers - (Top-Left Origin) - - 1 2 3 - * * * - * * * - 0*******0 - * * * - * * * - 3 2 1 - */ - -#define CANNY_PUSH(d) *(d) = (uchar)2, *stack_top++ = (d) -#define CANNY_POP(d) (d) = *--stack_top - - CvMat mag_row = cvMat(1, size.width, CV_32F); - - // Mat push_show = Mat::zeros(size.height+1, size.width+1, CV_8U); - - // calculate magnitude and angle of gradient, perform non-maxima supression. - // fill the map with one of the following values: - // 0 - the pixel might belong to an edge - // 1 - the pixel can not belong to an edge - // 2 - the pixel does belong to an edge - for (i = 0; i <= size.height; i++) - { - int* _mag = mag_buf[(i > 0) + 1] + 1; - float* _magf = (float*)_mag; - const short* _dx = (short*)(dx->data.ptr + dx->step*i); - const short* _dy = (short*)(dy->data.ptr + dy->step*i); - uchar* _map; - int x, y; - ptrdiff_t magstep1, magstep2; - int prev_flag = 0; - - if (i < size.height) - { - _mag[-1] = _mag[size.width] = 0; - - if (!L2gradient) - for (j = 0; j < size.width; j++) - _mag[j] = abs(_dx[j]) + abs(_dy[j]); - else - { - for (j = 0; j < size.width; j++) - { - x = _dx[j]; y = _dy[j]; - _magf[j] = (float)std::sqrt((double)x*x + (double)y*y); - } - } - } - else - memset(_mag - 1, 0, (size.width + 2) * sizeof(int)); - - // at the very beginning we do not have a complete ring - // buffer of 3 magnitude rows for non-maxima suppression - if (i == 0) - continue; - - _map = map + mapstep*i + 1; - _map[-1] = _map[size.width] = 1; - - _mag = mag_buf[1] + 1; // take the central row - _dx = (short*)(dx->data.ptr + dx->step*(i - 1)); - _dy = (short*)(dy->data.ptr + dy->step*(i - 1)); - - magstep1 = mag_buf[2] - mag_buf[1]; - magstep2 = mag_buf[0] - mag_buf[1]; - - if ((stack_top - stack_bottom) + size.width > maxsize) - { - int sz = (int)(stack_top - stack_bottom); - maxsize = MAX(maxsize * 3 / 2, maxsize + 8); - stack.resize(maxsize); - stack_bottom = &stack[0]; - stack_top = stack_bottom + sz; - } - -#define CANNY_SHIFT 15 -#define TG22 (int)(0.4142135623730950488016887242097*(1< low) - { - int tg22x = x * TG22; - int tg67x = tg22x + ((x + x) << CANNY_SHIFT); - int tmp = 1 << CANNY_SHIFT; - y <<= CANNY_SHIFT; - - if (y < tg22x) { - if (m > _mag[j - 1] && m >= _mag[j + 1]) { - if (m > high && !prev_flag && _map[j - mapstep] != 2) { - CANNY_PUSH(_map + j); // push_show.at(i, j) = 255; - prev_flag = 1; - } - else { - _map[j] = (uchar)0; - } - continue; - } - } - else if (y > tg67x) { - if (m > _mag[j + magstep2] && m >= _mag[j + magstep1]) { - if (m > high && !prev_flag && _map[j - mapstep] != 2) { - CANNY_PUSH(_map + j); // push_show.at(i, j) = 255; - prev_flag = 1; - } - else { - _map[j] = (uchar)0; - } - continue; - } - } - else { - s = s < 0 ? -1 : 1; - if (m > _mag[j + magstep2 - s] && m > _mag[j + magstep1 + s]) { - if (m > high && !prev_flag && _map[j - mapstep] != 2) { - CANNY_PUSH(_map + j); // push_show.at(i, j) = 255; - prev_flag = 1; - } - else { - _map[j] = (uchar)0; - } - continue; - } - } - } - prev_flag = 0; - _map[j] = (uchar)1; - } - - // scroll the ring buffer - _mag = mag_buf[0]; - mag_buf[0] = mag_buf[1]; - mag_buf[1] = mag_buf[2]; - mag_buf[2] = _mag; - } - - // imshow("mag", push_show); waitKey(); - // now track the edges (hysteresis thresholding) - while (stack_top > stack_bottom) - { - uchar* m; - if ((stack_top - stack_bottom) + 8 > maxsize) - { - int sz = (int)(stack_top - stack_bottom); - maxsize = MAX(maxsize * 3 / 2, maxsize + 8); - stack.resize(maxsize); - stack_bottom = &stack[0]; - stack_top = stack_bottom + sz; - } - - CANNY_POP(m); - - if (!m[-1]) - CANNY_PUSH(m - 1); - if (!m[1]) - CANNY_PUSH(m + 1); - if (!m[-mapstep - 1]) - CANNY_PUSH(m - mapstep - 1); - if (!m[-mapstep]) - CANNY_PUSH(m - mapstep); - if (!m[-mapstep + 1]) - CANNY_PUSH(m - mapstep + 1); - if (!m[mapstep - 1]) - CANNY_PUSH(m + mapstep - 1); - if (!m[mapstep]) - CANNY_PUSH(m + mapstep); - if (!m[mapstep + 1]) - CANNY_PUSH(m + mapstep + 1); - } - - // the final pass, form the final image - for (i = 0; i < size.height; i++) { - const uchar* _map = map + mapstep*(i + 1) + 1; - uchar* _dst = dst->data.ptr + dst->step*i; - - for (j = 0; j < size.width; j++) { - // if (_map[j] == 2) - // cout << (int)_map[j] << ", " << (int)(_map[j] >> 1) << ", " << (int)(uchar)-(_map[j] >> 1) << endl; - _dst[j] = (uchar)-(_map[j] >> 1); - } - } -} - -void _tag_canny(InputArray image, OutputArray _edges, - OutputArray _sobel_x, OutputArray _sobel_y, - int apertureSize, bool L2gradient, double percent_ne) { - - Mat src = image.getMat(); - _edges.create(src.size(), CV_8U); - _sobel_x.create(src.size(), CV_16S); - _sobel_y.create(src.size(), CV_16S); - - IplImage c_src = IplImage(src); - IplImage c_dst = IplImage(_edges.getMat()); - IplImage c_dx = IplImage(_sobel_x.getMat()); - IplImage c_dy = IplImage(_sobel_y.getMat()); - - cv_canny(&c_src, &c_dst, - &c_dx, &c_dy, - apertureSize, L2gradient, percent_ne); -} - -void _find_contours_eight(cv::Mat1b& image, std::vector& segments, int iMinLength) -{ - vector ims; ims.resize(8); - for (int i = 0; i < 8; i++) { - ims[i] = Mat1b::zeros(image.size()); - } - for (int r = 0; r < image.rows; r++) { - uchar* _e8 = image.ptr(r); - vector _es; _es.resize(8); - for (int i = 0; i < 8; i++) - _es[i] = ims[i].ptr(r); - - for (int c = 0; c < image.cols; c++) { - for (int i = 0; i < 8; i++) { - if (_e8[c] == (uchar)(i + 1)) { - _es[i][c] = (uchar)255; - } - } - } - } - - segments.resize(8); - for (int i = 0; i < 8; i++) { - _tag_find_contours(ims[i], segments[i], iMinLength); - } -} - -void _tag_find_contours(cv::Mat1b& image, VVP& segments, int iMinLength) { -#define RG_STACK_SIZE 8192*4 - - - // use stack to speed up the processing of global (even at the expense of memory occupied) - int stack2[RG_STACK_SIZE]; -#define RG_PUSH2(a) (stack2[sp2] = (a) , sp2++) -#define RG_POP2(a) (sp2-- , (a) = stack2[sp2]) - - // use stack to speed up the processing of global (even at the expense of memory occupied) - Point stack3[RG_STACK_SIZE]; -#define RG_PUSH3(a) (stack3[sp3] = (a) , sp3++) -#define RG_POP3(a) (sp3-- , (a) = stack3[sp3]) - - int i, w, h, iDim; - int x, y; - int x2, y2; - int sp2; // stack pointer - int sp3; - - Mat_ src = image.clone(); - w = src.cols; - h = src.rows; - iDim = w*h; - - Point point; - for (y = 0; y0) - { // rg traditional - RG_POP2(i); - x2 = i%w; - y2 = i / w; - - point.x = x2; - point.y = y2; - - if (src(y2, x2)) - { - RG_PUSH3(point); - src(y2, x2) = 0; - } - - // insert the new points in the stack only if there are - // and they are points labelled - - // 4 connected - // left - if (x2>0 && (src(y2, x2 - 1) != 0)) - RG_PUSH2(i - 1); - // up - if (y2>0 && (src(y2 - 1, x2) != 0)) - RG_PUSH2(i - w); - // down - if (y20 && y2>0 && (src(y2 - 1, x2 - 1) != 0)) - RG_PUSH2(i - w - 1); - if (x2>0 && y20 && (src(y2 - 1, x2 + 1) != 0)) - RG_PUSH2(i - w + 1); - if (x2= iMinLength) - { - vector component; - component.reserve(sp3); - - // push it to the points - for (i = 0; i src = image.clone(); - w = src.cols; - h = src.rows; - iDim = w*h; - - Point point; - for (y = 0; y component; - point.x = x; - point.y = y; - component.push_back(point); - x2 = x; - y2 = y; - - bool found = true; - sp2 = 0; - while (found && component.size() < 3) - { - found = false; - if (x2 > 0 && y2 < h - 1 && (src(y2 + 1, x2 - 1) != 0)) - { - src(y2 + 1, x2 - 1) = 0; if (!found) { found = true; point.x = x2 - 1; point.y = y2 + 1; component.push_back(point); } - } - if (x2 < w - 1 && y2 < h - 1 && (src(y2 + 1, x2 + 1) != 0)) - { - src(y2 + 1, x2 + 1) = 0; if (!found) { found = true; point.x = x2 + 1; point.y = y2 + 1; component.push_back(point); } - } - if (y2 < h - 1 && (src(y2 + 1, x2) != 0)) - { - src(y2 + 1, x2) = 0; if (!found) { found = true; point.x = x2; point.y = y2 + 1; component.push_back(point); } - } - if (x2 > 0 && (src(y2, x2 - 1) != 0)) - { - src(y2, x2 - 1) = 0; if (!found) { found = true; point.x = x2 - 1; point.y = y2; component.push_back(point); } - } - if (x2 < w - 1 && (src(y2, x2 + 1) != 0)) - { - src(y2, x2 + 1) = 0; if (!found) { found = true; point.x = x2 + 1; point.y = y2; component.push_back(point); } - } - if (x2 > 0 && y2 > 0 && (src(y2 - 1, x2 - 1) != 0)) - { - src(y2 - 1, x2 - 1) = 0; if (!found) { found = true; point.x = x2 - 1; point.y = y2 - 1; component.push_back(point); } - } - if (x2 < w - 1 && y2 > 0 && (src(y2 - 1, x2 + 1) != 0)) - { - src(y2 - 1, x2 + 1) = 0; if (!found) { found = true; point.x = x2 + 1; point.y = y2 - 1; component.push_back(point); } - } - if (y2 > 0 && (src(y2 - 1, x2) != 0)) - { - src(y2 - 1, x2) = 0; if (!found) { found = true; point.x = x2; point.y = y2 - 1; component.push_back(point); } - } - if (found) - { - sp2++; x2 = component[sp2].x; y2 = component[sp2].y; - } - } - - if (component.size() < 3) continue; - ang = _atan2(component[2].y - component[0].y, component[2].x - component[0].x); - sp1 = 0; - - found = true; - while (found) - { - ang_dmin = 1e3; - found = false; - if (x2 > 0 && y2 < h - 1 && (src(y2 + 1, x2 - 1) != 0)) - { - ang_t = _atan2(y2 + 1 - component[sp1].y, x2 - 1 - component[sp1].x); - ang_d = abs(ang - ang_t); - src(y2 + 1, x2 - 1) = 0; if (ang_d < ang_dmin) { ang_dmin = ang_d; ang_i = ang_t; xn = x2 - 1; yn = y2 + 1; } - } - if (x2 < w - 1 && y2 < h - 1 && (src(y2 + 1, x2 + 1) != 0)) - { - ang_t = _atan2(y2 + 1 - component[sp1].y, x2 + 1 - component[sp1].x); - ang_d = abs(ang - ang_t); - src(y2 + 1, x2 + 1) = 0; if (ang_d < ang_dmin) { ang_dmin = ang_d; ang_i = ang_t; xn = x2 + 1; yn = y2 + 1; } - } - if (y2 < h - 1 && (src(y2 + 1, x2) != 0)) - { - ang_t = _atan2(y2 + 1 - component[sp1].y, x2 - component[sp1].x); - ang_d = abs(ang - ang_t); - src(y2 + 1, x2) = 0; if (ang_d < ang_dmin) { ang_dmin = ang_d; ang_i = ang_t; xn = x2; yn = y2 + 1; } - } - if (x2 > 0 && (src(y2, x2 - 1) != 0)) - { - ang_t = _atan2(y2 - component[sp1].y, x2 - 1 - component[sp1].x); - ang_d = abs(ang - ang_t); - src(y2, x2 - 1) = 0; if (ang_d < ang_dmin) { ang_dmin = ang_d; ang_i = ang_t; xn = x2 - 1; yn = y2; } - } - if (x2 < w - 1 && (src(y2, x2 + 1) != 0)) - { - ang_t = _atan2(y2 - component[sp1].y, x2 + 1 - component[sp1].x); - ang_d = abs(ang - ang_t); - src(y2, x2 + 1) = 0; if (ang_d < ang_dmin) { ang_dmin = ang_d; ang_i = ang_t; xn = x2 + 1; yn = y2; } - } - if (x2 > 0 && y2 > 0 && (src(y2 - 1, x2 - 1) != 0)) - { - ang_t = _atan2(y2 - 1 - component[sp1].y, x2 - 1 - component[sp1].x); - ang_d = abs(ang - ang_t); - src(y2 - 1, x2 - 1) = 0; if (ang_d < ang_dmin) { ang_dmin = ang_d; ang_i = ang_t; xn = x2 - 1; yn = y2 - 1; } - } - if (x2 < w - 1 && y2 > 0 && (src(y2 - 1, x2 + 1) != 0)) - { - ang_t = _atan2(y2 - 1 - component[sp1].y, x2 + 1 - component[sp1].x); - ang_d = abs(ang - ang_t); - src(y2 - 1, x2 + 1) = 0; if (ang_d < ang_dmin) { ang_dmin = ang_d; ang_i = ang_t; xn = x2 + 1; yn = y2 - 1; } - } - if (y2 > 0 && (src(y2 - 1, x2) != 0)) - { - ang_t = _atan2(y2 - 1 - component[sp1].y, x2 - component[sp1].x); - ang_d = abs(ang - ang_t); - src(y2 - 1, x2) = 0; if (ang_d < ang_dmin) { ang_dmin = ang_d; ang_i = ang_t; xn = x2; yn = y2 - 1; } - } - if (ang_dmin < M_1_2_PI) - { - found = true; point.x = xn; point.y = yn; component.push_back(point); - x2 = xn; - y2 = yn; - sp1++; sp2++; - if (sp2 >= 9 && sp2 % 3 == 0) - { - float a1 = _atan2(component[sp2 - 6].y - component[sp2 - 9].y, component[sp2 - 6].x - component[sp2 - 9].x); - float a2 = _atan2(component[sp2 - 3].y - component[sp2 - 6].y, component[sp2 - 3].x - component[sp2 - 6].x); - theta_s = a1 - a2; - - a1 = _atan2(component[sp2 - 3].y - component[sp2 - 6].y, component[sp2 - 3].x - component[sp2 - 6].x); - a2 = _atan2(component[sp2].y - component[sp2 - 3].y, component[sp2].x - component[sp2 - 3].x); - theta_i = a1 - a2; - if (abs(theta_s - theta_i) > 0.6) break; - } - ang = ang_i; - } - } - - if (component.size() >= iMinLength) - { - segments.push_back(component); - } - } - } - } -} - - - -void _tag_show_contours(cv::Mat1b& image, VVP& segments, const char* title) -{ - Mat3b contoursIm(image.rows, image.cols, Vec3b(0, 0, 0)); - for (size_t i = 0; i& segments8, const char* title) -{ - Mat3b contoursIm(image.rows, image.cols, Vec3b(0, 0, 0)); - for (size_t i = 0; i < segments8.size(); ++i) - { - Vec3b color(rand() % 255, 128 + rand() % 127, 128 + rand() % 127); - for (size_t j = 0; j < segments8[i].size(); ++j) - { - for (size_t k = 0; k < segments8[i][j].size(); ++k) - contoursIm(segments8[i][j][k]) = color; - } - } - imshow(title, contoursIm); -} - -bool _SortBottomLeft2TopRight(const Point& lhs, const Point& rhs) -{ - if (lhs.x == rhs.x) - { - return lhs.y > rhs.y; - } - return lhs.x < rhs.x; -} - -bool _SortBottomLeft2TopRight2f(const Point2f& lhs, const Point2f& rhs) -{ - if (lhs.x == rhs.x) - { - return lhs.y > rhs.y; - } - return lhs.x < rhs.x; -} - -bool _SortTopLeft2BottomRight(const Point& lhs, const Point& rhs) -{ - if (lhs.x == rhs.x) - { - return lhs.y < rhs.y; - } - return lhs.x < rhs.x; -} - - - - - - -// #define DEBUG_SPEED -// #define DEBUG_ELLFIT -// #define DEBUG_PREPROCESSING - -// #define DISCARD_TCN -#define DISCARD_TCN2 -#define DISCARD_CONSTRAINT_OBOX - -// #define DISCARD_CONSTRAINT_CONVEXITY -// #define DISCARD_CONSTRAINT_POSITION -#define CONSTRAINT_CNC_1 -// #define CONSTRAINT_CNC_2 -// #define CONSTRAINT_CNC_3 -// #define DISCARD_CONSTRAINT_CENTER - -// #define T_CNC 0.2f -// #define T_TCN_L 0.4f // filter lines -// #define T_TCN_P 0.6f - -// #define Thre_r 0.2f - - -void _concate_arcs(VP& arc1, VP& arc2, VP& arc3, VP& arc) -{ - for (int i = 0; i < arc1.size(); i++) - arc.push_back(arc1[i]); - for (int i = 0; i < arc2.size(); i++) - arc.push_back(arc2[i]); - for (int i = 0; i < arc3.size(); i++) - arc.push_back(arc3[i]); -} - -void _concate_arcs(VP& arc1, VP& arc2, VP& arc) -{ - for (int i = 0; i < arc1.size(); i++) - arc.push_back(arc1[i]); - for (int i = 0; i < arc2.size(); i++) - arc.push_back(arc2[i]); -} - -EllipseDetector::EllipseDetector(void) -{ - // Default Parameters Settings - szPreProcessingGaussKernel_ = Size(5, 5); - dPreProcessingGaussSigma_ = 1.0; - fThrArcPosition_ = 1.0f; - fMaxCenterDistance_ = 100.0f * 0.05f; - fMaxCenterDistance2_ = fMaxCenterDistance_ * fMaxCenterDistance_; - iMinEdgeLength_ = 16; - fMinOrientedRectSide_ = 3.0f; - fDistanceToEllipseContour_ = 0.1f; - fMinScore_ = 0.7f; - fMinReliability_ = 0.5f; - uNs_ = 16; - dPercentNe_ = 0.9; - - fT_CNC_ = 0.2f; - fT_TCN_L_ = 0.4f; // filter lines - fT_TCN_P_ = 0.6f; - fThre_r_ = 0.2f; - - srand(unsigned(time(NULL))); -} - -EllipseDetector::~EllipseDetector(void) -{ -} - -void EllipseDetector::SetParameters(Size szPreProcessingGaussKernel, - double dPreProcessingGaussSigma, // 高斯模糊,去噪声 - float fThPosition, // 位置约束,阈值,低于阈值不要 - float fMaxCenterDistance, // 中心点约束 - int iMinEdgeLength, // 最小弧度常识,一般 8-16, 小于的弧不要 - float fMinOrientedRectSide, // 筛选弧参数,最小外接矩形,短边长度 - float fDistanceToEllipseContour, // fitting得分d, 一般几个像素 - float fMinScore, // 0.5 - 0.7, 最终椭圆得分阈值 - float fMinReliability, // 弧长与椭圆周长比值阈值 - int iNs, // 中心性约数, 2个弧中心与椭圆中心距离 一般22 - double dPercentNe, // 边缘检测阈值, 越大保留的弧数量越多 0.9 - 0.99 - float fT_CNC, // 判读集合CNC, 一般不会调 - float fT_TCN_L, - float fT_TCN_P, - float fThre_r -) -{ - szPreProcessingGaussKernel_ = szPreProcessingGaussKernel; - dPreProcessingGaussSigma_ = dPreProcessingGaussSigma; - fThrArcPosition_ = fThPosition; - fMaxCenterDistance_ = fMaxCenterDistance; - iMinEdgeLength_ = iMinEdgeLength; - fMinOrientedRectSide_ = fMinOrientedRectSide; - fDistanceToEllipseContour_ = fDistanceToEllipseContour; - fMinScore_ = fMinScore; - fMinReliability_ = fMinReliability; - uNs_ = iNs; - dPercentNe_ = dPercentNe; - - fT_CNC_ = fT_CNC; - fT_TCN_L_ = fT_TCN_L; // filter lines - fT_TCN_P_ = fT_TCN_P; - fThre_r_ = fThre_r; - - fMaxCenterDistance2_ = fMaxCenterDistance_ * fMaxCenterDistance_; -} - -void EllipseDetector::RemoveStraightLine(VVP& segments, VVP& segments_update, int id) -{ - int countedges = 0; - // For each edge - for (int i = 0; i < segments.size(); ++i) - { - VP& edgeSegment = segments[i]; - -#ifndef DISCARD_CONSTRAINT_OBOX - // Selection strategy - Step 1 - See Sect [3.1.2] of the paper - // Constraint on axes aspect ratio - RotatedRect oriented = minAreaRect(edgeSegment); - float o_min = min(oriented.size.width, oriented.size.height); - - if (o_min < fMinOrientedRectSide_) - { - countedges++; - continue; - } -#endif - - // Order edge points of the same arc - if (id == 0 || id == 1 || id == 4 || id == 5) { - sort(edgeSegment.begin(), edgeSegment.end(), _SortTopLeft2BottomRight); - } - else if (id == 2 || id == 3 || id == 6 || id == 7) { - sort(edgeSegment.begin(), edgeSegment.end(), _SortBottomLeft2TopRight); - } - - int iEdgeSegmentSize = int(edgeSegment.size()); - - // Get extrema of the arc - Point& left = edgeSegment[0]; - Point& right = edgeSegment[iEdgeSegmentSize - 1]; - -#ifndef DISCARD_TCN -#ifndef DISCARD_TCN2 - int flag = 0; - for (int j = 0; j fT_TCN_L_) { - flag = 1; - break; - } - } - if (0 == flag) { - countedges++; - continue; - } -#endif -#ifndef DISCARD_TCN1 - Point& mid = edgeSegment[iEdgeSegmentSize / 2]; - float data[] = { float(left.x), float(left.y), 1.0f, float(mid.x), float(mid.y), - 1.0f, float(right.x), float(right.y), 1.0f }; - Mat threePoints(Size(3, 3), CV_32FC1, data); - double ans = determinant(threePoints); - - float dx = 1.0f*(left.x - right.x); - float dy = 1.0f*(left.y - right.y); - float edgelength2 = dx*dx + dy*dy; - // double TCNl = ans / edgelength2; - double TCNl = ans / (2 * pow(edgelength2, fT_TCN_P_)); - if (abs(TCNl) < fT_TCN_L_) { - countedges++; - continue; - } -#endif -#endif - - segments_update.push_back(edgeSegment); - } -} - -void EllipseDetector::Detect(cv::Mat& I, std::vector& ellipses) -{ - countOfFindEllipse_ = 0; - countOfGetFastCenter_ = 0; - - Mat1b gray; - cvtColor(I, gray, CV_BGR2GRAY); - - - // Set the image size - szIm_ = I.size(); - - // Initialize temporary data structures - Mat1b DP = Mat1b::zeros(szIm_); // arcs along positive diagonal - Mat1b DN = Mat1b::zeros(szIm_); // arcs along negative diagonal - - - ACC_N_SIZE = 101; - ACC_R_SIZE = 180; - ACC_A_SIZE = max(szIm_.height, szIm_.width); - - // Allocate accumulators - accN = new int[ACC_N_SIZE]; - accR = new int[ACC_R_SIZE]; - accA = new int[ACC_A_SIZE]; - - - // Other temporary - - unordered_map centers; // hash map for reusing already computed EllipseData - - PreProcessing(gray, DP, DN); - - - points_1.clear(); - points_2.clear(); - points_3.clear(); - points_4.clear(); - // Detect edges and find convexities - DetectEdges13(DP, points_1, points_3); - DetectEdges24(DN, points_2, points_4); - - - Triplets124(points_1, points_2, points_4, centers, ellipses); - Triplets231(points_2, points_3, points_1, centers, ellipses); - Triplets342(points_3, points_4, points_2, centers, ellipses); - Triplets413(points_4, points_1, points_3, centers, ellipses); - - sort(ellipses.begin(), ellipses.end()); - - - // Free accumulator memory - delete[] accN; - delete[] accR; - delete[] accA; - - // Cluster detections - ClusterEllipses(ellipses); -} - -void EllipseDetector::Detect(Mat3b& I, vector& ellipses) -{ - countOfFindEllipse_ = 0; - countOfGetFastCenter_ = 0; - -#ifdef DEBUG_SPEED - Tic(0); // prepare data structure -#endif - - // Convert to grayscale - Mat1b gray; - cvtColor(I, gray, CV_BGR2GRAY); - - // Set the image size - szIm_ = I.size(); - - // Initialize temporary data structures - Mat1b DP = Mat1b::zeros(szIm_); // arcs along positive diagonal - Mat1b DN = Mat1b::zeros(szIm_); // arcs along negative diagonal - - // Initialize accumulator dimensions - ACC_N_SIZE = 101; - ACC_R_SIZE = 180; - ACC_A_SIZE = max(szIm_.height, szIm_.width); - - // Allocate accumulators - accN = new int[ACC_N_SIZE]; - accR = new int[ACC_R_SIZE]; - accA = new int[ACC_A_SIZE]; - - // Other temporary - - unordered_map centers; // hash map for reusing already computed EllipseData - -#ifdef DEBUG_SPEED - Toc(0, "prepare data structure"); // prepare data structure -#endif - - // Preprocessing - // From input image I, find edge point with coarse convexity along positive (DP) or negative (DN) diagonal - PreProcessing(gray, DP, DN); - -#ifdef DEBUG_SPEED - Tic(3); // preprocessing -#endif - - points_1.clear(); - points_2.clear(); - points_3.clear(); - points_4.clear(); - // Detect edges and find convexities - DetectEdges13(DP, points_1, points_3); - DetectEdges24(DN, points_2, points_4); - -#ifdef DEBUG_SPEED - Toc(3, "preprocessing_2"); // preprocessing -#endif - - // #define DEBUG_PREPROCESSING_S4 -#ifdef DEBUG_PREPROCESSING_S4 - Mat3b out(I.rows, I.cols, Vec3b(0, 0, 0)); - for (unsigned i = 0; i < points_1.size(); ++i) - { - // Vec3b color(rand()%255, 128+rand()%127, 128+rand()%127); - Vec3b color(255, 0, 0); - for (unsigned j = 0; j < points_1[i].size(); ++j) - out(points_1[i][j]) = color; - } - for (unsigned i = 0; i < points_2.size(); ++i) - { - // Vec3b color(rand()%255, 128+rand()%127, 128+rand()%127); - Vec3b color(0, 255, 0); - for (unsigned j = 0; j < points_2[i].size(); ++j) - out(points_2[i][j]) = color; - } - for (unsigned i = 0; i < points_3.size(); ++i) - { - // Vec3b color(rand()%255, 128+rand()%127, 128+rand()%127); - Vec3b color(0, 0, 255); - for (unsigned j = 0; j < points_3[i].size(); ++j) - out(points_3[i][j]) = color; - } - for (unsigned i = 0; i < points_4.size(); ++i) - { - // Vec3b color(rand()%255, 128+rand()%127, 128+rand()%127); - Vec3b color(255, 0, 255); - for (unsigned j = 0; j < points_4[i].size(); ++j) - out(points_4[i][j]) = color; - } - cv::imshow("PreProcessing->Output", out); - waitKey(); -#endif - -#ifdef DEBUG_SPEED - Tic(4); // grouping -#endif - - - Triplets124(points_1, points_2, points_4, centers, ellipses); - Triplets231(points_2, points_3, points_1, centers, ellipses); - Triplets342(points_3, points_4, points_2, centers, ellipses); - Triplets413(points_4, points_1, points_3, centers, ellipses); - -#ifdef DEBUG_SPEED - Toc(4, "grouping"); // grouping -#endif - -#ifdef DEBUG_SPEED - Tic(5); -#endif - - // Sort detected ellipses with respect to score - sort(ellipses.begin(), ellipses.end()); - - // Free accumulator memory - delete[] accN; - delete[] accR; - delete[] accA; - - // Cluster detections - ClusterEllipses(ellipses); - -#ifdef DEBUG_SPEED - Toc(5, "cluster detections"); -#endif -} - -void EllipseDetector::PreProcessing(Mat1b& I, Mat1b& arcs8) -{ - GaussianBlur(I, I, szPreProcessingGaussKernel_, dPreProcessingGaussSigma_); - - // Temp variables - Mat1b E; // edge mask - Mat1s DX, DY; // sobel derivatives - - _tag_canny(I, E, DX, DY, 3, false, dPercentNe_); // Detect edges - - // Mat1f dxf, dyf; - // normalize(DX, dxf, 0, 1, NORM_MINMAX); - // normalize(DY, dyf, 0, 1, NORM_MINMAX); - //Mat1f dx, dy; - //DX.convertTo(dx, CV_32F); - //DY.convertTo(dy, CV_32F); - //// DYDX_ = dy / dx; - - ////Mat1f edx, edy; - ////Sobel(E, edx, CV_32F, 1, 0); - ////Sobel(E, edy, CV_32F, 0, 1); - - //DYDX_ = -1 / (dy / dx); - //CV_Assert(DYDX_.type() == CV_32F); - - //cv::GaussianBlur(dx, dx, Size(5, 5), 0, 0); - //cv::GaussianBlur(dy, dy, Size(5, 5), 0, 0); - //cv::phase(dx, dy, EO_); - -#define SIGN(n) ((n)<=0?((n)<0?-1:0):1) - - // cout << SIGN(0) << " " << SIGN(-1) << " " << SIGN(1) << endl; - - int sign_x, sign_y, sign_xy; - for (int i = 0; i < szIm_.height; ++i) { - short* _dx = DX.ptr(i); - short* _dy = DY.ptr(i); - uchar* _e = E.ptr(i); - uchar* _arc = arcs8.ptr(i); - - for (int j = 0; j < szIm_.width; ++j) { - if (!(_e[j] <= 0)) // !!!!!! - { - sign_x = SIGN(_dx[j]); - sign_y = SIGN(_dy[j]); - sign_xy = SIGN(abs(_dx[j]) - abs(_dy[j])); - if (sign_x == 1 && sign_y == 1 && sign_xy == 1) { - _arc[j] = (uchar)3; - } - else if (sign_x == 1 && sign_y == 1 && sign_xy == -1) { - _arc[j] = (uchar)4; - } - else if (sign_x == 1 && sign_y == -1 && sign_xy == -1) { - _arc[j] = (uchar)1; - } - else if (sign_x == 1 && sign_y == -1 && sign_xy == 1) { - _arc[j] = (uchar)2; - } - else if (sign_x == -1 && sign_y == -1 && sign_xy == 1) { - _arc[j] = (uchar)7; - } - else if (sign_x == -1 && sign_y == -1 && sign_xy == -1) { - _arc[j] = (uchar)8; - } - else if (sign_x == -1 && sign_y == 1 && sign_xy == -1) { - _arc[j] = (uchar)5; - } - else if (sign_x == -1 && sign_y == 1 && sign_xy == 1) { - _arc[j] = (uchar)6; - } - } - } - } -} - -void EllipseDetector::PreProcessing(Mat1b& I, Mat1b& DP, Mat1b& DN) -{ -#ifdef DEBUG_SPEED - Tic(1); // edge detection -#endif - - GaussianBlur(I, I, szPreProcessingGaussKernel_, dPreProcessingGaussSigma_); - - // Temp variables - Mat1b E; // edge mask - Mat1s DX, DY; // sobel derivatives - - // Detect edges - _tag_canny(I, E, DX, DY, 3, false, dPercentNe_); - - Mat1f dx, dy; - DX.convertTo(dx, CV_32F); - DY.convertTo(dy, CV_32F); - //// cv::GaussianBlur(dx, dx, Size(5, 5), 0, 0); - //// cv::GaussianBlur(dy, dy, Size(5, 5), 0, 0); - cv::phase(dx, dy, EO_); - -#ifdef DEBUG_PREPROCESSING - imshow("PreProcessing->Edge", E); waitKey(50); -#endif - -#ifdef DEBUG_SPEED - Toc(1, "edge detection"); // edge detection - Tic(2); // preprocessing -#endif - - float M_3_2_PI = M_PI + M_1_2_PI; - // For each edge points, compute the edge direction - for (int i = 0; i < szIm_.height; ++i) { - float* _o = EO_.ptr(i); - uchar* _e = E.ptr(i); - uchar* _dp = DP.ptr(i); - uchar* _dn = DN.ptr(i); - - for (int j = 0; j < szIm_.width; ++j) { - if (!(_e[j] <= 0)) // !!!!!! - { - if (_o[j] == 0 || _o[j] == M_1_2_PI || _o[j] == M_PI || _o[j] == M_3_2_PI) { - _dn[j] = (uchar)255; _dp[j] = (uchar)255; - } - else if ((_o[j] > 0 && _o[j] < M_1_2_PI) || (_o[j] > M_PI && _o[j] < M_3_2_PI)) _dn[j] = (uchar)255; - else _dp[j] = (uchar)255; - } - } - } - -#ifdef DEBUG_PREPROCESSING - imshow("PreProcessing->DP", DP); waitKey(50); - imshow("PreProcessing->DN", DN); waitKey(); -#endif -#ifdef DEBUG_SPEED - Toc(2, "preprocessing"); // preprocessing -#endif -} - -void EllipseDetector::DetectEdges13(Mat1b& DP, VVP& points_1, VVP& points_3) -{ - // Vector of connected edge points - VVP contours; - int countedges = 0; - // Labeling 8-connected edge points, discarding edge too small - _tag_find_contours(DP, contours, iMinEdgeLength_); // put small arc edges to a vector - -#ifdef DEBUG_PREPROCESSING - Mat1b DP_show = DP.clone(); - _tag_show_contours(DP_show, contours, "PreProcessing->Contours13"); waitKey(); -#endif - - - // For each edge - for (int i = 0; i < contours.size(); ++i) - { - VP& edgeSegment = contours[i]; - -#ifndef DISCARD_CONSTRAINT_OBOX - // Selection strategy - Step 1 - See Sect [3.1.2] of the paper - // Constraint on axes aspect ratio - RotatedRect oriented = minAreaRect(edgeSegment); - float o_min = min(oriented.size.width, oriented.size.height); - - if (o_min < fMinOrientedRectSide_) - { - countedges++; - continue; - } -#endif - - // Order edge points of the same arc - sort(edgeSegment.begin(), edgeSegment.end(), _SortTopLeft2BottomRight); - int iEdgeSegmentSize = int(edgeSegment.size()); - - // Get extrema of the arc - Point& left = edgeSegment[0]; - Point& right = edgeSegment[iEdgeSegmentSize - 1]; - -#ifndef DISCARD_TCN -#ifndef DISCARD_TCN2 - int flag = 0; - for (int j = 0; j fT_TCN_L_) { - flag = 1; - break; - } - } - if (0 == flag) { - countedges++; - continue; - } -#endif -#ifndef DISCARD_TCN1 - Point& mid = edgeSegment[iEdgeSegmentSize / 2]; - float data[] = { float(left.x), float(left.y), 1.0f, float(mid.x), float(mid.y), - 1.0f, float(right.x), float(right.y), 1.0f }; - Mat threePoints(Size(3, 3), CV_32FC1, data); - double ans = determinant(threePoints); - - float dx = 1.0f*(left.x - right.x); - float dy = 1.0f*(left.y - right.y); - float edgelength2 = dx*dx + dy*dy; - // double TCNl = ans / edgelength2; - double TCNl = ans / (2 * pow(edgelength2, fT_TCN_P_)); - if (abs(TCNl) < fT_TCN_L_) { - countedges++; - continue; - } -#endif -#endif - - // Find convexity - See Sect [3.1.3] of the paper - int iCountTop = 0; - int xx = left.x; - for (int k = 1; k < iEdgeSegmentSize; ++k) - { - if (edgeSegment[k].x == xx) continue; - - iCountTop += (edgeSegment[k].y - left.y); - xx = edgeSegment[k].x; - } - - int width = abs(right.x - left.x) + 1; - int height = abs(right.y - left.y) + 1; - int iCountBottom = (width * height) - iEdgeSegmentSize - iCountTop; - - if (iCountBottom > iCountTop) - { // 1 - points_1.push_back(edgeSegment); - } - else if (iCountBottom < iCountTop) - { // 3 - points_3.push_back(edgeSegment); - } - } - -} - -void EllipseDetector::DetectEdges24(Mat1b& DN, VVP& points_2, VVP& points_4) -{ - // Vector of connected edge points - VVP contours; - int countedges = 0; - /// Labeling 8-connected edge points, discarding edge too small - _tag_find_contours(DN, contours, iMinEdgeLength_); - -#ifdef DEBUG_PREPROCESSING - _tag_show_contours(DN, contours, "PreProcessing->Contours24"); waitKey(); -#endif - - int iContoursSize = unsigned(contours.size()); - - - // For each edge - for (int i = 0; i < iContoursSize; ++i) - { - VP& edgeSegment = contours[i]; - -#ifndef DISCARD_CONSTRAINT_OBOX - // Selection strategy - Step 1 - See Sect [3.1.2] of the paper - // Constraint on axes aspect ratio - RotatedRect oriented = minAreaRect(edgeSegment); - float o_min = min(oriented.size.width, oriented.size.height); - - if (o_min < fMinOrientedRectSide_) - { - countedges++; - continue; - } -#endif - - // Order edge points of the same arc - sort(edgeSegment.begin(), edgeSegment.end(), _SortBottomLeft2TopRight); - int iEdgeSegmentSize = unsigned(edgeSegment.size()); - - // Get extrema of the arc - Point& left = edgeSegment[0]; - Point& right = edgeSegment[iEdgeSegmentSize - 1]; - -#ifndef DISCARD_TCN -#ifndef DISCARD_TCN2 - int flag = 0; - for (int j = 0; j fT_TCN_L_) { - flag = 1; - break; - } - } - if (0 == flag) { - countedges++; - continue; - } -#endif -#ifndef DISCARD_TCN1 - Point& mid = edgeSegment[iEdgeSegmentSize / 2]; - float data[] = { float(left.x), float(left.y), 1.0f, float(mid.x), float(mid.y), - 1.0f, float(right.x), float(right.y), 1.0f }; - Mat threePoints(Size(3, 3), CV_32FC1, data); - double ans = determinant(threePoints); - - float dx = 1.0f*(left.x - right.x); - float dy = 1.0f*(left.y - right.y); - float edgelength2 = dx*dx + dy*dy; - // double TCNl = ans / edgelength2; - double TCNl = ans / (2 * pow(edgelength2, fT_TCN_P_)); - if (abs(TCNl) < fT_TCN_L_) { - countedges++; - continue; - } -#endif -#endif - - // Find convexity - See Sect [3.1.3] of the paper - int iCountBottom = 0; - int xx = left.x; - for (int k = 1; k < iEdgeSegmentSize; ++k) - { - if (edgeSegment[k].x == xx) continue; - - iCountBottom += (left.y - edgeSegment[k].y); - xx = edgeSegment[k].x; - } - - int width = abs(right.x - left.x) + 1; - int height = abs(right.y - left.y) + 1; - int iCountTop = (width *height) - iEdgeSegmentSize - iCountBottom; - - if (iCountBottom > iCountTop) - { - // 2 - points_2.push_back(edgeSegment); - } - else if (iCountBottom < iCountTop) - { - // 4 - points_4.push_back(edgeSegment); - } - } - -} - -float inline ed2(const cv::Point& A, const cv::Point& B) -{ - return float(((B.x - A.x)*(B.x - A.x) + (B.y - A.y)*(B.y - A.y))); -} - - -#define T124 pjf,pjm,pjl,pif,pim,pil // origin -#define T231 pil,pim,pif,pjf,pjm,pjl -#define T342 pif,pim,pil,pjf,pjm,pjl -#define T413 pif,pim,pil,pjl,pjm,pjf - - -// Verify triplets of arcs with convexity: i=1, j=2, k=4 -void EllipseDetector::Triplets124(VVP& pi, - VVP& pj, - VVP& pk, - unordered_map& data, - vector& ellipses -) -{ - // get arcs length - ushort sz_i = ushort(pi.size()); - ushort sz_j = ushort(pj.size()); - ushort sz_k = ushort(pk.size()); - - // For each edge i - for (ushort i = 0; i < sz_i; ++i) - { - VP& edge_i = pi[i]; - ushort sz_ei = ushort(edge_i.size()); - - Point& pif = edge_i[0]; - Point& pim = edge_i[sz_ei / 2]; - Point& pil = edge_i[sz_ei - 1]; - - // 1,2 -> reverse 1, swap - VP rev_i(edge_i.size()); - reverse_copy(edge_i.begin(), edge_i.end(), rev_i.begin()); - - // For each edge j - for (ushort j = 0; j < sz_j; ++j) - { - vector ellipses_i; - - VP& edge_j = pj[j]; - ushort sz_ej = ushort(edge_j.size()); - - Point& pjf = edge_j[0]; - Point& pjm = edge_j[sz_ej / 2]; - Point& pjl = edge_j[sz_ej - 1]; - -#ifndef DISCARD_CONSTRAINT_POSITION - - //if (sqrt((pjl.x - pif.x)*(pjl.x - pif.x) + (pjl.y - pif.y)*(pjl.y - pif.y)) > MAX(edge_i.size(), edge_j.size())) - // continue; - double tm1 = _tic(); - // CONSTRAINTS on position - if (pjl.x > pif.x + fThrArcPosition_) //is right - continue; - -#endif - - tm1 = _tic(); - if (_ed2(pil, pjf) / _ed2(pif, pjl) < fThre_r_) - continue; - -#ifdef CONSTRAINT_CNC_1 - tm1 = _tic(); - // cnc constraint1 2se se1//pil,pim,pif,pjf,pjm,pjl pjf,pjm,pjl,pif,pim,pil - if (fabs(_value4SixPoints(T124) - 1) > fT_CNC_) - continue; -#endif - - tm1 = _tic(); - EllipseData data_ij; - uint key_ij = GenerateKey(PAIR_12, i, j); - // If the data for the pair i-j have not been computed yet - if (data.count(key_ij) == 0) - { - // 1,2 -> reverse 1, swap - // Compute data! - GetFastCenter(edge_j, rev_i, data_ij); - // Insert computed data in the hash table - data.insert(pair(key_ij, data_ij)); - } - else - { - // Otherwise, just lookup the data in the hash table - data_ij = data.at(key_ij); - } - - // for each edge k - for (ushort k = 0; k < sz_k; ++k) - { - VP& edge_k = pk[k]; - ushort sz_ek = ushort(edge_k.size()); - - Point& pkf = edge_k[0]; - Point& pkm = edge_k[sz_ek / 2]; - Point& pkl = edge_k[sz_ek - 1]; - -#ifndef DISCARD_CONSTRAINT_POSITION - // CONSTRAINTS on position - if (pkl.y < pil.y - fThrArcPosition_) - continue; -#endif -#ifdef CONSTRAINT_CNC_2 - // cnc constraint2 - if (fabs(_value4SixPoints(pif, pim, pil, pkf, pkm, pkl) - 1) > fT_CNC_) - continue; -#endif -#ifdef CONSTRAINT_CNC_3 - // cnc constraint3 - if (fabs(_value4SixPoints(pjf, pjm, pjl, pkf, pkm, pkl) - 1) > fT_CNC_) - continue; -#endif - - uint key_ik = GenerateKey(PAIR_14, i, k); - - // Find centers - EllipseData data_ik; - - // If the data for the pair i-k have not been computed yet - if (data.count(key_ik) == 0) - { - // 1,4 -> ok - // Compute data! - GetFastCenter(edge_i, edge_k, data_ik); - // Insert computed data in the hash table - data.insert(pair(key_ik, data_ik)); - } - else - { - // Otherwise, just lookup the data in the hash table - data_ik = data.at(key_ik); - } - - // INVALID CENTERS - if (!data_ij.isValid || !data_ik.isValid) - { - continue; - } - -#ifndef DISCARD_CONSTRAINT_CENTER - // Selection strategy - Step 3. See Sect [3.2.2] in the paper - // The computed centers are not close enough - if (ed2(data_ij.Cab, data_ik.Cab) > fMaxCenterDistance2_) - { - // discard - continue; - } -#endif - - // If all constraints of the selection strategy have been satisfied, - // we can start estimating the ellipse parameters - - // Find ellipse parameters - // Get the coordinates of the center (xc, yc) - Point2f center = GetCenterCoordinates(data_ij, data_ik); - - Ellipse ell; - // Find remaining paramters (A,B,rho) - FindEllipses(center, edge_i, edge_j, edge_k, data_ij, data_ik, ell); - ellipses_i.push_back(ell); - } - /*-----------------------------------------------------------------*/ - int rt = -1; - float rs = 0; - for (int t = 0; t < (int)ellipses_i.size(); t++) - { - if (ellipses_i[t].score_ > rs) - { - rs = ellipses_i[t].score_; rt = t; - } - } - if (rt > -1) - { - ellipses.push_back(ellipses_i[rt]); - } - /*-----------------------------------------------------------------*/ - } - } -} - -void EllipseDetector::Triplets231(VVP& pi, - VVP& pj, - VVP& pk, - unordered_map& data, - vector& ellipses -) -{ - ushort sz_i = ushort(pi.size()); - ushort sz_j = ushort(pj.size()); - ushort sz_k = ushort(pk.size()); - - // For each edge i - for (ushort i = 0; i < sz_i; ++i) - { - VP& edge_i = pi[i]; - ushort sz_ei = ushort(edge_i.size()); - - Point& pif = edge_i[0]; - Point& pim = edge_i[sz_ei / 2]; - Point& pil = edge_i[sz_ei - 1]; - - VP rev_i(edge_i.size()); - reverse_copy(edge_i.begin(), edge_i.end(), rev_i.begin()); - - // For each edge j - for (ushort j = 0; j < sz_j; ++j) - { - vector ellipses_i; - - VP& edge_j = pj[j]; - ushort sz_ej = ushort(edge_j.size()); - - Point& pjf = edge_j[0]; - Point& pjm = edge_j[sz_ej / 2]; - Point& pjl = edge_j[sz_ej - 1]; - -#ifndef DISCARD_CONSTRAINT_POSITION - //if (sqrt((pjf.x - pif.x)*(pjf.x - pif.x) + (pjf.y - pif.y)*(pjf.y - pif.y)) > MAX(edge_i.size(), edge_j.size())) - // continue; - - double tm1 = _tic(); - // CONSTRAINTS on position - if (pjf.y < pif.y - fThrArcPosition_) - continue; - -#endif - - tm1 = _tic(); - if (_ed2(pif, pjf) / _ed2(pil, pjl) < fThre_r_) - continue; - -#ifdef CONSTRAINT_CNC_1 - tm1 = _tic(); - // cnc constraint1 2es se3 // pif,pim,pil,pjf,pjm,pjl pil,pim,pif,pjf,pjm,pjl - if (fabs(_value4SixPoints(T231) - 1) > fT_CNC_) - continue; -#endif - - tm1 = _tic(); - VP rev_j(edge_j.size()); - reverse_copy(edge_j.begin(), edge_j.end(), rev_j.begin()); - - EllipseData data_ij; - uint key_ij = GenerateKey(PAIR_23, i, j); - if (data.count(key_ij) == 0) - { - // 2,3 -> reverse 2,3 - GetFastCenter(rev_i, rev_j, data_ij); - data.insert(pair(key_ij, data_ij)); - } - else - { - data_ij = data.at(key_ij); - } - - // For each edge k - for (ushort k = 0; k < sz_k; ++k) - { - VP& edge_k = pk[k]; - - ushort sz_ek = ushort(edge_k.size()); - - Point& pkf = edge_k[0]; - Point& pkm = edge_k[sz_ek / 2]; - Point& pkl = edge_k[sz_ek - 1]; - -#ifndef DISCARD_CONSTRAINT_POSITION - // CONSTRAINTS on position - if (pkf.x < pil.x - fThrArcPosition_) - continue; -#endif - -#ifdef CONSTRAINT_CNC_2 - // cnc constraint2 - if (fabs(_value4SixPoints(pif, pim, pil, pkf, pkm, pkl) - 1) > fT_CNC_) - continue; -#endif -#ifdef CONSTRAINT_CNC_3 - // cnc constraint3 - if (fabs(_value4SixPoints(pjf, pjm, pjl, pkf, pkm, pkl) - 1) > fT_CNC_) - continue; -#endif - uint key_ik = GenerateKey(PAIR_12, k, i); - - // Find centers - - EllipseData data_ik; - - - if (data.count(key_ik) == 0) - { - // 2,1 -> reverse 1 - VP rev_k(edge_k.size()); - reverse_copy(edge_k.begin(), edge_k.end(), rev_k.begin()); - - GetFastCenter(edge_i, rev_k, data_ik); - data.insert(pair(key_ik, data_ik)); - } - else - { - data_ik = data.at(key_ik); - } - - // INVALID CENTERS - if (!data_ij.isValid || !data_ik.isValid) - { - continue; - } - -#ifndef DISCARD_CONSTRAINT_CENTER - // CONSTRAINT ON CENTERS - if (ed2(data_ij.Cab, data_ik.Cab) > fMaxCenterDistance2_) - { - // discard - continue; - } -#endif - // Find ellipse parameters - Point2f center = GetCenterCoordinates(data_ij, data_ik); - - Ellipse ell; - FindEllipses(center, edge_i, edge_j, edge_k, data_ij, data_ik, ell); - ellipses_i.push_back(ell); - } - /*-----------------------------------------------------------------*/ - int rt = -1; - float rs = 0; - for (int t = 0; t < (int)ellipses_i.size(); t++) - { - if (ellipses_i[t].score_ > rs) - { - rs = ellipses_i[t].score_; rt = t; - } - } - if (rt > -1) - { - ellipses.push_back(ellipses_i[rt]); - } - /*-----------------------------------------------------------------*/ - } - } -} - -void EllipseDetector::Triplets342(VVP& pi, - VVP& pj, - VVP& pk, - unordered_map& data, - vector& ellipses -) -{ - ushort sz_i = ushort(pi.size()); - ushort sz_j = ushort(pj.size()); - ushort sz_k = ushort(pk.size()); - - // For each edge i - for (ushort i = 0; i < sz_i; ++i) - { - VP& edge_i = pi[i]; - ushort sz_ei = ushort(edge_i.size()); - - Point& pif = edge_i[0]; - Point& pim = edge_i[sz_ei / 2]; - Point& pil = edge_i[sz_ei - 1]; - - VP rev_i(edge_i.size()); - reverse_copy(edge_i.begin(), edge_i.end(), rev_i.begin()); - - // For each edge j - for (ushort j = 0; j < sz_j; ++j) - { - vector ellipses_i; - - VP& edge_j = pj[j]; - ushort sz_ej = ushort(edge_j.size()); - - Point& pjf = edge_j[0]; - Point& pjm = edge_j[sz_ej / 2]; - Point& pjl = edge_j[sz_ej - 1]; - -#ifndef DISCARD_CONSTRAINT_POSITION - //if (sqrt((pjf.x - pil.x)*(pjf.x - pil.x) + (pjf.y - pil.y)*(pjf.y - pil.y)) > MAX(edge_i.size(), edge_j.size())) - // continue; - - double tm1 = _tic(); - // CONSTRAINTS on position - if (pjf.x < pil.x - fThrArcPosition_) // is left - continue; - -#endif - - tm1 = _tic(); - if (_ed2(pil, pjf) / _ed2(pif, pjl) < fThre_r_) - continue; - -#ifdef CONSTRAINT_CNC_1 - tm1 = _tic(); - // cnc constraint1 3se se4 // pil,pim,pif,pjf,pjm,pjl pif,pim,pil,pjf,pjm,pjl - if (fabs(_value4SixPoints(T342) - 1) > fT_CNC_) - continue; - -#endif - - tm1 = _tic(); - VP rev_j(edge_j.size()); - reverse_copy(edge_j.begin(), edge_j.end(), rev_j.begin()); - - EllipseData data_ij; - uint key_ij = GenerateKey(PAIR_34, i, j); - - if (data.count(key_ij) == 0) - { - // 3,4 -> reverse 4 - - GetFastCenter(edge_i, rev_j, data_ij); - data.insert(pair(key_ij, data_ij)); - } - else - { - data_ij = data.at(key_ij); - } - - - // For each edge k - for (ushort k = 0; k < sz_k; ++k) - { - VP& edge_k = pk[k]; - ushort sz_ek = ushort(edge_k.size()); - - Point& pkf = edge_k[0]; - Point& pkm = edge_k[sz_ek / 2]; - Point& pkl = edge_k[sz_ek - 1]; - -#ifndef DISCARD_CONSTRAINT_POSITION - // CONSTRAINTS on position - if (pkf.y > pif.y + fThrArcPosition_) - continue; -#endif - -#ifdef CONSTRAINT_CNC_2 - // cnc constraint2 - if (fabs(_value4SixPoints(pif, pim, pil, pkf, pkm, pkl) - 1) > fT_CNC_) - continue; -#endif -#ifdef CONSTRAINT_CNC_3 - // cnc constraint3 - if (fabs(_value4SixPoints(pjf, pjm, pjl, pkf, pkm, pkl) - 1) > fT_CNC_) - continue; -#endif - uint key_ik = GenerateKey(PAIR_23, k, i); - - // Find centers - - EllipseData data_ik; - - - - if (data.count(key_ik) == 0) - { - // 3,2 -> reverse 3,2 - - VP rev_k(edge_k.size()); - reverse_copy(edge_k.begin(), edge_k.end(), rev_k.begin()); - - GetFastCenter(rev_i, rev_k, data_ik); - - data.insert(pair(key_ik, data_ik)); - } - else - { - data_ik = data.at(key_ik); - } - - - // INVALID CENTERS - if (!data_ij.isValid || !data_ik.isValid) - { - continue; - } - -#ifndef DISCARD_CONSTRAINT_CENTER - // CONSTRAINT ON CENTERS - if (ed2(data_ij.Cab, data_ik.Cab) > fMaxCenterDistance2_) - { - // discard - continue; - } -#endif - // Find ellipse parameters - Point2f center = GetCenterCoordinates(data_ij, data_ik); - - Ellipse ell; - FindEllipses(center, edge_i, edge_j, edge_k, data_ij, data_ik, ell); - ellipses_i.push_back(ell); - } - /*-----------------------------------------------------------------*/ - int rt = -1; - float rs = 0; - for (int t = 0; t < (int)ellipses_i.size(); t++) - { - if (ellipses_i[t].score_ > rs) - { - rs = ellipses_i[t].score_; rt = t; - } - } - if (rt > -1) - { - ellipses.push_back(ellipses_i[rt]); - } - /*-----------------------------------------------------------------*/ - } - } -} - -void EllipseDetector::Triplets413(VVP& pi, - VVP& pj, - VVP& pk, - unordered_map& data, - vector& ellipses -) -{ - ushort sz_i = ushort(pi.size()); - ushort sz_j = ushort(pj.size()); - ushort sz_k = ushort(pk.size()); - - // For each edge i - for (ushort i = 0; i < sz_i; ++i) - { - VP& edge_i = pi[i]; - ushort sz_ei = ushort(edge_i.size()); - - Point& pif = edge_i[0]; - Point& pim = edge_i[sz_ei / 2]; - Point& pil = edge_i[sz_ei - 1]; - - VP rev_i(edge_i.size()); - reverse_copy(edge_i.begin(), edge_i.end(), rev_i.begin()); - - // For each edge j - for (ushort j = 0; j < sz_j; ++j) - { - vector ellipses_i; - - VP& edge_j = pj[j]; - ushort sz_ej = ushort(edge_j.size()); - - Point& pjf = edge_j[0]; - Point& pjm = edge_j[sz_ej / 2]; - Point& pjl = edge_j[sz_ej - 1]; - -#ifndef DISCARD_CONSTRAINT_POSITION - //if (sqrt((pjl.x - pil.x)*(pjl.x - pil.x) + (pjl.y - pil.y)*(pjl.y - pil.y)) > MAX(edge_i.size(), edge_j.size())) - // continue; - - double tm1 = _tic(); - // CONSTRAINTS on position - if (pjl.y > pil.y + fThrArcPosition_) // is below - continue; - -#endif - - tm1 = _tic(); - if (_ed2(pif, pjf) / _ed2(pil, pjl) < fThre_r_) - continue; - -#ifdef CONSTRAINT_CNC_1 - tm1 = _tic(); - // cnc constraint1 4se es1 // pif,pim,pil,pjf,pjm,pjl pil,pim,pif,pjl,pjm,pjf pif,pim,pil,pjl,pjm,pjf - if (fabs(_value4SixPoints(T413) - 1) > fT_CNC_) - continue; -#endif - - tm1 = _tic(); - EllipseData data_ij; - uint key_ij = GenerateKey(PAIR_14, j, i); - - if (data.count(key_ij) == 0) - { - // 4,1 -> OK - GetFastCenter(edge_i, edge_j, data_ij); - data.insert(pair(key_ij, data_ij)); - } - else - { - data_ij = data.at(key_ij); - } - - // For each edge k - for (ushort k = 0; k < sz_k; ++k) - { - VP& edge_k = pk[k]; - ushort sz_ek = ushort(edge_k.size()); - - Point& pkf = edge_k[0]; - Point& pkm = edge_k[sz_ek / 2]; - Point& pkl = edge_k[sz_ek - 1]; - -#ifndef DISCARD_CONSTRAINT_POSITION - // CONSTRAINTS on position - if (pkl.x > pif.x + fThrArcPosition_) - continue; -#endif - -#ifdef CONSTRAINT_CNC_2 - // cnc constraint2 - if (fabs(_value4SixPoints(pif, pim, pil, pkf, pkm, pkl) - 1) > fT_CNC_) - continue; -#endif -#ifdef CONSTRAINT_CNC_3 - // cnc constraint2 - if (fabs(_value4SixPoints(pjf, pjm, pjl, pkf, pkm, pkl) - 1) > fT_CNC_) - continue; -#endif - uint key_ik = GenerateKey(PAIR_34, k, i); - - // Find centers - - EllipseData data_ik; - - - - if (data.count(key_ik) == 0) - { - // 4,3 -> reverse 4 - GetFastCenter(rev_i, edge_k, data_ik); - data.insert(pair(key_ik, data_ik)); - } - else - { - data_ik = data.at(key_ik); - } - - // INVALID CENTERS - if (!data_ij.isValid || !data_ik.isValid) - { - continue; - } - -#ifndef DISCARD_CONSTRAINT_CENTER - // CONSTRAIN ON CENTERS - if (ed2(data_ij.Cab, data_ik.Cab) > fMaxCenterDistance2_) - { - // discard - continue; - } -#endif - // Find ellipse parameters - Point2f center = GetCenterCoordinates(data_ij, data_ik); - - Ellipse ell; - FindEllipses(center, edge_i, edge_j, edge_k, data_ij, data_ik, ell); - ellipses_i.push_back(ell); - } - /*-----------------------------------------------------------------*/ - int rt = -1; - float rs = 0; - for (int t = 0; t < (int)ellipses_i.size(); t++) - { - if (ellipses_i[t].score_ > rs) - { - rs = ellipses_i[t].score_; rt = t; - } - } - if (rt > -1) - { - ellipses.push_back(ellipses_i[rt]); - } - /*-----------------------------------------------------------------*/ - } - } -} - -int EllipseDetector::FindMaxK(const int* v) const -{ - int max_val = 0; - int max_idx = 0; - for (int i = 0; i max_val) ? max_val = v[i], max_idx = i : (int)NULL; - } - - return max_idx + 90; -} - -int EllipseDetector::FindMaxN(const int* v) const -{ - int max_val = 0; - int max_idx = 0; - for (int i = 0; i max_val) ? max_val = v[i], max_idx = i : (int)NULL; - } - - return max_idx; -} - -int EllipseDetector::FindMaxA(const int* v) const -{ - int max_val = 0; - int max_idx = 0; - for (int i = 0; i max_val) ? max_val = v[i], max_idx = i : (int)NULL; - } - - return max_idx; -} - -// Most important function for detecting ellipses. See Sect[3.2.3] of the paper -void EllipseDetector::FindEllipses(Point2f& center, - VP& edge_i, VP& edge_j, VP& edge_k, - EllipseData& data_ij, EllipseData& data_ik, Ellipse& ell) -{ - countOfFindEllipse_++; - // Find ellipse parameters - - // 0-initialize accumulators - memset(accN, 0, sizeof(int)*ACC_N_SIZE); - memset(accR, 0, sizeof(int)*ACC_R_SIZE); - memset(accA, 0, sizeof(int)*ACC_A_SIZE); - - // estimation - // Get size of the 4 vectors of slopes (2 pairs of arcs) - int sz_ij1 = int(data_ij.Sa.size()); - int sz_ij2 = int(data_ij.Sb.size()); - int sz_ik1 = int(data_ik.Sa.size()); - int sz_ik2 = int(data_ik.Sb.size()); - - // Get the size of the 3 arcs - size_t sz_ei = edge_i.size(); - size_t sz_ej = edge_j.size(); - size_t sz_ek = edge_k.size(); - - // Center of the estimated ellipse - float a0 = center.x; - float b0 = center.y; - - - // Estimation of remaining parameters - // Uses 4 combinations of parameters. See Table 1 and Sect [3.2.3] of the paper. - // ij1 and ik - { - float q1 = data_ij.ra; - float q3 = data_ik.ra; - float q5 = data_ik.rb; - - for (int ij1 = 0; ij1 < sz_ij1; ++ij1) - { - float q2 = data_ij.Sa[ij1]; // need iter \A3\BF - - float q1xq2 = q1*q2; - // ij1 and ik1 - for (int ik1 = 0; ik1 < sz_ik1; ++ik1) - { - float q4 = data_ik.Sa[ik1]; // need iter \A3\BF - - float q3xq4 = q3*q4; - - // See Eq. [13-18] in the paper - - float a = (q1xq2 - q3xq4); // gama - float b = (q3xq4 + 1)*(q1 + q2) - (q1xq2 + 1)*(q3 + q4); // beta - float Kp = (-b + sqrt(b*b + 4 * a*a)) / (2 * a); // K+ - float zplus = ((q1 - Kp)*(q2 - Kp)) / ((1 + q1*Kp)*(1 + q2*Kp)); - // check zplus and K is linear - if (zplus >= 0.0f) continue; - - float Np = sqrt(-zplus); // N+ - float rho = atan(Kp); // rho tmp - int rhoDeg; - if (Np > 1.f) - { - Np = 1.f / Np; - rhoDeg = cvRound((rho * 180 / CV_PI) + 180) % 180; // [0,180) - } - else - { - rhoDeg = cvRound((rho * 180 / CV_PI) + 90) % 180; // [0,180) rho angel rep and norm - } - - int iNp = cvRound(Np * 100); // [0, 100] - - if (0 <= iNp && iNp < ACC_N_SIZE && - 0 <= rhoDeg && rhoDeg < ACC_R_SIZE - ) - { // why iter all. beacause zplus and K is not linear? - ++accN[iNp]; // Increment N accumulator - ++accR[rhoDeg]; // Increment R accumulator - } - } - // ij1 and ik2 - for (int ik2 = 0; ik2 < sz_ik2; ++ik2) - { - float q4 = data_ik.Sb[ik2]; - - float q5xq4 = q5*q4; - - // See Eq. [13-18] in the paper - - float a = (q1xq2 - q5xq4); - float b = (q5xq4 + 1)*(q1 + q2) - (q1xq2 + 1)*(q5 + q4); - float Kp = (-b + sqrt(b*b + 4 * a*a)) / (2 * a); - float zplus = ((q1 - Kp)*(q2 - Kp)) / ((1 + q1*Kp)*(1 + q2*Kp)); - - if (zplus >= 0.0f) - { - continue; - } - - float Np = sqrt(-zplus); - float rho = atan(Kp); - int rhoDeg; - if (Np > 1.f) - { - Np = 1.f / Np; - rhoDeg = cvRound((rho * 180 / CV_PI) + 180) % 180; // [0,180) - } - else - { - rhoDeg = cvRound((rho * 180 / CV_PI) + 90) % 180; // [0,180) - } - - int iNp = cvRound(Np * 100); // [0, 100] - - if (0 <= iNp && iNp < ACC_N_SIZE && - 0 <= rhoDeg && rhoDeg < ACC_R_SIZE - ) - { - ++accN[iNp]; // Increment N accumulator - ++accR[rhoDeg]; // Increment R accumulator - } - } - - } - } - - // ij2 and ik - { - float q1 = data_ij.rb; - float q3 = data_ik.rb; - float q5 = data_ik.ra; - - for (int ij2 = 0; ij2 < sz_ij2; ++ij2) - { - float q2 = data_ij.Sb[ij2]; - - float q1xq2 = q1*q2; - // ij2 and ik2 - for (int ik2 = 0; ik2 < sz_ik2; ++ik2) - { - float q4 = data_ik.Sb[ik2]; - - float q3xq4 = q3*q4; - - // See Eq. [13-18] in the paper - - float a = (q1xq2 - q3xq4); - float b = (q3xq4 + 1)*(q1 + q2) - (q1xq2 + 1)*(q3 + q4); - float Kp = (-b + sqrt(b*b + 4 * a*a)) / (2 * a); - float zplus = ((q1 - Kp)*(q2 - Kp)) / ((1 + q1*Kp)*(1 + q2*Kp)); - - if (zplus >= 0.0f) - { - continue; - } - - float Np = sqrt(-zplus); - float rho = atan(Kp); - int rhoDeg; - if (Np > 1.f) - { - Np = 1.f / Np; - rhoDeg = cvRound((rho * 180 / CV_PI) + 180) % 180; // [0,180) - } - else - { - rhoDeg = cvRound((rho * 180 / CV_PI) + 90) % 180; // [0,180) - } - - int iNp = cvRound(Np * 100); // [0, 100] - - if (0 <= iNp && iNp < ACC_N_SIZE && - 0 <= rhoDeg && rhoDeg < ACC_R_SIZE - ) - { - ++accN[iNp]; // Increment N accumulator - ++accR[rhoDeg]; // Increment R accumulator - } - } - - // ij2 and ik1 - for (int ik1 = 0; ik1 < sz_ik1; ++ik1) - { - float q4 = data_ik.Sa[ik1]; - - float q5xq4 = q5*q4; - - // See Eq. [13-18] in the paper - - float a = (q1xq2 - q5xq4); - float b = (q5xq4 + 1)*(q1 + q2) - (q1xq2 + 1)*(q5 + q4); - float Kp = (-b + sqrt(b*b + 4 * a*a)) / (2 * a); - float zplus = ((q1 - Kp)*(q2 - Kp)) / ((1 + q1*Kp)*(1 + q2*Kp)); - - if (zplus >= 0.0f) - { - continue; - } - - float Np = sqrt(-zplus); - float rho = atan(Kp); - int rhoDeg; - if (Np > 1.f) - { - Np = 1.f / Np; - rhoDeg = cvRound((rho * 180 / CV_PI) + 180) % 180; // [0,180) - } - else - { - rhoDeg = cvRound((rho * 180 / CV_PI) + 90) % 180; // [0,180) - } - - int iNp = cvRound(Np * 100); // [0, 100] - - if (0 <= iNp && iNp < ACC_N_SIZE && - 0 <= rhoDeg && rhoDeg < ACC_R_SIZE - ) - { - ++accN[iNp]; // Increment N accumulator - ++accR[rhoDeg]; // Increment R accumulator - } - } - - } - } - - // Find peak in N and K accumulator - int iN = FindMaxN(accN); - int iK = FindMaxK(accR); - - // Recover real values - float fK = float(iK); - float Np = float(iN) * 0.01f; - float rho = fK * float(CV_PI) / 180.f; // deg 2 rad - float Kp = tan(rho); - - // Estimate A. See Eq. [19 - 22] in Sect [3.2.3] of the paper - // - // may optm - for (ushort l = 0; l < sz_ei; ++l) - { - Point& pp = edge_i[l]; - float sk = 1.f / sqrt(Kp*Kp + 1.f); // cos rho - float x0 = ((pp.x - a0) * sk) + (((pp.y - b0)*Kp) * sk); // may optm - float y0 = -(((pp.x - a0) * Kp) * sk) + ((pp.y - b0) * sk); // may optm - float Ax = sqrt((x0*x0*Np*Np + y0*y0) / ((Np*Np)*(1.f + Kp*Kp))); - int A = cvRound(abs(Ax / cos(rho))); // may optm - if ((0 <= A) && (A < ACC_A_SIZE)) - { - ++accA[A]; - } - } - - for (ushort l = 0; l < sz_ej; ++l) - { - Point& pp = edge_j[l]; - float sk = 1.f / sqrt(Kp*Kp + 1.f); - float x0 = ((pp.x - a0) * sk) + (((pp.y - b0)*Kp) * sk); - float y0 = -(((pp.x - a0) * Kp) * sk) + ((pp.y - b0) * sk); - float Ax = sqrt((x0*x0*Np*Np + y0*y0) / ((Np*Np)*(1.f + Kp*Kp))); - int A = cvRound(abs(Ax / cos(rho))); - if ((0 <= A) && (A < ACC_A_SIZE)) - { - ++accA[A]; - } - } - - for (ushort l = 0; l < sz_ek; ++l) - { - Point& pp = edge_k[l]; - float sk = 1.f / sqrt(Kp*Kp + 1.f); - float x0 = ((pp.x - a0) * sk) + (((pp.y - b0)*Kp) * sk); - float y0 = -(((pp.x - a0) * Kp) * sk) + ((pp.y - b0) * sk); - float Ax = sqrt((x0*x0*Np*Np + y0*y0) / ((Np*Np)*(1.f + Kp*Kp))); - int A = cvRound(abs(Ax / cos(rho))); - if ((0 <= A) && (A < ACC_A_SIZE)) - { - ++accA[A]; - } - } - - // Find peak in A accumulator - int A = FindMaxA(accA); - float fA = float(A); - - // Find B value. See Eq [23] in the paper - float fB = abs(fA * Np); - - // Got all ellipse parameters! - // Ellipse ell(a0, b0, fA, fB, fmod(rho + float(CV_PI)*2.f, float(CV_PI))); - ell.xc_ = a0; - ell.yc_ = b0; - ell.a_ = fA; - ell.b_ = fB; - ell.rad_ = fmod(rho + float(CV_PI)*2.f, float(CV_PI)); - - // estimation end - // validation start - // Get the score. See Sect [3.3.1] in the paper - - // Find the number of edge pixel lying on the ellipse - float _cos = cos(-ell.rad_); - float _sin = sin(-ell.rad_); - - float invA2 = 1.f / (ell.a_ * ell.a_); - float invB2 = 1.f / (ell.b_ * ell.b_); - - float invNofPoints = 1.f / float(sz_ei + sz_ej + sz_ek); - int counter_on_perimeter = 0; - float probc_on_perimeter = 0; - - for (ushort l = 0; l < sz_ei; ++l) - { - float tx = float(edge_i[l].x) - ell.xc_; - float ty = float(edge_i[l].y) - ell.yc_; - float rx = (tx*_cos - ty*_sin); - float ry = (tx*_sin + ty*_cos); - - float h = (rx*rx)*invA2 + (ry*ry)*invB2; - if (abs(h - 1.f) < fDistanceToEllipseContour_) - { - ++counter_on_perimeter; - probc_on_perimeter += 1; // (fDistanceToEllipseContour_ - abs(h - 1.f)) / fDistanceToEllipseContour_; - } - } - - for (ushort l = 0; l < sz_ej; ++l) - { - float tx = float(edge_j[l].x) - ell.xc_; - float ty = float(edge_j[l].y) - ell.yc_; - float rx = (tx*_cos - ty*_sin); - float ry = (tx*_sin + ty*_cos); - - float h = (rx*rx)*invA2 + (ry*ry)*invB2; - if (abs(h - 1.f) < fDistanceToEllipseContour_) - { - ++counter_on_perimeter; - probc_on_perimeter += 1; // (fDistanceToEllipseContour_ - abs(h - 1.f)) / fDistanceToEllipseContour_; - } - } - - for (ushort l = 0; l < sz_ek; ++l) - { - float tx = float(edge_k[l].x) - ell.xc_; - float ty = float(edge_k[l].y) - ell.yc_; - float rx = (tx*_cos - ty*_sin); - float ry = (tx*_sin + ty*_cos); - - float h = (rx*rx)*invA2 + (ry*ry)*invB2; - if (abs(h - 1.f) < fDistanceToEllipseContour_) - { - ++counter_on_perimeter; - probc_on_perimeter += 1; // (fDistanceToEllipseContour_ - abs(h - 1.f)) / fDistanceToEllipseContour_; - } - } - - - // no points found on the ellipse - if (counter_on_perimeter <= 0) - { - // validation - return; - } - - // Compute score - // float score = float(counter_on_perimeter) * invNofPoints; - float score = probc_on_perimeter * invNofPoints; - if (score < fMinScore_) - { - // validation - return; - } - - // Compute reliability - // this metric is not described in the paper, mostly due to space limitations. - // The main idea is that for a given ellipse (TD) even if the score is high, the arcs - // can cover only a small amount of the contour of the estimated ellipse. - // A low reliability indicate that the arcs form an elliptic shape by chance, but do not underlie - // an actual ellipse. The value is normalized between 0 and 1. - // The default value is 0.4. - - // It is somehow similar to the "Angular Circumreference Ratio" saliency criteria - // as in the paper: - // D. K. Prasad, M. K. Leung, S.-Y. Cho, Edge curvature and convexity - // based ellipse detection method, Pattern Recognition 45 (2012) 3204-3221. - - float di, dj, dk; - { - Point2f p1(float(edge_i[0].x), float(edge_i[0].y)); - Point2f p2(float(edge_i[sz_ei - 1].x), float(edge_i[sz_ei - 1].y)); - p1.x -= ell.xc_; - p1.y -= ell.yc_; - p2.x -= ell.xc_; - p2.y -= ell.yc_; - Point2f r1((p1.x*_cos - p1.y*_sin), (p1.x*_sin + p1.y*_cos)); - Point2f r2((p2.x*_cos - p2.y*_sin), (p2.x*_sin + p2.y*_cos)); - di = abs(r2.x - r1.x) + abs(r2.y - r1.y); - } - { - Point2f p1(float(edge_j[0].x), float(edge_j[0].y)); - Point2f p2(float(edge_j[sz_ej - 1].x), float(edge_j[sz_ej - 1].y)); - p1.x -= ell.xc_; - p1.y -= ell.yc_; - p2.x -= ell.xc_; - p2.y -= ell.yc_; - Point2f r1((p1.x*_cos - p1.y*_sin), (p1.x*_sin + p1.y*_cos)); - Point2f r2((p2.x*_cos - p2.y*_sin), (p2.x*_sin + p2.y*_cos)); - dj = abs(r2.x - r1.x) + abs(r2.y - r1.y); - } - { - Point2f p1(float(edge_k[0].x), float(edge_k[0].y)); - Point2f p2(float(edge_k[sz_ek - 1].x), float(edge_k[sz_ek - 1].y)); - p1.x -= ell.xc_; - p1.y -= ell.yc_; - p2.x -= ell.xc_; - p2.y -= ell.yc_; - Point2f r1((p1.x*_cos - p1.y*_sin), (p1.x*_sin + p1.y*_cos)); - Point2f r2((p2.x*_cos - p2.y*_sin), (p2.x*_sin + p2.y*_cos)); - dk = abs(r2.x - r1.x) + abs(r2.y - r1.y); - } - - // This allows to get rid of thick edges - float rel = min(1.f, ((di + dj + dk) / (3 * (ell.a_ + ell.b_)))); - - if (rel < fMinReliability_) - { - // validation - return; - } - if (_isnan(rel)) - return; - - // Assign the new score! - ell.score_ = (score + rel) * 0.5f; // need to change - // ell.score_ = (score*rel); // need to change - - if (ell.score_ < fMinScore_) - { - return; - } - // The tentative detection has been confirmed. Save it! - // ellipses.push_back(ell); - - // Validation end -} - - - -// Ellipse clustering procedure. See Sect [3.3.2] in the paper. -void EllipseDetector::ClusterEllipses(vector& ellipses) -{ - float th_Da = 0.1f; - float th_Db = 0.1f; - float th_Dr = 0.1f; - - float th_Dc_ratio = 0.1f; - float th_Dr_circle = 0.9f; - - int iNumOfEllipses = int(ellipses.size()); - if (iNumOfEllipses == 0) return; - - // The first ellipse is assigned to a cluster - vector clusters; - clusters.push_back(ellipses[0]); - - // bool bFoundCluster = false; - - for (int i = 1; i th_Dc) - { - //not same cluster - continue; - } - - // a - float Da = abs(e1.a_ - e2.a_) / max(e1.a_, e2.a_); - if (Da > th_Da) - { - //not same cluster - continue; - } - - // b - float Db = abs(e1.b_ - e2.b_) / min(e1.b_, e2.b_); - if (Db > th_Db) - { - //not same cluster - continue; - } - - // angle - float Dr = GetMinAnglePI(e1.rad_, e2.rad_) / float(CV_PI); - if ((Dr > th_Dr) && (ba_e1 < th_Dr_circle) && (ba_e2 < th_Dr_circle)) - { - //not same cluster - continue; - } - - // Same cluster as e2 - bFoundCluster = true;// - // Discard, no need to create a new cluster - break; - } - - if (!bFoundCluster) - { - // Create a new cluster - clusters.push_back(e1); - } - } - - clusters.swap(ellipses); -} - -float EllipseDetector::GetMedianSlope(vector& med, Point2f& M, vector& slopes) -{ - // input med slopes, output M, return slope - // med : vector of points - // M : centroid of the points in med - // slopes : vector of the slopes - - unsigned iNofPoints = unsigned(med.size()); - // CV_Assert(iNofPoints >= 2); - - unsigned halfSize = iNofPoints >> 1; - unsigned quarterSize = halfSize >> 1; - - vector xx, yy; - slopes.reserve(halfSize); - xx.reserve(iNofPoints); - yy.reserve(iNofPoints); - - for (unsigned i = 0; i < halfSize; ++i) - { - Point2f& p1 = med[i]; - Point2f& p2 = med[halfSize + i]; - - xx.push_back(p1.x); - xx.push_back(p2.x); - yy.push_back(p1.y); - yy.push_back(p2.y); - - float den = (p2.x - p1.x); - float num = (p2.y - p1.y); - - if (den == 0) den = 0.00001f; - - slopes.push_back(num / den); - } - - nth_element(slopes.begin(), slopes.begin() + quarterSize, slopes.end()); - nth_element(xx.begin(), xx.begin() + halfSize, xx.end()); - nth_element(yy.begin(), yy.begin() + halfSize, yy.end()); - M.x = xx[halfSize]; - M.y = yy[halfSize]; - - return slopes[quarterSize]; -} - -int inline sgn(float val) { - return (0.f < val) - (val < 0.f); -} - -void EllipseDetector::GetFastCenter(vector& e1, vector& e2, EllipseData& data) -{ - countOfGetFastCenter_++; - data.isValid = true; - - unsigned size_1 = unsigned(e1.size()); - unsigned size_2 = unsigned(e2.size()); - - unsigned hsize_1 = size_1 >> 1; - unsigned hsize_2 = size_2 >> 1; - - Point& med1 = e1[hsize_1]; - Point& med2 = e2[hsize_2]; - - Point2f M12, M34; - float q2, q4; - - {// First to second Reference slope - float dx_ref = float(e1[0].x - med2.x); - float dy_ref = float(e1[0].y - med2.y); - - if (dy_ref == 0) dy_ref = 0.00001f; - - float m_ref = dy_ref / dx_ref; - data.ra = m_ref; - - // Find points with same slope as reference - vector med; - med.reserve(hsize_2); - - unsigned minPoints = (uNs_ < hsize_2) ? uNs_ : hsize_2; // parallel chords - - vector indexes(minPoints); - if (uNs_ < hsize_2) - { // hsize_2 bigger than uNs_ - unsigned iSzBin = hsize_2 / unsigned(uNs_); - unsigned iIdx = hsize_2 + (iSzBin / 2); - - for (unsigned i = 0; i> 1; - while (end - begin > 2) - { - float x2 = float(e1[j].x); - float y2 = float(e1[j].y); - float res = ((x2 - x1) * dy_ref) - ((y2 - y1) * dx_ref); - int sign_res = sgn(res); - - if (sign_res == 0) - { - // found - med.push_back(Point2f((x2 + x1)* 0.5f, (y2 + y1)* 0.5f)); - break; - } - - if (sign_res + sign_begin == 0) - { - sign_end = sign_res; - end = j; - } - else - { - sign_begin = sign_res; - begin = j; - } - j = (begin + end) >> 1; - } - // search end error ? - med.push_back(Point2f((e1[j].x + x1)* 0.5f, (e1[j].y + y1)* 0.5f)); - } - - if (med.size() < 2) - { - data.isValid = false; - return; - } - - /**************************************** - Mat3b out(480, 640, Vec3b(0, 0, 0)); - Vec3b color(0, 255, 0); - for (int ci = 0; ci < med.size(); ci++) - circle(out, med[ci], 2, color); - imshow("test", out); waitKey(100); - ****************************************/ - q2 = GetMedianSlope(med, M12, data.Sa); //get Sa ta = q2 Ma - } - - {// Second to first - // Reference slope - float dx_ref = float(med1.x - e2[0].x); - float dy_ref = float(med1.y - e2[0].y); - - if (dy_ref == 0) dy_ref = 0.00001f; - - float m_ref = dy_ref / dx_ref; - data.rb = m_ref; - - // Find points with same slope as reference - vector med; - med.reserve(hsize_1); - - uint minPoints = (uNs_ < hsize_1) ? uNs_ : hsize_1; - - vector indexes(minPoints); - if (uNs_ < hsize_1) - { - unsigned iSzBin = hsize_1 / unsigned(uNs_); - unsigned iIdx = hsize_1 + (iSzBin / 2); - - for (unsigned i = 0; i> 1; - - while (end - begin > 2) - { - float x2 = float(e2[j].x); - float y2 = float(e2[j].y); - float res = ((x2 - x1) * dy_ref) - ((y2 - y1) * dx_ref); - int sign_res = sgn(res); - - if (sign_res == 0) - { - //found - med.push_back(Point2f((x2 + x1)* 0.5f, (y2 + y1)* 0.5f)); - break; - } - - if (sign_res + sign_begin == 0) - { - sign_end = sign_res; - end = j; - } - else - { - sign_begin = sign_res; - begin = j; - } - j = (begin + end) >> 1; - } - - med.push_back(Point2f((e2[j].x + x1)* 0.5f, (e2[j].y + y1)* 0.5f)); - } - - if (med.size() < 2) - { - data.isValid = false; - return; - } - q4 = GetMedianSlope(med, M34, data.Sb); - } - - if (q2 == q4) - { - data.isValid = false; - return; - } - - float invDen = 1 / (q2 - q4); - data.Cab.x = (M34.y - q4*M34.x - M12.y + q2*M12.x) * invDen; - data.Cab.y = (q2*M34.y - q4*M12.y + q2*q4*(M12.x - M34.x)) * invDen; - data.ta = q2; - data.tb = q4; - data.Ma = M12; - data.Mb = M34; -} - -float EllipseDetector::GetMinAnglePI(float alpha, float beta) -{ - float pi = float(CV_PI); - float pi2 = float(2.0 * CV_PI); - - // normalize data in [0, 2*pi] - float a = fmod(alpha + pi2, pi2); - float b = fmod(beta + pi2, pi2); - - // normalize data in [0, pi] - if (a > pi) - a -= pi; - if (b > pi) - b -= pi; - - if (a > b) - { - swap(a, b); - } - - float diff1 = b - a; - float diff2 = pi - diff1; - return min(diff1, diff2); -} - -// Get the coordinates of the center, given the intersection of the estimated lines. See Fig. [8] in Sect [3.2.3] in the paper. -Point2f EllipseDetector::GetCenterCoordinates(EllipseData& data_ij, EllipseData& data_ik) -{ - float xx[7]; - float yy[7]; - - xx[0] = data_ij.Cab.x; - xx[1] = data_ik.Cab.x; - yy[0] = data_ij.Cab.y; - yy[1] = data_ik.Cab.y; - - { - // 1-1 - float q2 = data_ij.ta; - float q4 = data_ik.ta; - Point2f& M12 = data_ij.Ma; - Point2f& M34 = data_ik.Ma; - - float invDen = 1 / (q2 - q4); - xx[2] = (M34.y - q4*M34.x - M12.y + q2*M12.x) * invDen; - yy[2] = (q2*M34.y - q4*M12.y + q2*q4*(M12.x - M34.x)) * invDen; - } - - { - // 1-2 - float q2 = data_ij.ta; - float q4 = data_ik.tb; - Point2f& M12 = data_ij.Ma; - Point2f& M34 = data_ik.Mb; - - float invDen = 1 / (q2 - q4); - xx[3] = (M34.y - q4*M34.x - M12.y + q2*M12.x) * invDen; - yy[3] = (q2*M34.y - q4*M12.y + q2*q4*(M12.x - M34.x)) * invDen; - } - - { - // 2-2 - float q2 = data_ij.tb; - float q4 = data_ik.tb; - Point2f& M12 = data_ij.Mb; - Point2f& M34 = data_ik.Mb; - - float invDen = 1 / (q2 - q4); - xx[4] = (M34.y - q4*M34.x - M12.y + q2*M12.x) * invDen; - yy[4] = (q2*M34.y - q4*M12.y + q2*q4*(M12.x - M34.x)) * invDen; - } - - { - // 2-1 - float q2 = data_ij.tb; - float q4 = data_ik.ta; - Point2f& M12 = data_ij.Mb; - Point2f& M34 = data_ik.Ma; - - float invDen = 1 / (q2 - q4); - xx[5] = (M34.y - q4*M34.x - M12.y + q2*M12.x) * invDen; - yy[5] = (q2*M34.y - q4*M12.y + q2*q4*(M12.x - M34.x)) * invDen; - } - - xx[6] = (xx[0] + xx[1]) * 0.5f; - yy[6] = (yy[0] + yy[1]) * 0.5f; - - - // Median - nth_element(xx, xx + 3, xx + 7); - nth_element(yy, yy + 3, yy + 7); - float xc = xx[3]; - float yc = yy[3]; - - return Point2f(xc, yc); -} - -uint inline EllipseDetector::GenerateKey(uchar pair, ushort u, ushort v) -{ - return (pair << 30) + (u << 15) + v; -} - -// Draw at most iTopN detected ellipses. -void EllipseDetector::DrawDetectedEllipses(Mat& output, vector& ellipses, int iTopN, int thickness) -{ - int sz_ell = int(ellipses.size()); - int n = (iTopN == 0) ? sz_ell : min(iTopN, sz_ell); - for (int i = 0; i < n; ++i) - { - Ellipse& e = ellipses[n - i - 1]; - int g = cvRound(e.score_ * 255.f); - Scalar color(0, g, 0); - ellipse(output, Point(cvRound(e.xc_), cvRound(e.yc_)), Size(cvRound(e.a_), cvRound(e.b_)), e.rad_*180.0 / CV_PI, 0.0, 360.0, color, thickness); - // cv::circle(output, Point(e.xc_, e.yc_), 2, Scalar(0, 0, 255), 2); - } -} - -bool inline convex_check(VP& vp1, int start, int end) -{ - int x_min(4096), x_max(0), y_min(4096), y_max(0); - int integral_u(0), integral_d(0); - for (int i = start; i <= end; i++) - { - Point& val = vp1[i]; - x_min = MIN(x_min, val.x); - x_max = MAX(x_max, val.x); - y_min = MIN(y_min, val.y); - y_max = MAX(y_max, val.y); - } - for (int i = start; i <= end; i++) - { - Point& val = vp1[i]; - integral_u += (val.y - y_min); - integral_d += (y_max - val.y); - } - if (integral_u > integral_d) - return false; - else - return true; -} - -bool inline concave_check(VP& vp1, int start, int end) -{ - int x_min(4096), x_max(0), y_min(4096), y_max(0); - int integral_u(0), integral_d(0); - for (int i = start; i <= end; i++) - { - Point& val = vp1[i]; - x_min = MIN(x_min, val.x); - x_max = MAX(x_max, val.x); - y_min = MIN(y_min, val.y); - y_max = MAX(y_max, val.y); - } - for (int i = start; i <= end; i++) - { - Point& val = vp1[i]; - integral_u += (val.y - y_min); - integral_d += (y_max - val.y); - } - if (integral_u < integral_d) - return false; - else - return true; -} - -void EllipseDetector::ArcsCheck1234(VVP& points_1, VVP& points_2, VVP& points_3, VVP& points_4) -{ - static int nchecks = 21; - int i, j; - VVP vps_1, vps_2, vps_3, vps_4; - Mat3b out(480, 640, Vec3b(0, 0, 0)); - for (i = 0; i < points_2.size(); ++i) - { - // Vec3b color(rand()%255, 128+rand()%127, 128+rand()%127); - Vec3b color(255, 0, 0); - for (j = 0; j < points_2[i].size(); ++j) - out(points_2[i][j]) = color; - } - imshow("out", out); waitKey(); - - for (i = 0; i < points_1.size(); i++) - { - VP& vp1 = points_1[i]; - if (vp1.size() > nchecks) - { - VP vpn; - for (j = 0; j <= vp1.size() - nchecks; j++) - { - if (convex_check(vp1, j, j + nchecks - 1)) - { - vpn.push_back(vp1[j]); - } - else - { - vps_1.push_back(vpn); vpn.clear(); - } - } - vps_1.push_back(vpn); - } - else - { - cout << "==== small arc I ====" << endl; - } - } - for (i = 0; i < points_2.size(); i++) - { - VP& vp1 = points_2[i]; - if (vp1.size() > nchecks) - { - VP vpn; - for (j = 0; j <= vp1.size() - nchecks; j++) - { - if (concave_check(vp1, j, j + nchecks - 1)) - { - vpn.push_back(vp1[j]); - } - else - { - vps_2.push_back(vpn); vpn.clear(); - } - } - vps_2.push_back(vpn); - } - else - { - cout << "==== small arc II ====" << endl; - } - } - Mat3b out2(480, 640, Vec3b(0, 0, 0)); - for (i = 0; i < vps_2.size(); ++i) - { - // Vec3b color(rand()%255, 128+rand()%127, 128+rand()%127); - Vec3b color(255, 0, 0); - for (j = 0; j < vps_2[i].size(); ++j) - out2(vps_2[i][j]) = color; - } - imshow("out2", out2); waitKey(); - for (i = 0; i < points_3.size(); i++) - { - VP& vp1 = points_3[i]; - if (vp1.size() > nchecks) - { - VP vpn; - for (j = 0; j <= vp1.size() - nchecks; j++) - { - if (concave_check(vp1, j, j + nchecks - 1)) - { - vpn.push_back(vp1[j]); - } - else - { - vps_3.push_back(vpn); vpn.clear(); - } - } - vps_3.push_back(vpn); - } - else - { - cout << "==== small arc III ====" << endl; - } - } - for (i = 0; i < points_4.size(); i++) - { - VP& vp1 = points_4[i]; - if (vp1.size() > nchecks) - { - VP vpn; - for (j = 0; j <= vp1.size() - nchecks; j++) - { - if (convex_check(vp1, j, j + nchecks - 1)) - { - vpn.push_back(vp1[j]); - } - else - { - vps_4.push_back(vpn); vpn.clear(); - } - } - vps_4.push_back(vpn); - } - else - { - cout << "==== small arc IV ====" << endl; - } - } - points_1 = vps_1; points_2 = vps_2; points_3 = vps_3; points_4 = vps_4; -} diff --git a/src/无人机端代码/Modules/future_aircraft/src/ellipse_detector/ellipse_detector.h b/src/无人机端代码/Modules/future_aircraft/src/ellipse_detector/ellipse_detector.h deleted file mode 100644 index 7a8a1f90..00000000 --- a/src/无人机端代码/Modules/future_aircraft/src/ellipse_detector/ellipse_detector.h +++ /dev/null @@ -1,1374 +0,0 @@ -#ifndef SPIRE_ELLIPSEDETECTOR_H -#define SPIRE_ELLIPSEDETECTOR_H - -#include -#include -#include -#include -#include -#include - - - -#ifdef _WIN32 -/* -* Define architecture flags so we don't need to include windows.h. -* Avoiding windows.h makes it simpler to use windows sockets in conjunction -* with dirent.h. -*/ -#if !defined(_68K_) && !defined(_MPPC_) && !defined(_X86_) && !defined(_IA64_) && !defined(_AMD64_) && defined(_M_IX86) -# define _X86_ -#endif -#if !defined(_68K_) && !defined(_MPPC_) && !defined(_X86_) && !defined(_IA64_) && !defined(_AMD64_) && defined(_M_AMD64) -#define _AMD64_ -#endif - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* Indicates that d_type field is available in dirent structure */ -#define _DIRENT_HAVE_D_TYPE - -/* Indicates that d_namlen field is available in dirent structure */ -#define _DIRENT_HAVE_D_NAMLEN - -/* Entries missing from MSVC 6.0 */ -#if !defined(FILE_ATTRIBUTE_DEVICE) -# define FILE_ATTRIBUTE_DEVICE 0x40 -#endif - -/* File type and permission flags for stat(), general mask */ -#if !defined(S_IFMT) -# define S_IFMT _S_IFMT -#endif - -/* Directory bit */ -#if !defined(S_IFDIR) -# define S_IFDIR _S_IFDIR -#endif - -/* Character device bit */ -#if !defined(S_IFCHR) -# define S_IFCHR _S_IFCHR -#endif - -/* Pipe bit */ -#if !defined(S_IFFIFO) -# define S_IFFIFO _S_IFFIFO -#endif - -/* Regular file bit */ -#if !defined(S_IFREG) -# define S_IFREG _S_IFREG -#endif - -/* Read permission */ -#if !defined(S_IREAD) -# define S_IREAD _S_IREAD -#endif - -/* Write permission */ -#if !defined(S_IWRITE) -# define S_IWRITE _S_IWRITE -#endif - -/* Execute permission */ -#if !defined(S_IEXEC) -# define S_IEXEC _S_IEXEC -#endif - -/* Pipe */ -#if !defined(S_IFIFO) -# define S_IFIFO _S_IFIFO -#endif - -/* Block device */ -#if !defined(S_IFBLK) -# define S_IFBLK 0 -#endif - -/* Link */ -#if !defined(S_IFLNK) -# define S_IFLNK 0 -#endif - -/* Socket */ -#if !defined(S_IFSOCK) -# define S_IFSOCK 0 -#endif - -/* Read user permission */ -#if !defined(S_IRUSR) -# define S_IRUSR S_IREAD -#endif - -/* Write user permission */ -#if !defined(S_IWUSR) -# define S_IWUSR S_IWRITE -#endif - -/* Execute user permission */ -#if !defined(S_IXUSR) -# define S_IXUSR 0 -#endif - -/* Read group permission */ -#if !defined(S_IRGRP) -# define S_IRGRP 0 -#endif - -/* Write group permission */ -#if !defined(S_IWGRP) -# define S_IWGRP 0 -#endif - -/* Execute group permission */ -#if !defined(S_IXGRP) -# define S_IXGRP 0 -#endif - -/* Read others permission */ -#if !defined(S_IROTH) -# define S_IROTH 0 -#endif - -/* Write others permission */ -#if !defined(S_IWOTH) -# define S_IWOTH 0 -#endif - -/* Execute others permission */ -#if !defined(S_IXOTH) -# define S_IXOTH 0 -#endif - -/* Maximum length of file name */ -#if !defined(PATH_MAX) -# define PATH_MAX MAX_PATH -#endif -#if !defined(FILENAME_MAX) -# define FILENAME_MAX MAX_PATH -#endif -#if !defined(NAME_MAX) -# define NAME_MAX FILENAME_MAX -#endif - -/* File type flags for d_type */ -#define DT_UNKNOWN 0 -#define DT_REG S_IFREG -#define DT_DIR S_IFDIR -#define DT_FIFO S_IFIFO -#define DT_SOCK S_IFSOCK -#define DT_CHR S_IFCHR -#define DT_BLK S_IFBLK -#define DT_LNK S_IFLNK - -/* Macros for converting between st_mode and d_type */ -#define IFTODT(mode) ((mode) & S_IFMT) -#define DTTOIF(type) (type) - -/* -* File type macros. Note that block devices, sockets and links cannot be -* distinguished on Windows and the macros S_ISBLK, S_ISSOCK and S_ISLNK are -* only defined for compatibility. These macros should always return false -* on Windows. -*/ -#if !defined(S_ISFIFO) -# define S_ISFIFO(mode) (((mode) & S_IFMT) == S_IFIFO) -#endif -#if !defined(S_ISDIR) -# define S_ISDIR(mode) (((mode) & S_IFMT) == S_IFDIR) -#endif -#if !defined(S_ISREG) -# define S_ISREG(mode) (((mode) & S_IFMT) == S_IFREG) -#endif -#if !defined(S_ISLNK) -# define S_ISLNK(mode) (((mode) & S_IFMT) == S_IFLNK) -#endif -#if !defined(S_ISSOCK) -# define S_ISSOCK(mode) (((mode) & S_IFMT) == S_IFSOCK) -#endif -#if !defined(S_ISCHR) -# define S_ISCHR(mode) (((mode) & S_IFMT) == S_IFCHR) -#endif -#if !defined(S_ISBLK) -# define S_ISBLK(mode) (((mode) & S_IFMT) == S_IFBLK) -#endif - -/* Return the exact length of d_namlen without zero terminator */ -#define _D_EXACT_NAMLEN(p) ((p)->d_namlen) - -/* Return number of bytes needed to store d_namlen */ -#define _D_ALLOC_NAMLEN(p) (PATH_MAX) - - -#ifdef __cplusplus -extern "C" { -#endif - - - /* Wide-character version */ - struct _wdirent { - /* Always zero */ - long d_ino; - - /* Structure size */ - unsigned short d_reclen; - - /* Length of name without \0 */ - size_t d_namlen; - - /* File type */ - int d_type; - - /* File name */ - wchar_t d_name[PATH_MAX]; - }; - typedef struct _wdirent _wdirent; - - struct _WDIR { - /* Current directory entry */ - struct _wdirent ent; - - /* Private file data */ - WIN32_FIND_DATAW data; - - /* True if data is valid */ - int cached; - - /* Win32 search handle */ - HANDLE handle; - - /* Initial directory name */ - wchar_t *patt; - }; - typedef struct _WDIR _WDIR; - - static _WDIR *_wopendir(const wchar_t *dirname); - static struct _wdirent *_wreaddir(_WDIR *dirp); - static int _wclosedir(_WDIR *dirp); - static void _wrewinddir(_WDIR* dirp); - - - /* For compatibility with Symbian */ -#define wdirent _wdirent -#define WDIR _WDIR -#define wopendir _wopendir -#define wreaddir _wreaddir -#define wclosedir _wclosedir -#define wrewinddir _wrewinddir - - - /* Multi-byte character versions */ - struct dirent { - /* Always zero */ - long d_ino; - - /* Structure size */ - unsigned short d_reclen; - - /* Length of name without \0 */ - size_t d_namlen; - - /* File type */ - int d_type; - - /* File name */ - char d_name[PATH_MAX]; - }; - typedef struct dirent dirent; - - struct DIR { - struct dirent ent; - struct _WDIR *wdirp; - }; - typedef struct DIR DIR; - - static DIR *opendir(const char *dirname); - static struct dirent *readdir(DIR *dirp); - static int closedir(DIR *dirp); - static void rewinddir(DIR* dirp); - - - /* Internal utility functions */ - static WIN32_FIND_DATAW *dirent_first(_WDIR *dirp); - static WIN32_FIND_DATAW *dirent_next(_WDIR *dirp); - - static int dirent_mbstowcs_s( - size_t *pReturnValue, - wchar_t *wcstr, - size_t sizeInWords, - const char *mbstr, - size_t count); - - static int dirent_wcstombs_s( - size_t *pReturnValue, - char *mbstr, - size_t sizeInBytes, - const wchar_t *wcstr, - size_t count); - - static void dirent_set_errno(int error); - - /* - * Open directory stream DIRNAME for read and return a pointer to the - * internal working area that is used to retrieve individual directory - * entries. - */ - static _WDIR* - _wopendir( - const wchar_t *dirname) - { - _WDIR *dirp = NULL; - int error; - - /* Must have directory name */ - if (dirname == NULL || dirname[0] == '\0') { - dirent_set_errno(ENOENT); - return NULL; - } - - /* Allocate new _WDIR structure */ - dirp = (_WDIR*)malloc(sizeof(struct _WDIR)); - if (dirp != NULL) { - DWORD n; - - /* Reset _WDIR structure */ - dirp->handle = INVALID_HANDLE_VALUE; - dirp->patt = NULL; - dirp->cached = 0; - - /* Compute the length of full path plus zero terminator */ - n = GetFullPathNameW(dirname, 0, NULL, NULL); - - /* Allocate room for absolute directory name and search pattern */ - dirp->patt = (wchar_t*)malloc(sizeof(wchar_t) * n + 16); - if (dirp->patt) { - - /* - * Convert relative directory name to an absolute one. This - * allows rewinddir() to function correctly even when current - * working directory is changed between opendir() and rewinddir(). - */ - n = GetFullPathNameW(dirname, n, dirp->patt, NULL); - if (n > 0) { - wchar_t *p; - - /* Append search pattern \* to the directory name */ - p = dirp->patt + n; - if (dirp->patt < p) { - switch (p[-1]) { - case '\\': - case '/': - case ':': - /* Directory ends in path separator, e.g. c:\temp\ */ - /*NOP*/; - break; - - default: - /* Directory name doesn't end in path separator */ - *p++ = '\\'; - } - } - *p++ = '*'; - *p = '\0'; - - /* Open directory stream and retrieve the first entry */ - if (dirent_first(dirp)) { - /* Directory stream opened successfully */ - error = 0; - } - else { - /* Cannot retrieve first entry */ - error = 1; - dirent_set_errno(ENOENT); - } - - } - else { - /* Cannot retrieve full path name */ - dirent_set_errno(ENOENT); - error = 1; - } - - } - else { - /* Cannot allocate memory for search pattern */ - error = 1; - } - - } - else { - /* Cannot allocate _WDIR structure */ - error = 1; - } - - /* Clean up in case of error */ - if (error && dirp) { - _wclosedir(dirp); - dirp = NULL; - } - - return dirp; - } - - /* - * Read next directory entry. The directory entry is returned in dirent - * structure in the d_name field. Individual directory entries returned by - * this function include regular files, sub-directories, pseudo-directories - * "." and ".." as well as volume labels, hidden files and system files. - */ - static struct _wdirent* - _wreaddir( - _WDIR *dirp) - { - WIN32_FIND_DATAW *datap; - struct _wdirent *entp; - - /* Read next directory entry */ - datap = dirent_next(dirp); - if (datap) { - size_t n; - DWORD attr; - - /* Pointer to directory entry to return */ - entp = &dirp->ent; - - /* - * Copy file name as wide-character string. If the file name is too - * long to fit in to the destination buffer, then truncate file name - * to PATH_MAX characters and zero-terminate the buffer. - */ - n = 0; - while (n + 1 < PATH_MAX && datap->cFileName[n] != 0) { - entp->d_name[n] = datap->cFileName[n]; - n++; - } - dirp->ent.d_name[n] = 0; - - /* Length of file name excluding zero terminator */ - entp->d_namlen = n; - - /* File type */ - attr = datap->dwFileAttributes; - if ((attr & FILE_ATTRIBUTE_DEVICE) != 0) { - entp->d_type = DT_CHR; - } - else if ((attr & FILE_ATTRIBUTE_DIRECTORY) != 0) { - entp->d_type = DT_DIR; - } - else { - entp->d_type = DT_REG; - } - - /* Reset dummy fields */ - entp->d_ino = 0; - entp->d_reclen = sizeof(struct _wdirent); - - } - else { - - /* Last directory entry read */ - entp = NULL; - - } - - return entp; - } - - /* - * Close directory stream opened by opendir() function. This invalidates the - * DIR structure as well as any directory entry read previously by - * _wreaddir(). - */ - static int - _wclosedir( - _WDIR *dirp) - { - int ok; - if (dirp) { - - /* Release search handle */ - if (dirp->handle != INVALID_HANDLE_VALUE) { - FindClose(dirp->handle); - dirp->handle = INVALID_HANDLE_VALUE; - } - - /* Release search pattern */ - if (dirp->patt) { - free(dirp->patt); - dirp->patt = NULL; - } - - /* Release directory structure */ - free(dirp); - ok = /*success*/0; - - } - else { - /* Invalid directory stream */ - dirent_set_errno(EBADF); - ok = /*failure*/-1; - } - return ok; - } - - /* - * Rewind directory stream such that _wreaddir() returns the very first - * file name again. - */ - static void - _wrewinddir( - _WDIR* dirp) - { - if (dirp) { - /* Release existing search handle */ - if (dirp->handle != INVALID_HANDLE_VALUE) { - FindClose(dirp->handle); - } - - /* Open new search handle */ - dirent_first(dirp); - } - } - - /* Get first directory entry (internal) */ - static WIN32_FIND_DATAW* - dirent_first( - _WDIR *dirp) - { - WIN32_FIND_DATAW *datap; - - /* Open directory and retrieve the first entry */ - dirp->handle = FindFirstFileW(dirp->patt, &dirp->data); - if (dirp->handle != INVALID_HANDLE_VALUE) { - - /* a directory entry is now waiting in memory */ - datap = &dirp->data; - dirp->cached = 1; - - } - else { - - /* Failed to re-open directory: no directory entry in memory */ - dirp->cached = 0; - datap = NULL; - - } - return datap; - } - - /* Get next directory entry (internal) */ - static WIN32_FIND_DATAW* - dirent_next( - _WDIR *dirp) - { - WIN32_FIND_DATAW *p; - - /* Get next directory entry */ - if (dirp->cached != 0) { - - /* A valid directory entry already in memory */ - p = &dirp->data; - dirp->cached = 0; - - } - else if (dirp->handle != INVALID_HANDLE_VALUE) { - - /* Get the next directory entry from stream */ - if (FindNextFileW(dirp->handle, &dirp->data) != FALSE) { - /* Got a file */ - p = &dirp->data; - } - else { - /* The very last entry has been processed or an error occured */ - FindClose(dirp->handle); - dirp->handle = INVALID_HANDLE_VALUE; - p = NULL; - } - - } - else { - - /* End of directory stream reached */ - p = NULL; - - } - - return p; - } - - /* - * Open directory stream using plain old C-string. - */ - static DIR* - opendir( - const char *dirname) - { - struct DIR *dirp; - int error; - - /* Must have directory name */ - if (dirname == NULL || dirname[0] == '\0') { - dirent_set_errno(ENOENT); - return NULL; - } - - /* Allocate memory for DIR structure */ - dirp = (DIR*)malloc(sizeof(struct DIR)); - if (dirp) { - wchar_t wname[PATH_MAX]; - size_t n; - - /* Convert directory name to wide-character string */ - error = dirent_mbstowcs_s(&n, wname, PATH_MAX, dirname, PATH_MAX); - if (!error) { - - /* Open directory stream using wide-character name */ - dirp->wdirp = _wopendir(wname); - if (dirp->wdirp) { - /* Directory stream opened */ - error = 0; - } - else { - /* Failed to open directory stream */ - error = 1; - } - - } - else { - /* - * Cannot convert file name to wide-character string. This - * occurs if the string contains invalid multi-byte sequences or - * the output buffer is too small to contain the resulting - * string. - */ - error = 1; - } - - } - else { - /* Cannot allocate DIR structure */ - error = 1; - } - - /* Clean up in case of error */ - if (error && dirp) { - free(dirp); - dirp = NULL; - } - - return dirp; - } - - /* - * Read next directory entry. - * - * When working with text consoles, please note that file names returned by - * readdir() are represented in the default ANSI code page while any output to - * console is typically formatted on another code page. Thus, non-ASCII - * characters in file names will not usually display correctly on console. The - * problem can be fixed in two ways: (1) change the character set of console - * to 1252 using chcp utility and use Lucida Console font, or (2) use - * _cprintf function when writing to console. The _cprinf() will re-encode - * ANSI strings to the console code page so many non-ASCII characters will - * display correcly. - */ - static struct dirent* - readdir( - DIR *dirp) - { - WIN32_FIND_DATAW *datap; - struct dirent *entp; - - /* Read next directory entry */ - datap = dirent_next(dirp->wdirp); - if (datap) { - size_t n; - int error; - - /* Attempt to convert file name to multi-byte string */ - error = dirent_wcstombs_s( - &n, dirp->ent.d_name, PATH_MAX, datap->cFileName, PATH_MAX); - - /* - * If the file name cannot be represented by a multi-byte string, - * then attempt to use old 8+3 file name. This allows traditional - * Unix-code to access some file names despite of unicode - * characters, although file names may seem unfamiliar to the user. - * - * Be ware that the code below cannot come up with a short file - * name unless the file system provides one. At least - * VirtualBox shared folders fail to do this. - */ - if (error && datap->cAlternateFileName[0] != '\0') { - error = dirent_wcstombs_s( - &n, dirp->ent.d_name, PATH_MAX, - datap->cAlternateFileName, PATH_MAX); - } - - if (!error) { - DWORD attr; - - /* Initialize directory entry for return */ - entp = &dirp->ent; - - /* Length of file name excluding zero terminator */ - entp->d_namlen = n - 1; - - /* File attributes */ - attr = datap->dwFileAttributes; - if ((attr & FILE_ATTRIBUTE_DEVICE) != 0) { - entp->d_type = DT_CHR; - } - else if ((attr & FILE_ATTRIBUTE_DIRECTORY) != 0) { - entp->d_type = DT_DIR; - } - else { - entp->d_type = DT_REG; - } - - /* Reset dummy fields */ - entp->d_ino = 0; - entp->d_reclen = sizeof(struct dirent); - - } - else { - /* - * Cannot convert file name to multi-byte string so construct - * an errornous directory entry and return that. Note that - * we cannot return NULL as that would stop the processing - * of directory entries completely. - */ - entp = &dirp->ent; - entp->d_name[0] = '?'; - entp->d_name[1] = '\0'; - entp->d_namlen = 1; - entp->d_type = DT_UNKNOWN; - entp->d_ino = 0; - entp->d_reclen = 0; - } - - } - else { - /* No more directory entries */ - entp = NULL; - } - - return entp; - } - - /* - * Close directory stream. - */ - static int - closedir( - DIR *dirp) - { - int ok; - if (dirp) { - - /* Close wide-character directory stream */ - ok = _wclosedir(dirp->wdirp); - dirp->wdirp = NULL; - - /* Release multi-byte character version */ - free(dirp); - - } - else { - - /* Invalid directory stream */ - dirent_set_errno(EBADF); - ok = /*failure*/-1; - - } - return ok; - } - - /* - * Rewind directory stream to beginning. - */ - static void - rewinddir( - DIR* dirp) - { - /* Rewind wide-character string directory stream */ - _wrewinddir(dirp->wdirp); - } - - /* Convert multi-byte string to wide character string */ - static int - dirent_mbstowcs_s( - size_t *pReturnValue, - wchar_t *wcstr, - size_t sizeInWords, - const char *mbstr, - size_t count) - { - int error; - -#if defined(_MSC_VER) && _MSC_VER >= 1400 - - /* Microsoft Visual Studio 2005 or later */ - error = mbstowcs_s(pReturnValue, wcstr, sizeInWords, mbstr, count); - -#else - - /* Older Visual Studio or non-Microsoft compiler */ - size_t n; - - /* Convert to wide-character string (or count characters) */ - n = mbstowcs(wcstr, mbstr, sizeInWords); - if (!wcstr || n < count) { - - /* Zero-terminate output buffer */ - if (wcstr && sizeInWords) { - if (n >= sizeInWords) { - n = sizeInWords - 1; - } - wcstr[n] = 0; - } - - /* Length of resuting multi-byte string WITH zero terminator */ - if (pReturnValue) { - *pReturnValue = n + 1; - } - - /* Success */ - error = 0; - - } - else { - - /* Could not convert string */ - error = 1; - - } - -#endif - - return error; - } - - /* Convert wide-character string to multi-byte string */ - static int - dirent_wcstombs_s( - size_t *pReturnValue, - char *mbstr, - size_t sizeInBytes, /* max size of mbstr */ - const wchar_t *wcstr, - size_t count) - { - int error; - -#if defined(_MSC_VER) && _MSC_VER >= 1400 - - /* Microsoft Visual Studio 2005 or later */ - error = wcstombs_s(pReturnValue, mbstr, sizeInBytes, wcstr, count); - -#else - - /* Older Visual Studio or non-Microsoft compiler */ - size_t n; - - /* Convert to multi-byte string (or count the number of bytes needed) */ - n = wcstombs(mbstr, wcstr, sizeInBytes); - if (!mbstr || n < count) { - - /* Zero-terminate output buffer */ - if (mbstr && sizeInBytes) { - if (n >= sizeInBytes) { - n = sizeInBytes - 1; - } - mbstr[n] = '\0'; - } - - /* Lenght of resulting multi-bytes string WITH zero-terminator */ - if (pReturnValue) { - *pReturnValue = n + 1; - } - - /* Success */ - error = 0; - - } - else { - - /* Cannot convert string */ - error = 1; - - } - -#endif - - return error; - } - - /* Set errno variable */ - static void - dirent_set_errno( - int error) - { -#if defined(_MSC_VER) && _MSC_VER >= 1400 - - /* Microsoft Visual Studio 2005 and later */ - _set_errno(error); - -#else - - /* Non-Microsoft compiler or older Microsoft compiler */ - errno = error; - -#endif - } - - -#ifdef __cplusplus -} -#endif -#include -#else -#include -// #include -#endif - -#ifdef USE_OMP -#include -#else -int omp_get_max_threads(); -int omp_get_thread_num(); -// int omp_set_num_threads(int); -#endif - -typedef std::vector VP; -typedef std::vector< VP > VVP; -typedef unsigned int uint; - - -void _list_dir(std::string dir, std::vector& files, std::string suffixs = "", bool r = false); - -std::vector _split(const std::string& srcstr, const std::string& delimeter); -bool _startswith(const std::string& str, const std::string& start); -bool _endswith(const std::string& str, const std::string& end); -void _randperm(int n, int m, int arr[], bool sort_ = true); - -/***************** math-related functions ****************/ -float _atan2(float y, float x); -void _mean_std(std::vector& vec, float& mean, float& std); -int inline _sgn(float val) { return (0.f < val) - (val < 0.f); } -float inline _ed2(const cv::Point& A, const cv::Point& B) -{ - return float(((B.x - A.x)*(B.x - A.x) + (B.y - A.y)*(B.y - A.y))); -} -float _get_min_angle_PI(float alpha, float beta); - -double inline _tic() -{ - return (double)cv::getTickCount(); -} -double inline _toc(double tic) // ms -{ - return ((double)cv::getTickCount() - tic)*1000. / cv::getTickFrequency(); -} -inline int _isnan(double x) { return x != x; } - - -void _tag_canny(cv::InputArray image, cv::OutputArray _edges, - cv::OutputArray _sobel_x, cv::OutputArray _sobel_y, - int apertureSize, bool L2gradient, double percent_ne); - - -void _find_contours_oneway(cv::Mat1b& image, VVP& segments, int iMinLength); -void _find_contours_eight(cv::Mat1b& image, std::vector& segments, int iMinLength); -void _show_contours_eight(cv::Mat1b& image, std::vector& segments, const char* title); - -void _tag_find_contours(cv::Mat1b& image, VVP& segments, int iMinLength); -void _tag_show_contours(cv::Mat1b& image, VVP& segments, const char* title); -void _tag_show_contours(cv::Size& imsz, VVP& segments, const char* title); - -bool _SortBottomLeft2TopRight(const cv::Point& lhs, const cv::Point& rhs); -bool _SortBottomLeft2TopRight2f(const cv::Point2f& lhs, const cv::Point2f& rhs); -bool _SortTopLeft2BottomRight(const cv::Point& lhs, const cv::Point& rhs); - - -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif -#define M_2__PI 6.28318530718 -#define M_1_2_PI 1.57079632679 - -// Elliptical struct definition -class Ellipse -{ -public: - float xc_; - float yc_; - float a_; - float b_; - float rad_; - float score_; - - // Elliptic General equations Ax^2 + Bxy + Cy^2 + Dx + Ey + 1 = 0 - float A_; - float B_; - float C_; - float D_; - float E_; - float F_; - - Ellipse() : xc_(0.f), yc_(0.f), a_(0.f), b_(0.f), rad_(0.f), score_(0.f), - A_(0.f), B_(0.f), C_(0.f), D_(0.f), E_(0.f), F_(1.f) {} - Ellipse(float xc, float yc, float a, float b, float rad, float score = 0.f) : xc_(xc), yc_(yc), a_(a), b_(b), rad_(rad), score_(score) {} - Ellipse(const Ellipse& other) : xc_(other.xc_), yc_(other.yc_), a_(other.a_), b_(other.b_), rad_(other.rad_), score_(other.score_), - A_(other.A_), B_(other.B_), C_(other.C_), D_(other.D_), E_(other.E_) {} - - void Draw(cv::Mat& img, const cv::Scalar& color, const int thickness) - { - if (IsValid()) - ellipse(img, cv::Point(cvRound(xc_), cvRound(yc_)), cv::Size(cvRound(a_), cvRound(b_)), rad_ * 180.0 / CV_PI, 0.0, 360.0, color, thickness); - } - - void Draw(cv::Mat3b& img, const int thickness) - { - cv::Scalar color(0, cvFloor(255.f * score_), 0); - if (IsValid()) - ellipse(img, cv::Point(cvRound(xc_), cvRound(yc_)), cv::Size(cvRound(a_), cvRound(b_)), rad_ * 180.0 / CV_PI, 0.0, 360.0, color, thickness); - } - - bool operator<(const Ellipse& other) const - { // use for sorting - if (score_ == other.score_) - { - float lhs_e = b_ / a_; - float rhs_e = other.b_ / other.a_; - if (lhs_e == rhs_e) - { - return false; - } - return lhs_e > rhs_e; - } - return score_ > other.score_; - } - - // Elliptic General equations Ax^2 + Bxy + Cy^2 + Dx + Ey + F = 0 - void TransferFromGeneral() { - float denominator = (B_*B_ - 4 * A_*C_); - - xc_ = (2 * C_*D_ - B_*E_) / denominator; - yc_ = (2 * A_*E_ - B_*D_) / denominator; - - float pre = 2 * (A_*E_*E_ + C_*D_*D_ - B_*D_*E_ + denominator*F_); - float lst = sqrt((A_ - C_)*(A_ - C_) + B_*B_); - - a_ = -sqrt(pre*(A_ + C_ + lst)) / denominator; - b_ = -sqrt(pre*(A_ + C_ - lst)) / denominator; - - if (B_ == 0 && A_C_) - rad_ = CV_PI / 2; - else - rad_ = atan((C_ - A_ - lst) / B_); - } - - // Elliptic General equations Ax^2 + Bxy + Cy^2 + Dx + Ey + F = 0 - void TransferToGeneral() { - A_ = a_*a_*sin(rad_)*sin(rad_) + b_*b_*cos(rad_)*cos(rad_); - B_ = 2.f*(b_*b_ - a_*a_)*sin(rad_)*cos(rad_); - C_ = a_*a_*cos(rad_)*cos(rad_) + b_*b_*sin(rad_)*sin(rad_); - D_ = -2.f*A_*xc_ - B_*yc_; - E_ = -B_*xc_ - 2.f*C_*yc_; - F_ = A_*xc_*xc_ + B_*xc_*yc_ + C_*yc_*yc_ - a_*a_*b_*b_; - } - - void GetRectangle(cv::Rect& rect) { - float sin_theta = sin(-rad_); - float cos_theta = cos(-rad_); - float A = a_*a_ * sin_theta * sin_theta + b_* b_ * cos_theta * cos_theta; - float B = 2 * (a_* a_ - b_ * b_) * sin_theta * cos_theta; - float C = a_* a_ * cos_theta * cos_theta + b_ * b_ * sin_theta * sin_theta; - float F = - a_ * a_ * b_ * b_; - - float y = sqrt(4 * A * F / (B * B - 4 * A * C)); - float y1 = -abs(y), y2 = abs(y); - float x = sqrt(4 * C * F / (B * B - 4 * C * A)); - float x1 = -abs(x), x2 = abs(x); - - rect = cv::Rect(int(round(xc_ + x1)), int(round(yc_ + y1)), int(round(x2 - x1)), int(round(y2 - y1))); - } - - float Perimeter() { - // return 2*CV_PI*b_ + 4*(a_ - b_); - return CV_PI*(3.f*(a_ + b_) - sqrt((3.f*a_ + b_)*(a_ + 3.f*b_))); - } - - float Area() { - return CV_PI*a_*b_; - } - - bool IsValid() { - bool nan = isnan(xc_) || isnan(yc_) || isnan(a_) || isnan(b_) || isnan(rad_); - return !nan; - } -}; - -// Data available after selection strategy. -// They are kept in an associative array to: -// 1) avoid recomputing data when starting from same arcs -// 2) be reused in firther proprecessing -struct EllipseData -{ - bool isValid; - float ta; // arc_a center line gradient - float tb; // arc_b - float ra; // gradient of a (slope of start of chord_1 and center of chord_2) - float rb; // gradient of b (slope of center of chord_1 and last of chord_2) - cv::Point2f Ma; // arc_a center of element - cv::Point2f Mb; // arc_b - cv::Point2f Cab; // center of ellipse - std::vector Sa; // arc_a's center line of parallel chords - std::vector Sb; // arc_b's center line of parallel chords -}; - -struct EllipseThreePoint -{ - bool isValid; - cv::Point Cab; - VP ArcI; - VP ArcJ; - VP ArcK; -}; - -/********************** EllipseFitting functions **********************/ -void _ellipse_foci(float *param, float *foci); -float _ellipse_normal_angle(float x, float y, float *foci); -float _angle_diff(float a, float b); - -/*************************** CNC functions ****************************/ -float _value4SixPoints(cv::Point2f p3, cv::Point2f p2, cv::Point2f p1, cv::Point2f p4, cv::Point2f p5, cv::Point2f p6); - - -/**************** ellipse-evaluation-related functions ****************/ -void _load_ellipse_GT(const std::string& gt_file_name, std::vector & gt_ellipses, bool is_angle_radians = true); -void _load_ellipse_DT(const std::string& dt_file_name, std::vector & dt_ellipses, bool is_angle_radians = true); - -bool _ellipse_overlap(const cv::Mat1b& gt, const cv::Mat1b& dt, float th); -float _ellipse_overlap_real(const cv::Mat1b& gt, const cv::Mat1b& dt); -int _bool_count(const std::vector vb); -float _ellipse_evaluate_one(const std::vector& ell_gt, const std::vector& ell_dt, const cv::Mat3b& img); -float _ellipse_evaluate(std::vector& image_fns, std::vector& gt_fns, std::vector& dt_fns, - bool gt_angle_radians = true); - - - - -class EllipseDetector -{ - // Parameters - - // Preprocessing - Gaussian filter. See Sect [] in the paper - cv::Size szPreProcessingGaussKernel_; // size of the Gaussian filter in preprocessing step - double dPreProcessingGaussSigma_; // sigma of the Gaussian filter in the preprocessing step - - - // Selection strategy - Step 1 - Discard noisy or straight arcs. See Sect [] in the paper - int iMinEdgeLength_; // minimum edge size - float fMinOrientedRectSide_; // minumum size of the oriented bounding box containing the arc - float fMaxRectAxesRatio_; // maximum aspect ratio of the oriented bounding box containing the arc - - // Selection strategy - Step 2 - Remove according to mutual convexities. See Sect [] in the paper - float fThrArcPosition_; - - // Selection Strategy - Step 3 - Number of points considered for slope estimation when estimating the center. See Sect [] in the paper - unsigned uNs_; // Find at most Ns parallel chords. - - // Selection strategy - Step 3 - Discard pairs of arcs if their estimated center is not close enough. See Sect [] in the paper - float fMaxCenterDistance_; // maximum distance in pixel between 2 center points - float fMaxCenterDistance2_; // _fMaxCenterDistance * _fMaxCenterDistance - - // Validation - Points within a this threshold are considered to lie on the ellipse contour. See Sect [] in the paper - float fDistanceToEllipseContour_; // maximum distance between a point and the contour. See equation [] in the paper - - // Validation - Assign a score. See Sect [] in the paper - float fMinScore_; // minimum score to confirm a detection - float fMinReliability_; // minimum auxiliary score to confirm a detection - - double dPercentNe_; - - float fT_CNC_; - float fT_TCN_L_; // filter lines - float fT_TCN_P_; - float fThre_r_; - - // auxiliary variables - cv::Size szIm_; // input image size - - std::vector times_; // times_ is a vector containing the execution time of each step. - - int ACC_N_SIZE; // size of accumulator N = B/A - int ACC_R_SIZE; // size of accumulator R = rho = atan(K) - int ACC_A_SIZE; // size of accumulator A - - int* accN; // pointer to accumulator N - int* accR; // pointer to accumulator R - int* accA; // pointer to accumulator A - - cv::Mat1f EO_; - - VVP points_1, points_2, points_3, points_4; // vector of points, one for each convexity class - -public: - - // Constructor and Destructor - EllipseDetector(void); - ~EllipseDetector(void); - - // Detect the ellipses in the gray image - void Detect(cv::Mat3b& I, std::vector& ellipses); - void Detect(cv::Mat& I, std::vector& ellipses); - - // Draw the first iTopN ellipses on output - void DrawDetectedEllipses(cv::Mat& output, std::vector& ellipses, int iTopN = 0, int thickness = 2); - - // Set the parameters of the detector - void SetParameters(cv::Size szPreProcessingGaussKernelSize, - double dPreProcessingGaussSigma, - float fThPosition, - float fMaxCenterDistance, - int iMinEdgeLength, - float fMinOrientedRectSide, - float fDistanceToEllipseContour, - float fMinScore, - float fMinReliability, - int iNs, - double dPercentNe, - float fT_CNC, - float fT_TCN_L, - float fT_TCN_P, - float fThre_r - ); - - // Return the execution time - double GetExecTime() { - double time_all(0); - for (size_t i = 0; i < times_.size(); i++) time_all += times_[i]; - return time_all; - } - std::vector GetTimes() { return times_; } - - float countOfFindEllipse_; - float countOfGetFastCenter_; - -private: - - // keys for hash table - static const ushort PAIR_12 = 0x00; - static const ushort PAIR_23 = 0x01; - static const ushort PAIR_34 = 0x02; - static const ushort PAIR_14 = 0x03; - - // generate keys from pair and indicse - uint inline GenerateKey(uchar pair, ushort u, ushort v); - - void PreProcessing(cv::Mat1b& I, cv::Mat1b& arcs8); - void RemoveStraightLine(VVP& segments, VVP& segments_update, int id = 0); - void PreProcessing(cv::Mat1b& I, cv::Mat1b& DP, cv::Mat1b& DN); - - void ClusterEllipses(std::vector& ellipses); - - // int FindMaxK(const std::vector& v) const; - // int FindMaxN(const std::vector& v) const; - // int FindMaxA(const std::vector& v) const; - - int FindMaxK(const int* v) const; - int FindMaxN(const int* v) const; - int FindMaxA(const int* v) const; - - float GetMedianSlope(std::vector& med, cv::Point2f& M, std::vector& slopes); - void GetFastCenter(std::vector& e1, std::vector& e2, EllipseData& data); - float GetMinAnglePI(float alpha, float beta); - - void DetectEdges13(cv::Mat1b& DP, VVP& points_1, VVP& points_3); - void DetectEdges24(cv::Mat1b& DN, VVP& points_2, VVP& points_4); - - void ArcsCheck1234(VVP& points_1, VVP& points_2, VVP& points_3, VVP& points_4); - - void FindEllipses(cv::Point2f& center, - VP& edge_i, - VP& edge_j, - VP& edge_k, - EllipseData& data_ij, - EllipseData& data_ik, - Ellipse& ell - ); - - cv::Point2f GetCenterCoordinates(EllipseData& data_ij, EllipseData& data_ik); - - void Triplets124(VVP& pi, - VVP& pj, - VVP& pk, - std::unordered_map& data, - std::vector& ellipses - ); - - void Triplets231(VVP& pi, - VVP& pj, - VVP& pk, - std::unordered_map& data, - std::vector& ellipses - ); - - void Triplets342(VVP& pi, - VVP& pj, - VVP& pk, - std::unordered_map& data, - std::vector& ellipses - ); - - void Triplets413(VVP& pi, - VVP& pj, - VVP& pk, - std::unordered_map& data, - std::vector& ellipses - ); - - void Tic(unsigned idx = 0) //start - { - while (idx >= timesSign_.size()) { - timesSign_.push_back(0); - times_.push_back(.0); - } - timesSign_[idx] = 0; - timesSign_[idx]++; - times_[idx] = (double)cv::getTickCount(); - } - - void Toc(unsigned idx = 0, std::string step = "") //stop - { - assert(timesSign_[idx] == 1); - timesSign_[idx]++; - times_[idx] = ((double)cv::getTickCount() - times_[idx])*1000. / cv::getTickFrequency(); - // #ifdef DEBUG_SPEED - std::cout << "Cost time: " << times_[idx] << " ms [" << idx << "] - " << step << std::endl; - if (idx == times_.size() - 1) - std::cout << "Totally cost time: " << this->GetExecTime() << " ms" << std::endl; - // #endif - } - -private: - std::vector timesSign_; -}; - - - -#endif // SPIRE_ELLIPSEDETECTOR_H diff --git a/src/无人机端代码/Modules/future_aircraft/src/future_aircraft.cpp b/src/无人机端代码/Modules/future_aircraft/src/future_aircraft.cpp deleted file mode 100644 index a21be801..00000000 --- a/src/无人机端代码/Modules/future_aircraft/src/future_aircraft.cpp +++ /dev/null @@ -1,475 +0,0 @@ -/****************************************************************************** -*例程简介: 第二届大学生未来飞行器挑战赛的实践类仿真demo -* -*效果说明: 无人机由指定的位置一键起飞后，立即转换为自动模式，开始通过机载传感器 -* 自主搜索这些目标标靶，并向目标标靶投掷模拟子弹；完成所有的投掷任务后， -* 自主回到起飞点降落。 -* -*备注:该例程仅支持Prometheus仿真,真机测试需要熟练掌握相关接口的定义后以及真机适配修改后使用 -******************************************************************************/ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "printf_utils.h" -#include "mission_utils.h" - -using namespace std; - -#define VISION_THRES 10 -#define HOLD_THRES 60 -const float PI = 3.1415926; - -//创建无人机相关数据变量 -prometheus_msgs::UAVCommand uav_command; -prometheus_msgs::UAVState uav_state; -prometheus_msgs::UAVControlState uav_control_state; -Eigen::Matrix3f R_Body_to_ENU; // 无人机机体系至惯性系转换矩阵 -prometheus_msgs::DetectionInfo ellipse_det; -float camera_offset[3]; -//static target1 -// 最大目标丢失计数 -constexpr int max_loss_count = 30; -int loss_count = max_loss_count; -int num_lost = 0; //视觉丢失计数器 -int num_regain = 0; -bool is_detected = false; -int hold_count = 0; -int hold_lost = 0; -bool is_holded = false; - - -void uav_state_cb(const prometheus_msgs::UAVState::ConstPtr &msg) -{ - uav_state = *msg; - R_Body_to_ENU = get_rotation_matrix(uav_state.attitude[0],uav_state.attitude[1],uav_state.attitude[2]); - -} - -//无人机控制状态回调函数 -void uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg) -{ - uav_control_state = *msg; -} - -void ellipse_det_cb(const prometheus_msgs::MultiDetectionInfo::ConstPtr &msg) -{ - // g_ellipse_det. = false; - for(auto &ellipes : msg->detection_infos) - { - ellipse_det = ellipes; - if (ellipse_det.detected && ellipse_det.object_name == "T") - { - num_regain++; - num_lost = 0; - } - else{ - num_regain = 0; - num_lost++; - } - if(num_lost > VISION_THRES) - { - is_detected = false; - // PCOUT(1, GREEN, "no detect"); - } - if(num_regain > VISION_THRES){ - is_detected = true; - // PCOUT(1, GREEN, "detected"); - } - ellipse_det.sight_angle[0] = ellipes.sight_angle[1]; - ellipse_det.sight_angle[1] = ellipes.sight_angle[0]; - // ellipse_det.position[2] = -ellipes.position[2]; - } - -} -// 创建圆形跟踪的相关变量 -// 整个圆形的飞行时间 -float circular_time; -// 每次控制数据更新时的弧度增量 -float angle_increment; -// 无人机的合速度也就是圆的线速度 -float line_velocity; -// 无人机的控制频率 -float control_rate; -// 圆的半径 -float radius; -//通过设定整个圆的飞行时间,控制频率,圆的半径来获取相关参数 -void get_circular_property(float time, int rate, float radius) -{ - //计算角速度(rad/s) - float w = 2 * PI / time; - //计算线速度(m/s) - line_velocity = radius * w; - //计算控制数据发布的弧度增量 - angle_increment = w / rate; -} - -//主函数 -int main(int argc, char** argv) -{ - //ROS初始化,设定节点名 - ros::init(argc , argv, "future_aircraft"); - //创建句柄 - ros::NodeHandle n; - //声明起飞高度,无人机id变量 - float takeoff_height; - int uav_id; - //获取起飞高度参数 - ros::param::get("/uav_control_main_1/control/Takeoff_height", takeoff_height); - ros::param::get("~uav_id", uav_id); - // 相机安装偏移,规定为:相机在机体系(质心原点)的位置 - n.param("camera_offset_x", camera_offset[0], 0.0); - n.param("camera_offset_y", camera_offset[1], 0.0); - n.param("camera_offset_z", camera_offset[2], 0.0); - //创建无人机控制命令发布者 - ros::Publisher uav_command_pub = n.advertise("/uav1/prometheus/command", 10); - //创建无人机状态命令订阅者 - ros::Subscriber uav_state_sub = n.subscribe("/uav1/prometheus/state", 10, uav_state_cb); - //创建无人机控制状态命令订阅者 - ros::Subscriber uav_control_state_sub = n.subscribe("/uav1/prometheus/control_state", 10, uav_control_state_cb); - - ros::Subscriber ellipse_det_sub = n.subscribe("/prometheus/ellipse_det", 10, ellipse_det_cb); - - // 圆轨迹周期 - circular_time = 40; - control_rate = 20; - // 圆轨迹半径 - radius = 2.5; - - // 获取线速度line_velocity， 角速度angle_increment - get_circular_property(circular_time, control_rate, radius); - int count = 0; - bool circular_success = false; - - //循环频率设置为**HZ - ros::Rate r(control_rate); - - Eigen::Vector3d waypoint1 = {-4.5, -3.0, 1.5}; - Eigen::Vector3d waypoint2 = {-4.5, 3.0, 1.5}; - Eigen::Vector3d waypoint3 = {4.5, 0, 2.3}; - //固定的浮点显示 - cout.setf(ios::fixed); - // setprecision(n) 设显示小数精度为2位 - cout << setprecision(2); - //左对齐 - cout.setf(ios::left); - // 强制显示小数点 - cout.setf(ios::showpoint); - // 强制显示符号 - cout.setf(ios::showpos); - - //打印demo相关信息 - cout << GREEN << " [fly race demo " << TAIL << endl; - sleep(1); - cout << GREEN << " Level: [Advance] " << TAIL << endl; - sleep(1); - cout << GREEN << " Please use the RC SWA to armed, and the SWB to switch the drone to [COMMAND_CONTROL] mode " << TAIL << endl; - - // 四种状态机 - enum EXEC_STATE - { - TAKEOFF, - WAY1, - WAY2, - WAY3, - SEARCH, - SEARCH_MOVING, - TRACKING, - TRACKING_MOVING, - LOST, - RETURN, - }; - EXEC_STATE exec_state = TAKEOFF;//志位： detected 用作标志位 ture代表识别到目标 false代表丢失目标 - - while(ros::ok()) - { - //调用一次回调函数 - ros::spinOnce(); - // 等待进入COMMAND_CONTROL模式 - if (uav_control_state.control_state != prometheus_msgs::UAVControlState::COMMAND_CONTROL) - { - PCOUT(-1, TAIL, "Waiting for enter COMMAND_CONTROL state"); - continue; - } - // else{ - // uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Current_Pos_Hover; - // PCOUT(-1, TAIL, "hold for now"); - // } - - std::ostringstream info; - uav_command.header.stamp = ros::Time::now(); - switch (exec_state) - { - case TAKEOFF: - uav_command.header.frame_id = "ENU"; - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Init_Pos_Hover; - PCOUT(1, GREEN, "Go to takeoff point"); - // info << "height is : " << uav_state.position[2]; - // PCOUT(1, GREEN, info.str()); - { - if(fabs(uav_state.position[2] - takeoff_height) < 0.1) - { - PCOUT(1, GREEN, " UAV arrived at takeoff point"); - exec_state = WAY1; - } - } - break; - case WAY1: - uav_command.header.frame_id = "ENU"; - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS; - uav_command.position_ref[0] = waypoint1[0]; - uav_command.position_ref[1] = waypoint1[1]; - uav_command.position_ref[2] = waypoint1[2]; - uav_command.yaw_ref = 0.0; - PCOUT(1, GREEN, "Waypoint1 "); - // exec_state = SEARCH; - { - Eigen::Vector3d uav_pos = {uav_state.position[0], uav_state.position[1], uav_state.position[2]}; - float distance = (uav_pos - waypoint1).norm(); - if (distance < 0.1) - { - // sleep(10); - PCOUT(1, GREEN, " UAV arrived at waypoint1 point"); - exec_state = SEARCH; - count = 0; - } - } - break; - case WAY2: - uav_command.header.frame_id = "ENU"; - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS; - uav_command.position_ref[0] = waypoint2[0]; - uav_command.position_ref[1] = waypoint2[1]; - uav_command.position_ref[2] = waypoint2[2]; - uav_command.yaw_ref = 0.0; - PCOUT(1, GREEN, "Waypoint2 "); - { - Eigen::Vector3d uav_pos = {uav_state.position[0], uav_state.position[1], uav_state.position[2]}; - float distance = (uav_pos - waypoint2).norm(); - if (distance < 0.2) - { - sleep(10); - PCOUT(1, GREEN, " UAV arrived at waypoint2 point"); - exec_state = SEARCH; - count = 0; - } - } - break; - case WAY3: - uav_command.header.frame_id = "ENU"; - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS; - uav_command.position_ref[0] = waypoint3[0]; - uav_command.position_ref[1] = waypoint3[1]; - uav_command.position_ref[2] = waypoint3[2]; - uav_command.yaw_ref = 0.0; - PCOUT(1, GREEN, "Waypoint3 "); - { - Eigen::Vector3d uav_pos = {uav_state.position[0], uav_state.position[1], uav_state.position[2]}; - float distance = (uav_pos - waypoint3).norm(); - if (distance < 0.2) - { - sleep(10); - PCOUT(1, GREEN, " UAV arrived at waypoint3 point"); - exec_state = SEARCH_MOVING; - } - } - break; - case SEARCH: - // sleep(10); - //坐标系 - uav_command.header.frame_id = "ENU"; - // Move模式 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - // Move_mode - uav_command.Move_mode = prometheus_msgs::UAVCommand::XY_VEL_Z_POS; - //无人机按照圆形轨迹飞行 - - uav_command.velocity_ref[0] = -line_velocity * std::sin(count * angle_increment); - uav_command.velocity_ref[1] = line_velocity * std::cos(count * angle_increment); - uav_command.velocity_ref[2] = 0; - uav_command.position_ref[2] = 1.5; - //发布的命令ID加1 - //发布降落命令 - //计数器 - if(count > control_rate*circular_time) - { - circular_success = true; - count = 0; - } - count++; - info << "Waypoint Tracking > Velocity_x: " << uav_command.velocity_ref[0] << " Veloticy_y: " << uav_command.velocity_ref[1]<< " count: " << count; - PCOUT(1, GREEN, info.str()); - if(is_detected && circular_success) - { - exec_state = TRACKING; - PCOUT(1, GREEN, "tracking"); - loss_count = max_loss_count; - } - break; - case SEARCH_MOVING: - // sleep(10); - //坐标系 - uav_command.header.frame_id = "ENU"; - // Move模式 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - // Move_mode - uav_command.Move_mode = prometheus_msgs::UAVCommand::XY_VEL_Z_POS; - //无人机按照圆形轨迹飞行 - get_circular_property(31, 40, 5); - - uav_command.velocity_ref[0] = -line_velocity * std::sin(count * angle_increment); - uav_command.velocity_ref[1] = line_velocity * std::cos(count * angle_increment); - uav_command.velocity_ref[2] = 0; - uav_command.position_ref[2] = 2.3; - //发布的命令ID加1 - //发布降落命令 - //计数器 - if(count > control_rate*circular_time) - { - circular_success = true; - } - count++; - info << "Waypoint Tracking > Velocity_x: " << uav_command.velocity_ref[0] << " Veloticy_y: " << uav_command.velocity_ref[1]; - PCOUT(1, GREEN, info.str()); - if(is_detected && circular_success) - { - exec_state = TRACKING_MOVING; - PCOUT(1, GREEN, "tracking"); - loss_count = max_loss_count; - } - break; - case TRACKING: - if (!is_detected) - { - --loss_count; - if(loss_count < 0) - exec_state = RETURN; - PCOUT(0, YELLOW, "Return"); - } - //坐标系 - uav_command.header.frame_id = "BODY"; - // Move模式 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - // 机体系下的速度控制 - uav_command.Move_mode = prometheus_msgs::UAVCommand::XY_VEL_Z_POS_BODY; - uav_command.velocity_ref[0] = -0.9 * ellipse_det.sight_angle[0]; - uav_command.velocity_ref[1] = 0.9 * ellipse_det.sight_angle[1]; - uav_command.velocity_ref[2] = 0; - uav_command.position_ref[2] = 1.5; - uav_command.yaw_ref = 0.0; - info << "Find object,Go to the target point > velocity_x: " << uav_command.velocity_ref[0] << " [m/s] " - << "velocity_y: " << uav_command.velocity_ref[1] << " [m/s] " - << std::endl; - PCOUT(1, GREEN, info.str()); - if(is_holded && circular_success){ - if(uav_state.position[0] > -9 && uav_state.position[0] < 0) - if(uav_state.position[1] > -4 && uav_state.position[1] < 0) - exec_state = WAY2; - if(uav_state.position[0] > -9 && uav_state.position[0] < 0) - if(uav_state.position[1] > 0 && uav_state.position[1] < 4) - exec_state = WAY3; - } - - if (std::abs(uav_command.velocity_ref[0]) + std::abs(uav_command.velocity_ref[1]) < 0.03) - { - hold_count++; - hold_lost = 0; - } - else{ - hold_count = 0; - hold_lost++; - } - if(hold_lost > HOLD_THRES) - { - is_holded = false; - // PCOUT(1, GREEN, "no hold"); - } - if(hold_count > HOLD_THRES){ - is_holded = true; - // PCOUT(1, GREEN, "holded"); - } - break; - case TRACKING_MOVING: - if (!is_detected) - { - --loss_count; - if(loss_count < 0) - exec_state = RETURN; - PCOUT(0, YELLOW, "Return"); - } - //坐标系 - uav_command.header.frame_id = "BODY"; - // Move模式 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - // 机体系下的速度控制 - uav_command.Move_mode = prometheus_msgs::UAVCommand::XY_VEL_Z_POS_BODY; - uav_command.velocity_ref[0] = -2.5 * ellipse_det.sight_angle[0]; - uav_command.velocity_ref[1] = 2 * ellipse_det.sight_angle[1]; - uav_command.velocity_ref[2] = 0; - uav_command.position_ref[2] = 2.3; - uav_command.yaw_ref = 0.0; - info << "Find object,Go to the target point > velocity_x: " << uav_command.velocity_ref[0] << " [m/s] " - << "velocity_y: " << uav_command.velocity_ref[1] << " [m/s] " - << std::endl; - PCOUT(1, GREEN, info.str()); - // if(is_holded){ - // if(uav_state.position[0] > -9 && uav_state.position[0] < 0) - // if(uav_state.position[1] > -4 && uav_state.position[1] < 0) - // exec_state = WAY2; - // if(uav_state.position[0] > -9 && uav_state.position[0] < 0) - // if(uav_state.position[1] > 0 && uav_state.position[1] < 4) - // exec_state = WAY3; - // } - - if (std::abs(uav_command.velocity_ref[0]) + std::abs(uav_command.velocity_ref[1]) < 0.03) - { - hold_count++; - hold_lost = 0; - } - else{ - hold_count = 0; - hold_lost++; - } - if(hold_lost > HOLD_THRES) - { - is_holded = false; - // PCOUT(1, GREEN, "no hold"); - } - if(hold_count > HOLD_THRES){ - is_holded = true; - // PCOUT(1, GREEN, "holded"); - } - break; - case RETURN: - // return to home - uav_command.header.frame_id = "ENU"; - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS; - // uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Init_Pos_Hover; - uav_command.position_ref[0] = 0; - uav_command.position_ref[1] = 0; - uav_command.position_ref[2] = 1; - uav_command.yaw_ref = 0.0; - cout << GREEN << "return to home" << TAIL << endl; - - sleep(15); - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Land; - cout << GREEN << "landing" << TAIL << endl; - - break; - } - uav_command.Command_ID += 1; - uav_command_pub.publish(uav_command); - r.sleep(); - } - return 0; -} diff --git a/src/无人机端代码/Modules/motion_planning/CMakeLists.txt b/src/无人机端代码/Modules/motion_planning/CMakeLists.txt deleted file mode 100644 index 66dd650a..00000000 --- a/src/无人机端代码/Modules/motion_planning/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -/opt/ros/melodic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/global_planner/CMakeLists.txt b/src/无人机端代码/Modules/motion_planning/global_planner/CMakeLists.txt deleted file mode 100644 index 02c9edc3..00000000 --- a/src/无人机端代码/Modules/motion_planning/global_planner/CMakeLists.txt +++ /dev/null @@ -1,58 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(prometheus_global_planner) - -find_package(Eigen3 REQUIRED) -find_package(PCL 1.7 REQUIRED) -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - sensor_msgs - laser_geometry - geometry_msgs - nav_msgs - pcl_ros - visualization_msgs - gazebo_msgs - prometheus_msgs -) - - -catkin_package( - INCLUDE_DIRS include -) - -include_directories( - SYSTEM - include - ${PROJECT_SOURCE_DIR}/include - ${catkin_INCLUDE_DIRS} - ${Eigen3_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${PROJECT_SOURCE_DIR}/../../common/include - ) -link_directories(${PCL_LIBRARY_DIRS}) - -add_library(occupy_map src/occupy_map.cpp) -target_link_libraries(occupy_map ${PCL_LIBRARIES}) -add_library(A_star src/A_star.cpp) -target_link_libraries(A_star occupy_map) -add_library(global_planner src/global_planner.cpp) -target_link_libraries(global_planner A_star) - -add_executable(global_planner_main src/global_planner_node.cpp) -add_dependencies(global_planner_main prometheus_global_planner_gencpp) -target_link_libraries(global_planner_main ${catkin_LIBRARIES}) -target_link_libraries(global_planner_main ${PCL_LIBRARIES}) -target_link_libraries(global_planner_main global_planner) - -## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" -) - -## Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch -) \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/global_planner/global_planner.md b/src/无人机端代码/Modules/motion_planning/global_planner/global_planner.md deleted file mode 100644 index 71824e7e..00000000 --- a/src/无人机端代码/Modules/motion_planning/global_planner/global_planner.md +++ /dev/null @@ -1,78 +0,0 @@ -## global_planner - - - 使用gazebo环境获取感知信息还是通过map_generator? - - 使用PX4自带的动力学or fake_odom模块? - - - -#### 情况讨论 - - - 全局点云情况(测试) - - 真实场景对应:已知全局地图 - - map_generator生成全局点云 - - 无人机不需要搭载传感器 - - PX4动力学 & fake_odom模块 - - -启动仿真环境 -roslaunch prometheus_simulator_utils map_generator_with_fake_odom.launch - - - 依次拨动swb，切换值COMMAND_CONTROL模式，无人机会自动起飞至指定高度 - -启动规划算法 -roslaunch prometheus_global_planner sitl_global_planner_with_global_point.launch - - - 在RVIZ中选定目标点，或者使用终端发布rostopic pub /uav1/prometheus/motion_planning/goal xxx - - - - - 局部点云情况（todo） - - 对应真实情况:D435i等RGBD相机,三维激光雷达 - - map_generator生成全局点云,模拟得到局部点云信息 - - 无人机不需要搭载传感器 - - PX4动力学 & fake_odom模块 - - -启动仿真环境 -roslaunch prometheus_simulator_utils map_generator_with_fake_odom.launch - - - 依次拨动swb，切换值COMMAND_CONTROL模式，无人机会自动起飞至指定高度 - -启动规划算法 -roslaunch prometheus_global_planner sitl_global_planner_with_local_point.launch - - - 在RVIZ中选定目标点，或者使用终端发布rostopic pub /uav1/prometheus/motion_planning/goal xxx - - - - 2dlidar情况（todo） - - 对应真实情况:二维激光雷达 - - projector_.projectLaser(*local_point, input_laser_scan)将scan转换为点云,即对应回到局部点云情况 - - map_generator生成全局点云,模拟得到局部点云信息 - - 无人机不需要搭载传感器 - - PX4动力学 & fake_odom模块 - -启动仿真环境 -roslaunch prometheus_simulator_utils map_generator_with_fake_odom.launch - - - 依次拨动swb，切换值COMMAND_CONTROL模式，无人机会自动起飞至指定高度 - -启动规划算法 -roslaunch prometheus_global_planner sitl_global_planner_with_2dlidar.launch - - - 在RVIZ中选定目标点，或者使用终端发布rostopic pub /uav1/prometheus/motion_planning/goal xxx - - - - 多机情况（todo） - - 建议使用 全局点云情况 + 多个无人机 - - fake_odom模块 - -## wiki安排 - - 6.1 simulator_utils - 6.1.1 map_generator - 6.1.2 fake_odom - 6.2 motion_planning - 6.2.1 global_planner - 6.2.2 local_planner - 6.3 ego_planner - - diff --git a/src/无人机端代码/Modules/motion_planning/global_planner/include/A_star.h b/src/无人机端代码/Modules/motion_planning/global_planner/include/A_star.h deleted file mode 100644 index 87bded62..00000000 --- a/src/无人机端代码/Modules/motion_planning/global_planner/include/A_star.h +++ /dev/null @@ -1,181 +0,0 @@ -#ifndef _ASTAR_H -#define _ASTAR_H - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "occupy_map.h" -#include "printf_utils.h" -#include "global_planner_utils.h" - -using namespace std; - - - -#define IN_CLOSE_SET 'a' -#define IN_OPEN_SET 'b' -#define NOT_EXPAND 'c' -#define inf 1 >> 30 - - class Node - { - public: - /* -------------------- */ - Eigen::Vector3i index; - Eigen::Vector3d position; - double g_score, f_score; - Node *parent; - char node_state; - - double time; // dyn - int time_idx; - - Node() - { - parent = NULL; - node_state = NOT_EXPAND; - } - ~Node(){}; - }; - typedef Node *NodePtr; - - // 为什么这么麻烦，不能直接比较吗 - class NodeComparator0 - { - public: - bool operator()(NodePtr node1, NodePtr node2) - { - return node1->f_score > node2->f_score; - } - }; - - template - struct matrix_hash0 : std::unary_function - { - std::size_t operator()(T const &matrix) const - { - size_t seed = 0; - for (size_t i = 0; i < matrix.size(); ++i) - { - auto elem = *(matrix.data() + i); - seed ^= std::hash()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2); - } - return seed; - } - }; - - class NodeHashTable0 - { - private: - /* data */ - std::unordered_map> data_3d_; - - public: - NodeHashTable0(/* args */) - { - } - ~NodeHashTable0() - { - } - void insert(Eigen::Vector3i idx, NodePtr node) - { - data_3d_.insert(std::make_pair(idx, node)); - } - - NodePtr find(Eigen::Vector3i idx) - { - auto iter = data_3d_.find(idx); - return iter == data_3d_.end() ? NULL : iter->second; - } - - void clear() - { - data_3d_.clear(); - } - }; - - class Astar - { - private: - // 备选路径点指针容器 - std::vector path_node_pool_; - // 使用节点计数器、迭代次数计数器 - int use_node_num_, iter_num_; - // 扩展的节点 - NodeHashTable0 expanded_nodes_; - // open set （根据规则已排序好） - std::priority_queue, NodeComparator0> open_set_; - // 最终路径点容器 - std::vector path_nodes_; - - // 参数 - // 启发式参数 - double lambda_heu_; - double lambda_cost_; - // 最大搜索次数 - int max_search_num; - // tie breaker - double tie_breaker_; - bool is_2D; - double fly_height; - - // 目标点 - Eigen::Vector3d goal_pos; - - // 地图相关 - std::vector occupancy_buffer_; - double resolution_, inv_resolution_; - Eigen::Vector3d origin_, map_size_3d_; - - // 辅助函数 - Eigen::Vector3i posToIndex(Eigen::Vector3d pt); - void indexToPos(Eigen::Vector3i id, Eigen::Vector3d &pos); - void retrievePath(NodePtr end_node); - - // 搜索启发函数，三种形式，选用其中一种即可 - double getDiagHeu(Eigen::Vector3d x1, Eigen::Vector3d x2); - double getManhHeu(Eigen::Vector3d x1, Eigen::Vector3d x2); - double getEuclHeu(Eigen::Vector3d x1, Eigen::Vector3d x2); - - public: - Astar(){} - ~Astar(); - - enum - { - REACH_END = 1, - NO_PATH = 2 - }; - - // 占据图类 - Occupy_map::Ptr Occupy_map_ptr; - - // 重置 - void reset(); - // 初始化 - void init(ros::NodeHandle &nh); - // 检查安全性 - bool check_safety(Eigen::Vector3d &cur_pos, double safe_distance); - // 搜索 - int search(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt); - // 返回路径 - std::vector getPath(); - // 返回ros消息格式的路径 - nav_msgs::Path get_ros_path(); - // 返回访问过的节点 - std::vector getVisitedNodes(); - - typedef std::shared_ptr Ptr; - }; - - - -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/global_planner/include/global_planner.h b/src/无人机端代码/Modules/motion_planning/global_planner/include/global_planner.h deleted file mode 100644 index 2d21b150..00000000 --- a/src/无人机端代码/Modules/motion_planning/global_planner/include/global_planner.h +++ /dev/null @@ -1,121 +0,0 @@ -#ifndef GLOBAL_PLANNER -#define GLOBAL_PLANNER - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "A_star.h" -#include "occupy_map.h" -#include "printf_utils.h" -#include "global_planner_utils.h" -using namespace std; - - - class GlobalPlanner - { - public: - GlobalPlanner(ros::NodeHandle &nh); - ros::NodeHandle global_planner_nh; - - private: - // 参数 - int uav_id; - bool sim_mode; - int map_input_source; - double fly_height; - double safe_distance; - double time_per_path; - double replan_time; - bool consider_neighbour; - string global_pcl_topic_name, local_pcl_topic_name, scan_topic_name; - - // 订阅无人机状态、目标点、传感器数据（生成地图） - ros::Subscriber goal_sub; - ros::Subscriber uav_state_sub; - // 支持2维激光雷达、3维激光雷达、D435i等实体传感器 - // 支持直接输入全局已知点云 - ros::Subscriber Gpointcloud_sub; - ros::Subscriber Lpointcloud_sub; - ros::Subscriber laserscan_sub; - ros::Subscriber uav_control_state_sub; - // 发布控制指令 - ros::Publisher uav_cmd_pub; - ros::Publisher path_cmd_pub; - ros::Timer mainloop_timer; - ros::Timer track_path_timer; - ros::Timer safety_timer; - // A星规划器 - Astar::Ptr Astar_ptr; - - // laserscan2pointcloud2 投影器 - laser_geometry::LaserProjection projector_; - - prometheus_msgs::UAVState uav_state; // 无人机状态 - prometheus_msgs::UAVControlState uav_control_state; - nav_msgs::Odometry uav_odom; - Eigen::Vector3d uav_pos; // 无人机位置 - Eigen::Vector3d uav_vel; // 无人机速度 - Eigen::Quaterniond uav_quat; // 无人机四元数 - double uav_yaw; - // 规划终端状态 - Eigen::Vector3d goal_pos, goal_vel; - prometheus_msgs::UAVCommand uav_command; - nav_msgs::Path path_cmd; - double distance_to_goal; - - // 规划器状态 - bool odom_ready; - bool drone_ready; - bool sensor_ready; - bool goal_ready; - bool is_safety; - bool is_new_path; - bool path_ok; - int start_point_index; - int Num_total_wp; - int cur_id; - float desired_yaw; - - ros::Time last_replan_time; - - // 五种状态机 - enum EXEC_STATE - { - WAIT_GOAL, - PLANNING, - TRACKING, - LANDING, - }; - EXEC_STATE exec_state; - - // 回调函数 - void goal_cb(const geometry_msgs::PoseStampedConstPtr &msg); - void uav_state_cb(const prometheus_msgs::UAVStateConstPtr &msg); - void Gpointcloud_cb(const sensor_msgs::PointCloud2ConstPtr &msg); - void Lpointcloud_cb(const sensor_msgs::PointCloud2ConstPtr &msg); - void laser_cb(const sensor_msgs::LaserScanConstPtr &msg); - void uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg); - void safety_cb(const ros::TimerEvent &e); - void mainloop_cb(const ros::TimerEvent &e); - void track_path_cb(const ros::TimerEvent &e); - void debug_info(); - int get_start_point_id(void); - }; - - - -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/global_planner/include/global_planner_utils.h b/src/无人机端代码/Modules/motion_planning/global_planner/include/global_planner_utils.h deleted file mode 100644 index 6a4c455f..00000000 --- a/src/无人机端代码/Modules/motion_planning/global_planner/include/global_planner_utils.h +++ /dev/null @@ -1,8 +0,0 @@ -#ifndef GLOBAL_PLANNER_UTILS_H -#define GLOBAL_PLANNER_UTILS_H - -#define NODE_NAME "GlobalPlanner [Astar] " -#define MIN_DIS 0.1 - -#endif - diff --git a/src/无人机端代码/Modules/motion_planning/global_planner/include/occupy_map.h b/src/无人机端代码/Modules/motion_planning/global_planner/include/occupy_map.h deleted file mode 100644 index eaff87f9..00000000 --- a/src/无人机端代码/Modules/motion_planning/global_planner/include/occupy_map.h +++ /dev/null @@ -1,128 +0,0 @@ -#ifndef _OCCUPY_MAP_H -#define _OCCUPY_MAP_H - -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "printf_utils.h" -#include "global_planner_utils.h" -using namespace std; - - - class Occupy_map - { - public: - Occupy_map(){} - double fly_height; - - // 局部地图 滑窗 存储器 - map> point_cloud_pair; - // 全局地图点云指针 - pcl::PointCloud::Ptr global_point_cloud_map; - // 全局膨胀点云指针 - pcl::PointCloud::Ptr global_uav_pcl; - // 考虑变指针 - pcl::PointCloud::Ptr cloud_inflate_vis_; - // 临时指针 - pcl::PointCloud::Ptr input_point_cloud; - pcl::PointCloud::Ptr transformed_cloud; - pcl::PointCloud::Ptr pcl_ptr; - // 地图边界点云 - pcl::PointCloud border; - // VoxelGrid过滤器用于下采样 - pcl::VoxelGrid vg; - // OutlierRemoval用于去除离群值 - pcl::StatisticalOutlierRemoval sor; - // laserscan2pointcloud2 中间变量 - sensor_msgs::PointCloud2 input_laser_scan; - // laserscan2pointcloud2 投影器 - laser_geometry::LaserProjection projector_; - // 上一帧odom - double f_x, f_y, f_z, f_roll, f_pitch, f_yaw; - // 局部地图滑窗，指示器以及大小 - int st_it, queue_size; - // flag：展示地图边界 - bool show_border; - bool sim_mode; - // 地图是否占据容器， 从编程角度来讲，这就是地图变为单一序列化后的索引 - std::vector occupancy_buffer_; // 0 is free, 1 is occupied - // 代价地图 - std::vector cost_map_; // cost - // 地图分辨率 - double resolution_, inv_resolution_; - string node_name; - // 膨胀参数 - double inflate_; - double odom_inflate_; - // 地图原点,地图尺寸 - Eigen::Vector3d origin_, map_size_3d_, min_range_, max_range_; - // 占据图尺寸 = 地图尺寸 / 分辨率 - Eigen::Vector3i grid_size_; - int swarm_num_uav; // 集群数量 - string uav_name; // 无人机名字 - int uav_id; // 无人机编号 - bool get_gpcl, get_lpcl, get_laser; - Eigen::Vector3d enum_p[100], enum_p_uav[1000], enum_p_cost[1000]; - int ifn; - int inflate_index, inflate_index_uav, cost_index, cost_inflate; - // 发布点云用于rviz显示 - ros::Publisher global_pcl_pub, inflate_pcl_pub; - ros::Timer pcl_pub; - - //初始化 - void init(ros::NodeHandle &nh); - // 地图更新函数 - 输入：全局点云 - void map_update_gpcl(const sensor_msgs::PointCloud2ConstPtr &global_point); - // 工具函数：合并局部地图 - void local_map_merge_odom(const nav_msgs::Odometry &odom); - void uav_pcl_update(Eigen::Vector3d *input_uav_odom, bool *get_uav_odom); - // 地图更新函数 - 输入：局部点云 - void map_update_lpcl(const sensor_msgs::PointCloud2ConstPtr &local_point, const nav_msgs::Odometry &odom); - // 地图更新函数 - 输入：二维激光雷达 - void map_update_laser(const sensor_msgs::LaserScanConstPtr &local_point, const nav_msgs::Odometry &odom); - // 地图膨胀 - void inflate_point_cloud(void); - // 判断当前点是否在地图内 - bool isInMap(Eigen::Vector3d pos); - // 设置占据 - void setOccupancy(Eigen::Vector3d &pos, int occ); - // 设置代价 - void updateCostMap(Eigen::Vector3d &pos, double cost); - // 由位置计算索引 - void posToIndex(Eigen::Vector3d &pos, Eigen::Vector3i &id); - // 由索引计算位置 - void indexToPos(Eigen::Vector3i &id, Eigen::Vector3d &pos); - // 根据位置返回占据状态 - int getOccupancy(Eigen::Vector3d &pos); - // 根据索引返回占据状态 - int getOccupancy(Eigen::Vector3i &id); - // 根据索引返回代价 - double getCost(Eigen::Vector3d &pos); - // 检查安全 - bool check_safety(Eigen::Vector3d &pos, double check_distance /*, Eigen::Vector3d& map_point*/); - void pub_pcl_cb(const ros::TimerEvent &e); - // 定义该类的指针 - typedef std::shared_ptr Ptr; - }; - - - -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_2dlidar.launch b/src/无人机端代码/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_2dlidar.launch deleted file mode 100644 index ffb24872..00000000 --- a/src/无人机端代码/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_2dlidar.launch +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_global_point.launch b/src/无人机端代码/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_global_point.launch deleted file mode 100644 index 29e7a507..00000000 --- a/src/无人机端代码/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_global_point.launch +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_local_point.launch b/src/无人机端代码/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_local_point.launch deleted file mode 100644 index 12b24557..00000000 --- a/src/无人机端代码/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_local_point.launch +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/global_planner/package.xml b/src/无人机端代码/Modules/motion_planning/global_planner/package.xml deleted file mode 100644 index d3f79c05..00000000 --- a/src/无人机端代码/Modules/motion_planning/global_planner/package.xml +++ /dev/null @@ -1,40 +0,0 @@ - - - prometheus_global_planner - 0.0.0 - The prometheus_global_planner package - - Yuhua Qi - - TODO - - catkin - roscpp - rospy - std_msgs - sensor_msgs - geometry_msgs - nav_msgs - prometheus_msgs - pcl_ros - roscpp - rospy - std_msgs - sensor_msgs - geometry_msgs - nav_msgs - prometheus_msgs - pcl_ros - roscpp - rospy - std_msgs - sensor_msgs - geometry_msgs - nav_msgs - prometheus_msgs - pcl_ros - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/global_planner/src/A_star.cpp b/src/无人机端代码/Modules/motion_planning/global_planner/src/A_star.cpp deleted file mode 100644 index d960c503..00000000 --- a/src/无人机端代码/Modules/motion_planning/global_planner/src/A_star.cpp +++ /dev/null @@ -1,367 +0,0 @@ -#include "A_star.h" - -using namespace std; -using namespace Eigen; - -Astar::~Astar() -{ - for (int i = 0; i < max_search_num; i++) - { - // delete表示释放堆内存 - delete path_node_pool_[i]; - } -} - -void Astar::init(ros::NodeHandle &nh) -{ - // 【参数】2d参数 - nh.param("global_planner/is_2D", is_2D, true); // 1代表2D平面规划及搜索,0代表3D - nh.param("global_planner/fly_height", fly_height, 1.0); // 2D规划时,定高高度 - // 【参数】规划搜索相关参数 - nh.param("astar/lambda_heu", lambda_heu_, 2.0); // 加速引导参数 - nh.param("astar/lambda_cost", lambda_cost_, 300.0); // 参数 - nh.param("astar/allocate_num", max_search_num, 500000); //最大搜索节点数 - nh.param("map/resolution", resolution_, 0.2); // 地图分辨率 - - tie_breaker_ = 1.0 + 1.0 / max_search_num; - - this->inv_resolution_ = 1.0 / resolution_; - - path_node_pool_.resize(max_search_num); - - // 新建 - for (int i = 0; i < max_search_num; i++) - { - path_node_pool_[i] = new Node; - } - - use_node_num_ = 0; - iter_num_ = 0; - - // 初始化占据地图 - Occupy_map_ptr.reset(new Occupy_map); - Occupy_map_ptr->init(nh); - - // 读取地图参数 - origin_ = Occupy_map_ptr->min_range_; - map_size_3d_ = Occupy_map_ptr->max_range_ - Occupy_map_ptr->min_range_; -} - -void Astar::reset() -{ - // 重置与搜索相关的变量 - expanded_nodes_.clear(); - path_nodes_.clear(); - - std::priority_queue, NodeComparator0> empty_queue; - open_set_.swap(empty_queue); - - for (int i = 0; i < use_node_num_; i++) - { - NodePtr node = path_node_pool_[i]; - node->parent = NULL; - node->node_state = NOT_EXPAND; - } - - use_node_num_ = 0; - iter_num_ = 0; -} - -// 搜索函数，输入为：起始点及终点 -// 将传输的数组通通变为指针！！！！ 以后改 -int Astar::search(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt) -{ - // 首先检查目标点是否可到达 - if (Occupy_map_ptr->getOccupancy(end_pt)) - { - cout << RED << "Astar search: [ Astar can't find path: goal point is occupied ]" << TAIL << endl; - return NO_PATH; - } - - // 计时 - ros::Time time_astar_start = ros::Time::now(); - - goal_pos = end_pt; - Eigen::Vector3i end_index = posToIndex(end_pt); - - // 初始化,将起始点设为第一个路径点 - NodePtr cur_node = path_node_pool_[0]; - cur_node->parent = NULL; - cur_node->position = start_pt; - cur_node->index = posToIndex(start_pt); - cur_node->g_score = 0.0; - cur_node->f_score = lambda_heu_ * getEuclHeu(cur_node->position, end_pt); - cur_node->node_state = IN_OPEN_SET; - - // 将当前点推入open set - open_set_.push(cur_node); - // 迭代次数+1 - use_node_num_ += 1; - // 记录当前为已扩展 - expanded_nodes_.insert(cur_node->index, cur_node); - - NodePtr terminate_node = NULL; - - // 搜索主循环 - while (!open_set_.empty()) - { - // 获取f_score最低的点 - cur_node = open_set_.top(); - - // 判断终止条件 - bool reach_end = abs(cur_node->index(0) - end_index(0)) <= 1 && - abs(cur_node->index(1) - end_index(1)) <= 1 && - abs(cur_node->index(2) - end_index(2)) <= 1; - - if (reach_end) - { - // 将当前点设为终止点，并往回形成路径 - terminate_node = cur_node; - retrievePath(terminate_node); - - // 时间一般很短，远远小于膨胀点云的时间 - // printf("Astar take time %f s. \n", (ros::Time::now() - time_astar_start).toSec()); - - return REACH_END; - } - - /* ---------- pop node and add to close set ---------- */ - open_set_.pop(); - // 将当前点推入close set - cur_node->node_state = IN_CLOSE_SET; // in expand set - iter_num_ += 1; - - /* ---------- init neighbor expansion ---------- */ - Eigen::Vector3d cur_pos = cur_node->position; - Eigen::Vector3d expand_node_pos; - - vector inputs; - Eigen::Vector3d d_pos; - - /* ---------- expansion loop ---------- */ - // 扩展： 3*3*3 - 1 = 26种可能 - for (double dx = -resolution_; dx <= resolution_ + 1e-3; dx += resolution_) - { - for (double dy = -resolution_; dy <= resolution_ + 1e-3; dy += resolution_) - { - for (double dz = -resolution_; dz <= resolution_ + 1e-3; dz += resolution_) - { - - d_pos << dx, dy, dz; - - // 对于2d情况，不扩展z轴 - if (is_2D) - { - d_pos(2) = 0.0; - } - - // 跳过自己那个格子 - if (d_pos.norm() < 1e-3) - { - continue; - } - - // 扩展节点的位置 - expand_node_pos = cur_pos + d_pos; - - // 确认该点在地图范围内 - if (!Occupy_map_ptr->isInMap(expand_node_pos)) - { - continue; - } - - // 计算扩展节点的index - Eigen::Vector3i d_pos_id; - d_pos_id << int(dx / resolution_), int(dy / resolution_), int(dz / resolution_); - Eigen::Vector3i expand_node_id = d_pos_id + cur_node->index; - - //检查当前扩展的点是否在close set中，如果是则跳过 - NodePtr expand_node = expanded_nodes_.find(expand_node_id); - if (expand_node != NULL && expand_node->node_state == IN_CLOSE_SET) - { - continue; - } - - // 检查当前扩展点是否被占据,如果是则跳过 - bool is_occupy = Occupy_map_ptr->getOccupancy(expand_node_pos); - if (is_occupy) - { - continue; - } - - // 如果能通过上述检查则 - double tmp_g_score, tmp_f_score; - tmp_g_score = d_pos.squaredNorm() + cur_node->g_score; - tmp_f_score = tmp_g_score + lambda_heu_ * getEuclHeu(expand_node_pos, end_pt) + lambda_cost_ * Occupy_map_ptr->getCost(expand_node_pos); - - // 如果扩展的当前节点为NULL，即未扩展过 - if (expand_node == NULL) - { - expand_node = path_node_pool_[use_node_num_]; - expand_node->index = expand_node_id; - expand_node->position = expand_node_pos; - expand_node->f_score = tmp_f_score; - expand_node->g_score = tmp_g_score; - expand_node->parent = cur_node; - expand_node->node_state = IN_OPEN_SET; - - open_set_.push(expand_node); - expanded_nodes_.insert(expand_node_id, expand_node); - - use_node_num_ += 1; - // 超过最大搜索次数 - if (use_node_num_ == max_search_num) - { - cout << RED << NODE_NAME << "Astar search: [ Astar can't find path: reach the max_search_num ]" << TAIL << endl; - return NO_PATH; - } - } - // 如果当前节点已被扩展过，则更新其状态 - else if (expand_node->node_state == IN_OPEN_SET) - { - if (tmp_g_score < expand_node->g_score) - { - // expand_node->index = expand_node_id; - expand_node->position = expand_node_pos; - expand_node->f_score = tmp_f_score; - expand_node->g_score = tmp_g_score; - expand_node->parent = cur_node; - } - } - } - } - } - } - - // 搜索完所有可行点，即使没达到最大搜索次数，也没有找到路径 - // 这种一般是因为无人机周围被占据，或者无人机与目标点之间无可通行路径造成的 - cout << RED << "Astar search: [ Astar can't find path: max_search_num: open set empty ]" << TAIL << endl; - return NO_PATH; -} - -// 由最终点往回生成路径 -void Astar::retrievePath(NodePtr end_node) -{ - NodePtr cur_node = end_node; - path_nodes_.push_back(cur_node); - - while (cur_node->parent != NULL) - { - cur_node = cur_node->parent; - path_nodes_.push_back(cur_node); - } - - // 反转顺序 - reverse(path_nodes_.begin(), path_nodes_.end()); -} - -std::vector Astar::getPath() -{ - vector path; - for (uint i = 0; i < path_nodes_.size(); ++i) - { - path.push_back(path_nodes_[i]->position); - } - path.push_back(goal_pos); - return path; -} - -nav_msgs::Path Astar::get_ros_path() -{ - nav_msgs::Path path; - - path.header.frame_id = "world"; - path.header.stamp = ros::Time::now(); - path.poses.clear(); - - geometry_msgs::PoseStamped path_i_pose; - for (uint i = 0; i < path_nodes_.size(); ++i) - { - path_i_pose.header.frame_id = "world"; - path_i_pose.pose.position.x = path_nodes_[i]->position[0]; - path_i_pose.pose.position.y = path_nodes_[i]->position[1]; - path_i_pose.pose.position.z = path_nodes_[i]->position[2]; - path.poses.push_back(path_i_pose); - } - - path_i_pose.header.frame_id = "world"; - path_i_pose.pose.position.x = goal_pos[0]; - path_i_pose.pose.position.y = goal_pos[1]; - path_i_pose.pose.position.z = goal_pos[2]; - path.poses.push_back(path_i_pose); - - return path; -} - -double Astar::getDiagHeu(Eigen::Vector3d x1, Eigen::Vector3d x2) -{ - double dx = fabs(x1(0) - x2(0)); - double dy = fabs(x1(1) - x2(1)); - double dz = fabs(x1(2) - x2(2)); - - double h = 0; - - int diag = min(min(dx, dy), dz); - dx -= diag; - dy -= diag; - dz -= diag; - - if (dx < 1e-4) - { - h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dy, dz) + 1.0 * abs(dy - dz); - } - if (dy < 1e-4) - { - h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dz) + 1.0 * abs(dx - dz); - } - if (dz < 1e-4) - { - h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dy) + 1.0 * abs(dx - dy); - } - - return tie_breaker_ * h; -} - -double Astar::getManhHeu(Eigen::Vector3d x1, Eigen::Vector3d x2) -{ - double dx = fabs(x1(0) - x2(0)); - double dy = fabs(x1(1) - x2(1)); - double dz = fabs(x1(2) - x2(2)); - - return tie_breaker_ * (dx + dy + dz); -} - -double Astar::getEuclHeu(Eigen::Vector3d x1, Eigen::Vector3d x2) -{ - return tie_breaker_ * (x2 - x1).norm(); -} - -std::vector Astar::getVisitedNodes() -{ - vector visited; - visited.assign(path_node_pool_.begin(), path_node_pool_.begin() + use_node_num_ - 1); - return visited; -} - -Eigen::Vector3i Astar::posToIndex(Eigen::Vector3d pt) -{ - Vector3i idx; - idx << floor((pt(0) - origin_(0)) * inv_resolution_), floor((pt(1) - origin_(1)) * inv_resolution_), - floor((pt(2) - origin_(2)) * inv_resolution_); - - return idx; -} - -void Astar::indexToPos(Eigen::Vector3i id, Eigen::Vector3d &pos) -{ - for (int i = 0; i < 3; ++i) - pos(i) = (id(i) + 0.5) * resolution_ + origin_(i); -} - -// 检查cur_pos是否安全 -bool Astar::check_safety(Eigen::Vector3d &cur_pos, double safe_distance) -{ - bool is_safety; - is_safety = Occupy_map_ptr->check_safety(cur_pos, safe_distance); - return is_safety; -} diff --git a/src/无人机端代码/Modules/motion_planning/global_planner/src/global_planner.cpp b/src/无人机端代码/Modules/motion_planning/global_planner/src/global_planner.cpp deleted file mode 100644 index 35359111..00000000 --- a/src/无人机端代码/Modules/motion_planning/global_planner/src/global_planner.cpp +++ /dev/null @@ -1,459 +0,0 @@ -#include "global_planner.h" - -// 初始化函数 -GlobalPlanner::GlobalPlanner(ros::NodeHandle &nh) -{ - // 【参数】无人机编号，从1开始编号 - nh.param("uav_id", uav_id, 0); - // 【参数】是否为仿真模式 - nh.param("global_planner/sim_mode", sim_mode, false); - // 【参数】选择地图更新方式：　0代表全局点云，1代表局部点云，2代表激光雷达scan数据 - nh.param("global_planner/map_input_source", map_input_source, 0); - // 【参数】无人机飞行高度 - nh.param("global_planner/fly_height", fly_height, 1.0); - // 【参数】安全距离，若膨胀距离设置已考虑安全距离，建议此处设为0 - nh.param("global_planner/safe_distance", safe_distance, 0.05); - // 【参数】路径追踪频率 - nh.param("global_planner/time_per_path", time_per_path, 1.0); - // 【参数】Astar重规划频率 - nh.param("global_planner/replan_time", replan_time, 2.0); - - //【订阅】目标点 - goal_sub = nh.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/motion_planning/goal", - 1, - &GlobalPlanner::goal_cb, this); - - //【订阅】无人机状态信息 - uav_state_sub = nh.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/state", - 1, - &GlobalPlanner::uav_state_cb, this); - - uav_control_state_sub = nh.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/control_state", - 1, - &GlobalPlanner::uav_control_state_cb, this); - - string uav_name = "/uav" + std::to_string(uav_id); - //【订阅】根据map_input_source选择地图更新方式 - if (map_input_source == 0) - { - nh.getParam("global_planner/global_pcl_topic_name", global_pcl_topic_name); - cout << GREEN << "Global pcl mode, subscirbe to " << global_pcl_topic_name << TAIL << endl; - Gpointcloud_sub = nh.subscribe(global_pcl_topic_name.c_str(), 1, &GlobalPlanner::Gpointcloud_cb, this); - } - else if (map_input_source == 1) - { - nh.getParam("global_planner/local_pcl_topic_name", local_pcl_topic_name); - cout << GREEN << "Local pcl mode, subscirbe to " << uav_name << local_pcl_topic_name << TAIL << endl; - Lpointcloud_sub = nh.subscribe("/uav" + std::to_string(uav_id) + local_pcl_topic_name, 1, &GlobalPlanner::Lpointcloud_cb, this); - } - else if (map_input_source == 2) - { - nh.getParam("global_planner/scan_topic_name", scan_topic_name); - cout << GREEN << "Laser scan mode, subscirbe to " << uav_name << scan_topic_name << TAIL << endl; - laserscan_sub = nh.subscribe("/uav" + std::to_string(uav_id) + scan_topic_name, 1, &GlobalPlanner::laser_cb, this); - } - - // 【发布】控制指令 - uav_cmd_pub = nh.advertise("/uav" + std::to_string(uav_id) + "/prometheus/command", 1); - // 【发布】发布路径用于显示 - path_cmd_pub = nh.advertise("/uav" + std::to_string(uav_id) + "/prometheus/global_planner/path_cmd", 1); - // 【定时器】安全检测 - // safety_timer = nh.createTimer(ros::Duration(2.0), &GlobalPlanner::safety_cb, this); - // 【定时器】主循环 - mainloop_timer = nh.createTimer(ros::Duration(1.0), &GlobalPlanner::mainloop_cb, this); - // 【定时器】路径追踪循环，快速移动场景应当适当提高执行频率 - track_path_timer = nh.createTimer(ros::Duration(time_per_path), &GlobalPlanner::track_path_cb, this); - - // 【初始化】Astar算法 - Astar_ptr.reset(new Astar); - Astar_ptr->init(nh); - cout << GREEN << NODE_NAME << "A_star init. " << TAIL << endl; - - // 规划器状态参数初始化 - exec_state = EXEC_STATE::WAIT_GOAL; - odom_ready = false; - drone_ready = false; - goal_ready = false; - sensor_ready = false; - is_safety = true; - is_new_path = false; - - // 初始化发布的指令 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Init_Pos_Hover; - uav_command.position_ref[0] = 0; - uav_command.position_ref[1] = 0; - uav_command.position_ref[2] = 0; - uav_command.yaw_ref = 0; - desired_yaw = 0.0; -} -void GlobalPlanner::debug_info() -{ - //固定的浮点显示 - cout.setf(ios::fixed); - // setprecision(n) 设显示小数精度为n位 - cout << setprecision(2); - //左对齐 - cout.setf(ios::left); - // 强制显示小数点 - cout.setf(ios::showpoint); - // 强制显示符号 - cout.setf(ios::showpos); - cout << GREEN << "--------------> Global Planner <------------- " << TAIL << endl; - cout << GREEN << "[ ID: " << uav_id << "] " << TAIL; - if (drone_ready) - { - cout << GREEN << "[ drone ready ] " << TAIL << endl; - } - else - { - cout << RED << "[ drone not ready ] " << TAIL << endl; - } - - if (odom_ready) - { - cout << GREEN << "[ odom ready ] " << TAIL << endl; - } - else - { - cout << RED << "[ odom not ready ] " << TAIL << endl; - } - - if (sensor_ready) - { - cout << GREEN << "[ sensor ready ] " << TAIL << endl; - } - else - { - cout << RED << "[ sensor not ready ] " << TAIL << endl; - } - - if (exec_state == EXEC_STATE::WAIT_GOAL) - { - cout << GREEN << "[ WAIT_GOAL ] " << TAIL << endl; - if (uav_control_state.control_state != prometheus_msgs::UAVControlState::COMMAND_CONTROL) - { - cout << YELLOW << "Please switch to COMMAND_CONTROL mode." << TAIL << endl; - } - if (!goal_ready) - { - cout << YELLOW << "Waiting for a new goal." << TAIL << endl; - } - } - else if (exec_state == EXEC_STATE::PLANNING) - { - cout << GREEN << "[ PLANNING ] " << TAIL << endl; - } - else if (exec_state == EXEC_STATE::TRACKING) - { - cout << GREEN << "[ TRACKING ] " << TAIL << endl; - cout << GREEN << "---->distance_to_goal:" << distance_to_goal << TAIL << endl; - } - else if (exec_state == EXEC_STATE::LANDING) - { - cout << GREEN << "[ LANDING ] " << TAIL << endl; - } -} -// 主循环 -void GlobalPlanner::mainloop_cb(const ros::TimerEvent &e) -{ - static int exec_num = 0; - exec_num++; - - if (exec_num == 5) - { - debug_info(); - exec_num = 0; - } - - // 检查当前状态，不满足规划条件则直接退出主循环 - if (!odom_ready || !drone_ready || !sensor_ready) - { - return; - } - else - { - // 对检查的状态进行重置 - odom_ready = false; - drone_ready = false; - sensor_ready = false; - } - - switch (exec_state) - { - case EXEC_STATE::WAIT_GOAL: - path_ok = false; - - // 保持到指定高度 - if (abs(fly_height - uav_pos[2]) > MIN_DIS) - { - uav_command.header.stamp = ros::Time::now(); - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS; - uav_command.position_ref[0] = uav_pos[0]; - uav_command.position_ref[1] = uav_pos[1]; - uav_command.position_ref[2] = fly_height; - uav_command.yaw_ref = 0; - uav_command.Command_ID = uav_command.Command_ID + 1; - uav_cmd_pub.publish(uav_command); - } - else if (goal_ready) - { - // 获取到目标点后，生成新轨迹 - exec_state = EXEC_STATE::PLANNING; - goal_ready = false; - } - - break; - case EXEC_STATE::PLANNING: - // 重置规划器 - Astar_ptr->reset(); - // 使用规划器执行搜索，返回搜索结果 - int astar_state; - astar_state = Astar_ptr->search(uav_pos, goal_pos); - - // 未寻找到路径 - if (astar_state == Astar::NO_PATH) - { - path_ok = false; - exec_state = EXEC_STATE::WAIT_GOAL; - cout << RED << NODE_NAME << " Planner can't find path!" << TAIL << endl; - } - else - { - path_ok = true; - is_new_path = true; - path_cmd = Astar_ptr->get_ros_path(); - Num_total_wp = path_cmd.poses.size(); - start_point_index = get_start_point_id(); - cur_id = start_point_index; - last_replan_time = ros::Time::now(); - exec_state = EXEC_STATE::TRACKING; - path_cmd_pub.publish(path_cmd); - cout << GREEN << NODE_NAME << " Get a new path!" << TAIL << endl; - } - - break; - - case EXEC_STATE::TRACKING: - { - if ( (ros::Time::now()-last_replan_time).toSec() >= replan_time) - { - exec_state = EXEC_STATE::PLANNING; - } - - break; - } - case EXEC_STATE::LANDING: - { - uav_command.header.stamp = ros::Time::now(); - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Land; - uav_command.Command_ID = uav_command.Command_ID + 1; - - uav_cmd_pub.publish(uav_command); - break; - } - } -} - -void GlobalPlanner::goal_cb(const geometry_msgs::PoseStampedConstPtr &msg) -{ - // 2D定高飞行 - goal_pos << msg->pose.position.x, msg->pose.position.y, fly_height; - goal_vel.setZero(); - goal_ready = true; - exec_state = EXEC_STATE::WAIT_GOAL; - - cout << GREEN << NODE_NAME << " Get a new goal point:" << goal_pos(0) << " [m] " << goal_pos(1) << " [m] " << goal_pos(2) << " [m] " << TAIL << endl; - - if (goal_pos(0) == 99 && goal_pos(1) == 99) - { - path_ok = false; - goal_ready = false; - exec_state = EXEC_STATE::LANDING; - cout << GREEN << NODE_NAME << " Land " << TAIL << endl; - } -} - -//无人机控制状态回调函数 -void GlobalPlanner::uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg) -{ - uav_control_state = *msg; -} - -void GlobalPlanner::uav_state_cb(const prometheus_msgs::UAVState::ConstPtr &msg) -{ - uav_state = *msg; - - if (uav_state.connected == true && uav_state.armed == true) - { - drone_ready = true; - } - else - { - drone_ready = false; - } - - if (uav_state.odom_valid) - { - odom_ready = true; - } - else - { - odom_ready = false; - } - - if (abs(fly_height - msg->position[2]) > 0.2) - { - PCOUT(2, YELLOW, "UAV is not in the desired height."); - } - - uav_odom.header = uav_state.header; - uav_odom.child_frame_id = "base_link"; - uav_odom.pose.pose.position.x = uav_state.position[0]; - uav_odom.pose.pose.position.y = uav_state.position[1]; - uav_odom.pose.pose.position.z = fly_height; - uav_odom.pose.pose.orientation = uav_state.attitude_q; - uav_odom.twist.twist.linear.x = uav_state.velocity[0]; - uav_odom.twist.twist.linear.y = uav_state.velocity[1]; - uav_odom.twist.twist.linear.z = uav_state.velocity[2]; - - uav_pos = Eigen::Vector3d(msg->position[0], msg->position[1], fly_height); - uav_vel = Eigen::Vector3d(msg->velocity[0], msg->velocity[1], msg->velocity[2]); - uav_yaw = msg->attitude[2]; -} - -// 根据全局点云更新地图 -// 情况：已知全局点云的场景、由SLAM实时获取的全局点云 -void GlobalPlanner::Gpointcloud_cb(const sensor_msgs::PointCloud2ConstPtr &msg) -{ - if (!odom_ready) - { - return; - } - sensor_ready = true; - // 因为全局点云一般较大，只更新一次 - if (!Astar_ptr->Occupy_map_ptr->get_gpcl) - { - // 对Astar中的地图进行更新 - Astar_ptr->Occupy_map_ptr->map_update_gpcl(msg); - } -} - -// 根据局部点云更新地图 -// 情况：RGBD相机、三维激光雷达 -void GlobalPlanner::Lpointcloud_cb(const sensor_msgs::PointCloud2ConstPtr &msg) -{ - if (!odom_ready) - { - return; - } - sensor_ready = true; - Astar_ptr->Occupy_map_ptr->map_update_lpcl(msg, uav_odom); -} - -// 根据2维雷达数据更新地图 -// 情况：2维激光雷达 -void GlobalPlanner::laser_cb(const sensor_msgs::LaserScanConstPtr &msg) -{ - if (!odom_ready) - { - return; - } - sensor_ready = true; - // 对Astar中的地图进行更新（laser+odom）并对地图进行膨胀 - Astar_ptr->Occupy_map_ptr->map_update_laser(msg, uav_odom); -} - -void GlobalPlanner::track_path_cb(const ros::TimerEvent &e) -{ - if (!path_ok) - { - return; - } - - is_new_path = false; - distance_to_goal = (uav_pos - goal_pos).norm(); - // 抵达终点 - if (cur_id == Num_total_wp - 1 || distance_to_goal < 0.2) - { - uav_command.header.stamp = ros::Time::now(); - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - uav_command.Command_ID = uav_command.Command_ID + 1; - - uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS; - uav_command.position_ref[0] = goal_pos[0]; - uav_command.position_ref[1] = goal_pos[1]; - uav_command.position_ref[2] = goal_pos[2]; - - uav_command.yaw_ref = desired_yaw; - uav_cmd_pub.publish(uav_command); - - cout << GREEN << NODE_NAME << "Reach the goal! " << TAIL << endl; - // 停止执行 - path_ok = false; - // 转换状态为等待目标 - exec_state = EXEC_STATE::WAIT_GOAL; - return; - } - - cout << "Moving to Waypoint: [ " << cur_id << " / " << Num_total_wp << " ] " << endl; - cout << "Moving to Waypoint: " - << path_cmd.poses[cur_id].pose.position.x << " [m] " - << path_cmd.poses[cur_id].pose.position.y << " [m] " - << path_cmd.poses[cur_id].pose.position.z << " [m] " << endl; - - // 追踪一条Astar算法给出的路径有几种方式: - // 1,控制方式如果是走航点，则需要对无人机进行限速，保证无人机的平滑移动 - // 2,采用轨迹控制的方式进行追踪，期望速度 = （期望位置 - 当前位置）/预计时间； - uav_command.header.stamp = ros::Time::now(); - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - uav_command.Command_ID = uav_command.Command_ID + 1; - - uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS; - uav_command.position_ref[0] = path_cmd.poses[cur_id].pose.position.x; - uav_command.position_ref[1] = path_cmd.poses[cur_id].pose.position.y; - uav_command.position_ref[2] = path_cmd.poses[cur_id].pose.position.z; - // uav_command.velocity_ref[0] = (path_cmd.poses[cur_id].pose.position.x - uav_pos[0]) / time_per_path; - // uav_command.velocity_ref[1] = (path_cmd.poses[cur_id].pose.position.y - uav_pos[1]) / time_per_path; - // uav_command.velocity_ref[2] = (path_cmd.poses[cur_id].pose.position.z - uav_pos[2]) / time_per_path; - uav_command.yaw_ref = desired_yaw; - - uav_cmd_pub.publish(uav_command); - - cur_id = cur_id + 1; -} - -void GlobalPlanner::safety_cb(const ros::TimerEvent &e) -{ - Eigen::Vector3d cur_pos(uav_pos[0], uav_pos[1], uav_pos[2]); - - is_safety = Astar_ptr->check_safety(cur_pos, safe_distance); -} - -int GlobalPlanner::get_start_point_id(void) -{ - // 选择与当前无人机所在位置最近的点,并从该点开始追踪 - int id = 0; - float distance_to_wp_min = abs(path_cmd.poses[0].pose.position.x - uav_state.position[0]) + abs(path_cmd.poses[0].pose.position.y - uav_state.position[1]) + abs(path_cmd.poses[0].pose.position.z - uav_state.position[2]); - - float distance_to_wp; - - for (int j = 1; j < Num_total_wp; j++) - { - distance_to_wp = abs(path_cmd.poses[j].pose.position.x - uav_state.position[0]) + abs(path_cmd.poses[j].pose.position.y - uav_state.position[1]) + abs(path_cmd.poses[j].pose.position.z - uav_state.position[2]); - - if (distance_to_wp < distance_to_wp_min) - { - distance_to_wp_min = distance_to_wp; - id = j; - } - } - - //　为防止出现回头的情况，此处对航点进行前馈处理 - if (id + 2 < Num_total_wp) - { - id = id + 2; - } - - return id; -} diff --git a/src/无人机端代码/Modules/motion_planning/global_planner/src/global_planner_node.cpp b/src/无人机端代码/Modules/motion_planning/global_planner/src/global_planner_node.cpp deleted file mode 100644 index 106f4ae1..00000000 --- a/src/无人机端代码/Modules/motion_planning/global_planner/src/global_planner_node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include -#include - -#include "global_planner.h" - -; - -void mySigintHandler(int sig) -{ - ROS_INFO("[global_planner_node] exit..."); - ros::shutdown(); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "global_planner"); - ros::NodeHandle nh("~"); - - signal(SIGINT, mySigintHandler); - ros::Duration(1.0).sleep(); - - GlobalPlanner global_planner(nh); - - ros::spin(); - - return 0; -} - diff --git a/src/无人机端代码/Modules/motion_planning/global_planner/src/occupy_map.cpp b/src/无人机端代码/Modules/motion_planning/global_planner/src/occupy_map.cpp deleted file mode 100644 index 23b4d591..00000000 --- a/src/无人机端代码/Modules/motion_planning/global_planner/src/occupy_map.cpp +++ /dev/null @@ -1,600 +0,0 @@ -#include - -// 初始化函数 -void Occupy_map::init(ros::NodeHandle &nh) -{ - // 【参数】编号，从1开始编号 - nh.param("uav_id", uav_id, 0); - // 【参数】仿真模式 - nh.param("global_planner/sim_mode", sim_mode, true); - // 【参数】2D规划时,定高高度 - nh.param("global_planner/fly_height", fly_height, 1.0); - // 集群数量 - nh.param("global_planner/swarm_num_uav", swarm_num_uav, 1); - nh.param("global_planner/odom_inflate", odom_inflate_, 0.6); - nh.param("global_planner/cost_inflate", cost_inflate, 5); - // 【参数】地图原点 - nh.param("map/origin_x", origin_(0), -5.0); - nh.param("map/origin_y", origin_(1), -5.0); - nh.param("map/origin_z", origin_(2), -0.5); - // 【参数】地图实际尺寸，单位：米 - nh.param("map/map_size_x", map_size_3d_(0), 10.0); - nh.param("map/map_size_y", map_size_3d_(1), 10.0); - nh.param("map/map_size_z", map_size_3d_(2), 2.0); - // 【参数】localmap slide window - nh.param("map/queue_size", queue_size, -1); - // 【参数】show border - nh.param("map/border", show_border, false); - // 【参数】地图分辨率，单位：米 - nh.param("map/resolution", resolution_, 0.2); - // 【参数】地图膨胀距离，单位：米 - nh.param("map/inflate", inflate_, 0.3); - - uav_name = "/uav" + std::to_string(uav_id); - // 【发布】全局点云地图 - global_pcl_pub = nh.advertise(uav_name + "/prometheus/global_planning/global_pcl", 1); - // 【发布】膨胀后的全局点云地图 - inflate_pcl_pub = nh.advertise(uav_name + "/prometheus/global_planning/global_inflate_pcl", 1); - // 【定时器】地图发布定时器 - pcl_pub = nh.createTimer(ros::Duration(0.2), &Occupy_map::pub_pcl_cb, this); - - // 全局地图点云指针（环境） - global_point_cloud_map.reset(new pcl::PointCloud); - // 全局地图点云指针（其他无人机） - global_uav_pcl.reset(new pcl::PointCloud); - // 膨胀点云指针 - cloud_inflate_vis_.reset(new pcl::PointCloud); - // 传入点云指针（临时指针） - input_point_cloud.reset(new pcl::PointCloud); - // tf变换后点云指针（临时指针） - transformed_cloud.reset(new pcl::PointCloud); - // 过滤后点云指针（临时指针） - pcl_ptr.reset(new pcl::PointCloud); - // 局部地图滑窗指示器 - st_it = 0; - // 存储的上一帧odom - f_x = f_y = f_z = f_pitch = f_yaw = f_roll = 0.0; - - this->inv_resolution_ = 1.0 / resolution_; - for (int i = 0; i < 3; ++i) - { - // 占据图尺寸 = 地图尺寸 / 分辨率 - grid_size_(i) = ceil(map_size_3d_(i) / resolution_); - } - - // 占据容器的大小 = 占据图尺寸 x*y*z - occupancy_buffer_.resize(grid_size_(0) * grid_size_(1) * grid_size_(2)); - cost_map_.resize(grid_size_(0) * grid_size_(1) * grid_size_(2)); - fill(occupancy_buffer_.begin(), occupancy_buffer_.end(), 0.0); - fill(cost_map_.begin(), cost_map_.end(), 0.0); - - min_range_ = origin_; - max_range_ = origin_ + map_size_3d_; - - min_range_(2) = fly_height - 2 * resolution_; - max_range_(2) = fly_height + 2 * resolution_; - get_gpcl = false; - get_lpcl = false; - get_laser = false; - // 生成地图边界：点云形式 - double dist = 0.1; //每多少距离一个点 - int numdist_x = (max_range_(0) - min_range_(0)) / dist; // x的点数 - int numdist_y = (max_range_(1) - min_range_(1)) / dist; // y的点数 - int numdist = 2 * (numdist_x + numdist_y); //总点数 - border.width = numdist; - border.height = 1; - border.points.resize(numdist); - - inflate_index_uav = 0; - ifn = ceil(odom_inflate_ * inv_resolution_); - for (int x = -ifn; x <= ifn; x++) - for (int y = -ifn; y <= ifn;) - { - enum_p_uav[inflate_index_uav++] << x * resolution_, y * resolution_, 0.0; - if (x == -ifn || x == ifn) - y++; - else - y += 2 * ifn; - } - - for (int x = -ifn - 1; x <= ifn + 1; x++) - for (int y = -ifn - 1; y <= ifn + 1;) - { - enum_p_uav[inflate_index_uav++] << x * resolution_, y * resolution_, 0.0; - if (x == -ifn - 1 || x == ifn + 1) - y++; - else - y += 2 * ifn + 2; - } - - // 膨胀格子数 = 膨胀距离/分辨率 - // ceil返回大于或者等于指定表达式的最小整数 - ifn = ceil(inflate_ * inv_resolution_); - - inflate_index = 0; - for (int x = -ifn; x <= ifn; x++) - for (int y = -ifn; y <= ifn;) - { - enum_p[inflate_index++] << x * resolution_, y * resolution_, 0.0; - if (x == -ifn || x == ifn) - y++; - else - y += 2 * ifn; - } - - cost_index = 0; - for (int x = -ifn - cost_inflate; x <= ifn + cost_inflate; x++) - for (int y = -ifn - cost_inflate; y <= ifn + cost_inflate;) - { - int tmp_dis = x * x + y * y; - if (tmp_dis <= (ifn + cost_inflate) * (ifn + cost_inflate)) - { - enum_p_cost[cost_index++] << x * resolution_, y * resolution_, tmp_dis; - } - if (x == -ifn - cost_inflate || x == ifn + cost_inflate) - y++; - else - y += 2 * ifn + 2 * cost_inflate; - } - // printf("cost map : %d %d\n", cost_inflate, cost_index); - - for (int i = 0; i < numdist_x; i++) // x边界 - { - border.points[i].x = min_range_(0) + i * dist; - border.points[i].y = min_range_(1); - border.points[i].z = min_range_(2); - - border.points[i + numdist_x].x = min_range_(0) + i * dist; - border.points[i + numdist_x].y = max_range_(1); - border.points[i + numdist_x].z = min_range_(2); - } - - for (int i = 0; i < numdist_y; i++) // y边界 - { - border.points[i + 2 * numdist_x].x = min_range_(0); - border.points[i + 2 * numdist_x].y = min_range_(1) + i * dist; - border.points[i + 2 * numdist_x].z = min_range_(2); - - border.points[i + 2 * numdist_x + numdist_y].x = max_range_(0); - border.points[i + 2 * numdist_x + numdist_y].y = min_range_(1) + i * dist; - border.points[i + 2 * numdist_x + numdist_y].z = min_range_(2); - } -} - -// 地图更新函数 - 输入：全局点云 -void Occupy_map::map_update_gpcl(const sensor_msgs::PointCloud2ConstPtr &global_point) -{ - // 全局地图只更新一次 - if (get_gpcl) - { - return; - } - - get_gpcl = true; - pcl::fromROSMsg(*global_point, *input_point_cloud); - global_point_cloud_map = input_point_cloud; - inflate_point_cloud(); -} - -// 地图更新函数 - 输入：局部点云 -void Occupy_map::map_update_lpcl(const sensor_msgs::PointCloud2ConstPtr &local_point, const nav_msgs::Odometry &odom) -{ - // 由sensor_msgs::PointCloud2 转为 pcl::PointCloud - pcl::fromROSMsg(*local_point, *input_point_cloud); - - // 仿真中的点云为全局点云,无需tf变换 - if (sim_mode) - { - if (queue_size <= 0) // without slide windows - { - // map_generator生成的点云为world坐标系 - *global_point_cloud_map += *input_point_cloud; - } - else // with slide windows - { - // slide windows with size: $queue_size - point_cloud_pair[st_it] = *input_point_cloud; // 加入新点云到滑窗 - st_it = (st_it + 1) % queue_size; // 指向下一个移除的点云位置 - - // 累计局部地图：需要20个加法，O（1）内存；增量式：需要19个加法，O（1.5）内存 - global_point_cloud_map.reset(new pcl::PointCloud); - map>::iterator iter; - for (iter = point_cloud_pair.begin(); iter != point_cloud_pair.end(); iter++) - { - *global_point_cloud_map += iter->second; - } - } - - // downsample - *pcl_ptr = *global_point_cloud_map; - vg.setInputCloud(pcl_ptr); - vg.setLeafSize(0.05f, 0.05f, 0.05f); // 下采样叶子节点大小（3D容器） - vg.filter(*global_point_cloud_map); - inflate_point_cloud(); - } - else - { - // 实际中的点云需要先进行tf变换 - local_map_merge_odom(odom); - } -} - -// 地图更新函数 - 输入：laser -void Occupy_map::map_update_laser(const sensor_msgs::LaserScanConstPtr &local_point, const nav_msgs::Odometry &odom) -{ - // 参考网页:http://wiki.ros.org/laser_geometry - // sensor_msgs::LaserScan 转为 sensor_msgs::PointCloud2 格式 - projector_.projectLaser(*local_point, input_laser_scan); - // 再由sensor_msgs::PointCloud2 转为 pcl::PointCloud - pcl::fromROSMsg(input_laser_scan, *input_point_cloud); - - local_map_merge_odom(odom); -} - -// 工具函数：合并局部地图 - 输入：odom以及局部点云 -void Occupy_map::local_map_merge_odom(const nav_msgs::Odometry &odom) -{ - // 从odom中取得6DOF - double x, y, z, roll, pitch, yaw; - // 平移（xyz） - x = odom.pose.pose.position.x; - y = odom.pose.pose.position.y; - z = odom.pose.pose.position.z; - // 旋转（从四元数到欧拉角） - tf::Quaternion orientation; - tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation); - tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); - - // uav is moving - bool pos_change = (abs(x - f_x) > 0.1) || (abs(y - f_y) > 0.1); - // update map even though uav doesn't move - static int update_num = 0; - update_num++; - - // merge local points to local map - if (pos_change || global_point_cloud_map == nullptr || update_num > 1) - { - update_num = 0; - - // accumulate pointcloud according to odom - pcl::transformPointCloud(*input_point_cloud, *transformed_cloud, pcl::getTransformation(x, y, z, 0.0, 0.0, yaw)); - if (queue_size <= 0) // without slide windows - { - *transformed_cloud += *global_point_cloud_map; - } - else // with slide windows - { - // slide windows with size: $queue_size - point_cloud_pair[st_it] = *transformed_cloud; // 加入新点云到滑窗 - st_it = (st_it + 1) % queue_size; // 指向下一个移除的点云位置 - - // 累计局部地图：需要20个加法，O（1）内存；增量式：需要19个加法，O（1.5）内存 - transformed_cloud.reset(new pcl::PointCloud); - map>::iterator iter; - for (iter = point_cloud_pair.begin(); iter != point_cloud_pair.end(); iter++) - { - *transformed_cloud += iter->second; - } - } - - // downsample - vg.setInputCloud(transformed_cloud); - vg.setLeafSize(0.2f, 0.2f, 0.2f); // 下采样叶子节点大小（3D容器） - vg.filter(*global_point_cloud_map); - - // store last odom data - f_x = x; - f_y = y; - f_z = z; - f_roll = roll; - f_pitch = pitch; - f_yaw = yaw; - inflate_point_cloud(); - } -} - -// function: update global uav occupy grid (10Hz, defined by fsm) -void Occupy_map::uav_pcl_update(Eigen::Vector3d *input_uav_odom, bool *get_uav_odom) -{ - Eigen::Vector3d p3d_inf; - - // update global uav occupy grid with input uav odom - pcl::PointXYZ pt; - global_uav_pcl.reset(new pcl::PointCloud); - - for (int i = 1; i <= swarm_num_uav; i++) - { - if (i == uav_id) - { - continue; - } - - if (get_uav_odom[i]) - for (int j = 0; j < inflate_index_uav; j++) - { - pt.x = input_uav_odom[i][0] + enum_p_uav[j](0); - pt.y = input_uav_odom[i][1] + enum_p_uav[j](1); - pt.z = input_uav_odom[i][2] + enum_p_uav[j](2); - //　在global_uav_pcl中添加膨胀点 - global_uav_pcl->points.push_back(pt); - } - } - global_uav_pcl->width = global_uav_pcl->points.size(); - global_uav_pcl->height = 1; - global_uav_pcl->is_dense = true; -} - -// 当global_planning节点接收到点云消息更新时，进行设置点云指针并膨胀 -// Astar规划路径时，采用的是此处膨胀后的点云（setOccupancy只在本函数中使用） -void Occupy_map::inflate_point_cloud(void) -{ - if (get_gpcl) - { - // occupancy_buffer_清零 - fill(occupancy_buffer_.begin(), occupancy_buffer_.end(), 0.0); - fill(cost_map_.begin(), cost_map_.end(), 0.0); - } - else if (queue_size > 0) - { - // queue_size设置为大于0时，代表仅使用过去一定数量的点云 - fill(occupancy_buffer_.begin(), occupancy_buffer_.end(), 0.0); - fill(cost_map_.begin(), cost_map_.end(), 0.0); - } - - //记录开始时间 - ros::Time time_start = ros::Time::now(); - - // 转化为PCL的格式进行处理 - pcl::PointCloud latest_global_cloud_ = *global_point_cloud_map; - - if ((int)latest_global_cloud_.points.size() == 0) - { - return; - } - - cloud_inflate_vis_->clear(); - - pcl::PointXYZ pt_inf; - Eigen::Vector3d p3d, p3d_inf, p3d_cost; - - //　无人机占据地图更新 - for (int i = 0; i < global_uav_pcl->points.size(); i++) - { - p3d_inf(0) = global_uav_pcl->points[i].x; - p3d_inf(1) = global_uav_pcl->points[i].y; - p3d_inf(2) = global_uav_pcl->points[i].z; - this->setOccupancy(p3d_inf, 1); // set to 1 - } - - // 遍历环境全局点云中的所有点 - for (size_t i = 0; i < latest_global_cloud_.points.size(); ++i) - { - // 取出第i个点 - p3d(0) = latest_global_cloud_.points[i].x; - p3d(1) = latest_global_cloud_.points[i].y; - p3d(2) = latest_global_cloud_.points[i].z; - - // 若取出的点不在地图内（膨胀时只考虑地图范围内的点） - if (!isInMap(p3d)) - { - continue; - } - - // cost map update - for (int j = 0; j < cost_index; j++) - { - p3d_cost(0) = p3d(0) + enum_p_cost[j](0); - p3d_cost(1) = p3d(1) + enum_p_cost[j](1); - p3d_cost(2) = p3d(2); - this->updateCostMap(p3d_cost, 1.0 / enum_p_cost[j](2)); - } - - // 根据膨胀距离，膨胀该点 - for (int i = 0; i < inflate_index; i++) - { - p3d_inf(0) = p3d(0) + enum_p[i](0); - p3d_inf(1) = p3d(1) + enum_p[i](1); - p3d_inf(2) = p3d(2) + enum_p[i](2); - - // 若膨胀的点不在地图内（膨胀时只考虑地图范围内的点） - if (!isInMap(p3d_inf)) - { - continue; - } - - pt_inf.x = p3d_inf(0); - pt_inf.y = p3d_inf(1); - pt_inf.z = p3d_inf(2); - cloud_inflate_vis_->push_back(pt_inf); - // 设置膨胀后的点被占据（不管他之前是否被占据） - this->setOccupancy(p3d_inf, 1); - } - } - - *cloud_inflate_vis_ += *global_uav_pcl; - // 加上border,仅用作显示作用（没有占据信息） - if (show_border) - { - *cloud_inflate_vis_ += border; - } - - static int exec_num = 0; - exec_num++; - - // 此处改为根据循环时间计算的数值 - if (exec_num == 50) - { - // 膨胀地图效率与地图大小有关 - cout << YELLOW << "Occupy map: inflate global point take " << (ros::Time::now() - time_start).toSec() << " [s]. " << TAIL << endl; - exec_num = 0; - } -} - -void Occupy_map::pub_pcl_cb(const ros::TimerEvent &e) -{ - // 发布未膨胀点云 - sensor_msgs::PointCloud2 global_env_; - // 假设此时收到的就是全局pcl - pcl::toROSMsg(*global_point_cloud_map, global_env_); - global_env_.header.frame_id = "world"; - global_pcl_pub.publish(global_env_); - - // 发布膨胀点云 - sensor_msgs::PointCloud2 map_inflate_vis; - pcl::toROSMsg(*cloud_inflate_vis_, map_inflate_vis); - map_inflate_vis.header.frame_id = "world"; - inflate_pcl_pub.publish(map_inflate_vis); -} - -void Occupy_map::setOccupancy(Eigen::Vector3d &pos, int occ) -{ - if (occ != 1 && occ != 0) - { - // cout << RED << "Occupy map: occ value error " << TAIL <= cost) - return; - cost_map_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)] = cost; -} - -bool Occupy_map::isInMap(Eigen::Vector3d pos) -{ - // min_range就是原点，max_range就是原点+地图尺寸 - // 比如设置0,0,0为原点，[0,0,0]点会被判断为不在地图里 - //　同时　对于２Ｄ情况，超出飞行高度的数据也会认为不在地图内部 - if (pos(0) < min_range_(0) + 1e-4 || pos(1) < min_range_(1) + 1e-4 || pos(2) < min_range_(2) + 1e-4) - { - return false; - } - - if (pos(0) > max_range_(0) - 1e-4 || pos(1) > max_range_(1) - 1e-4 || pos(2) > max_range_(2) - 1e-4) - { - return false; - } - - return true; -} - -bool Occupy_map::check_safety(Eigen::Vector3d &pos, double check_distance) -{ - if (!isInMap(pos)) - { - // 当前位置点不在地图内 - // cout << RED << "Occupy map, the odom point is not in map" << TAIL < 5) - { - return 0; - } - return 1; -} - -void Occupy_map::posToIndex(Eigen::Vector3d &pos, Eigen::Vector3i &id) -{ - for (int i = 0; i < 3; ++i) - { - id(i) = floor((pos(i) - origin_(i)) * inv_resolution_); - } -} - -void Occupy_map::indexToPos(Eigen::Vector3i &id, Eigen::Vector3d &pos) -{ - for (int i = 0; i < 3; ++i) - { - pos(i) = (id(i) + 0.5) * resolution_ + origin_(i); - } -} - -int Occupy_map::getOccupancy(Eigen::Vector3d &pos) -{ - if (!isInMap(pos)) - { - return -1; - } - - Eigen::Vector3i id; - posToIndex(pos, id); - - return occupancy_buffer_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)]; -} - -double Occupy_map::getCost(Eigen::Vector3d &pos) -{ - if (!isInMap(pos)) - { - return -1; - } - - Eigen::Vector3i id; - posToIndex(pos, id); - - return cost_map_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)]; -} - -int Occupy_map::getOccupancy(Eigen::Vector3i &id) -{ - if (id(0) < 0 || id(0) >= grid_size_(0) || id(1) < 0 || id(1) >= grid_size_(1) || id(2) < 0 || - id(2) >= grid_size_(2)) - { - return -1; - } - - return occupancy_buffer_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)]; -} diff --git a/src/无人机端代码/Modules/motion_planning/local_planner/CMakeLists.txt b/src/无人机端代码/Modules/motion_planning/local_planner/CMakeLists.txt deleted file mode 100644 index 8a1b1474..00000000 --- a/src/无人机端代码/Modules/motion_planning/local_planner/CMakeLists.txt +++ /dev/null @@ -1,50 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(prometheus_local_planner) - -find_package(Eigen3 REQUIRED) -find_package(PCL 1.7 REQUIRED) -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - sensor_msgs - laser_geometry - geometry_msgs - nav_msgs - pcl_ros - visualization_msgs - prometheus_msgs - mavros_msgs - ) - - -catkin_package( - INCLUDE_DIRS include - LIBRARIES LocalPlannerNS -) - -include_directories( - SYSTEM - include - ${PROJECT_SOURCE_DIR}/include - ${catkin_INCLUDE_DIRS} - ${Eigen3_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${PROJECT_SOURCE_DIR}/../../common/include - ) -link_directories(${PCL_LIBRARY_DIRS}) - - - -add_library(vfh src/vfh.cpp) -target_link_libraries(vfh ${PCL_LIBRARIES}) -add_library(apf src/apf.cpp) -target_link_libraries(apf ${PCL_LIBRARIES}) -add_library(local_planner src/local_planner.cpp) -target_link_libraries(local_planner vfh) -target_link_libraries(local_planner apf) - -add_executable(local_planner_main src/local_planner_node.cpp) -add_dependencies(local_planner_main prometheus_local_planner_gencpp) -target_link_libraries(local_planner_main ${catkin_LIBRARIES}) -target_link_libraries(local_planner_main local_planner) diff --git a/src/无人机端代码/Modules/motion_planning/local_planner/include/apf.h b/src/无人机端代码/Modules/motion_planning/local_planner/include/apf.h deleted file mode 100644 index 05715914..00000000 --- a/src/无人机端代码/Modules/motion_planning/local_planner/include/apf.h +++ /dev/null @@ -1,63 +0,0 @@ -#ifndef APF_H -#define APF_H - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include "local_planner_alg.h" -#include "local_planner_utils.h" - -using namespace std; - - - class APF : public local_planner_alg - { - private: - //　参数 - double inflate_distance; - double sensor_max_range; - double max_att_dist; - double k_repulsion; - double k_attraction; - double min_dist; - double ground_height; - double ground_safe_height; - double safe_distance; - - bool has_local_map_; - bool has_odom_; - - Eigen::Vector3d repulsive_force; - Eigen::Vector3d attractive_force; - - pcl::PointCloud latest_local_pcl_; - sensor_msgs::PointCloud2ConstPtr local_map_ptr_; - nav_msgs::Odometry current_odom; - - public: - virtual void set_odom(nav_msgs::Odometry cur_odom); - virtual void set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr); - virtual void set_local_map_pcl(pcl::PointCloud::Ptr &pcl_ptr); - virtual int compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel); - virtual void init(ros::NodeHandle &nh); - APF() {} - ~APF() {} - - typedef shared_ptr Ptr; - }; - - - -#endif diff --git a/src/无人机端代码/Modules/motion_planning/local_planner/include/local_planner.h b/src/无人机端代码/Modules/motion_planning/local_planner/include/local_planner.h deleted file mode 100644 index b19d747d..00000000 --- a/src/无人机端代码/Modules/motion_planning/local_planner/include/local_planner.h +++ /dev/null @@ -1,115 +0,0 @@ -#ifndef LOCAL_PLANNER_H -#define LOCAL_PLANNER_H - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "apf.h" -#include "vfh.h" -#include "local_planner_utils.h" - -using namespace std; - - - class LocalPlanner - { - public: - LocalPlanner(ros::NodeHandle &nh); - ros::NodeHandle local_planner_nh; - - private: - // 参数 - int uav_id; - int algorithm_mode; - int map_input_source; - double max_planning_vel; - double fly_height; - double safe_distance; - bool sim_mode; - bool map_groundtruth; - string local_pcl_topic_name; - - // 订阅无人机状态、目标点、传感器数据（生成地图） - ros::Subscriber goal_sub; - ros::Subscriber uav_state_sub; - ros::Subscriber uav_control_state_sub; - ros::Subscriber local_point_cloud_sub; - - // 发布控制指令 - ros::Publisher uav_cmd_pub; - ros::Publisher rviz_vel_pub; - ros::Timer mainloop_timer; - ros::Timer control_timer; - - // 局部避障算法 算子 - local_planner_alg::Ptr local_alg_ptr; - - prometheus_msgs::UAVState uav_state; // 无人机状态 - prometheus_msgs::UAVControlState uav_control_state; - nav_msgs::Odometry uav_odom; - prometheus_msgs::UAVCommand uav_command; - - double distance_to_goal; - // 规划器状态 - bool odom_ready; - bool drone_ready; - bool sensor_ready; - bool goal_ready; - bool is_safety; - bool path_ok; - - // 规划初始状态及终端状态 - Eigen::Vector3d uav_pos; // 无人机位置 - Eigen::Vector3d uav_vel; // 无人机速度 - Eigen::Quaterniond uav_quat; // 无人机四元数 - double uav_yaw; - // 规划终端状态 - Eigen::Vector3d goal_pos, goal_vel; - - int planner_state; - Eigen::Vector3d desired_vel; - float desired_yaw; - - // 五种状态机 - enum EXEC_STATE - { - WAIT_GOAL, - PLANNING, - TRACKING, - LANDING, - }; - EXEC_STATE exec_state; - - sensor_msgs::PointCloud2ConstPtr local_map_ptr_; - pcl::PointCloud::Ptr pcl_ptr; - pcl::PointCloud latest_local_pcl_; - - void goal_cb(const geometry_msgs::PoseStampedConstPtr &msg); - void uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg); - void uav_state_cb(const prometheus_msgs::UAVStateConstPtr &msg); - void pcl_cb(const sensor_msgs::PointCloud2ConstPtr &msg); - void laserscan_cb(const sensor_msgs::LaserScanConstPtr &msg); - void mainloop_cb(const ros::TimerEvent &e); - void control_cb(const ros::TimerEvent &e); - void debug_info(); - }; - - -#endif diff --git a/src/无人机端代码/Modules/motion_planning/local_planner/include/local_planner_alg.h b/src/无人机端代码/Modules/motion_planning/local_planner/include/local_planner_alg.h deleted file mode 100644 index 7c315d2d..00000000 --- a/src/无人机端代码/Modules/motion_planning/local_planner/include/local_planner_alg.h +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef LOCAL_PLANNING_ALG -#define LOCAL_PLANNING_ALG - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -using namespace std; - - - class local_planner_alg - { - public: - local_planner_alg() {} - ~local_planner_alg() {} - virtual void set_odom(nav_msgs::Odometry cur_odom) = 0; - virtual void set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr) = 0; - virtual void set_local_map_pcl(pcl::PointCloud::Ptr &pcl_ptr) = 0; - virtual int compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel) = 0; - virtual void init(ros::NodeHandle &nh) = 0; - - typedef shared_ptr Ptr; - }; - - - -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/local_planner/include/local_planner_utils.h b/src/无人机端代码/Modules/motion_planning/local_planner/include/local_planner_utils.h deleted file mode 100644 index 3caf4ff6..00000000 --- a/src/无人机端代码/Modules/motion_planning/local_planner/include/local_planner_utils.h +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef LOCAL_PLANNER_UTILS_H -#define LOCAL_PLANNER_UTILS_H - -#include "printf_utils.h" - -#define NODE_NAME "LocalPlanner" -#define MIN_DIS 0.1 - -#endif - diff --git a/src/无人机端代码/Modules/motion_planning/local_planner/include/vfh.h b/src/无人机端代码/Modules/motion_planning/local_planner/include/vfh.h deleted file mode 100644 index c070b735..00000000 --- a/src/无人机端代码/Modules/motion_planning/local_planner/include/vfh.h +++ /dev/null @@ -1,73 +0,0 @@ -#ifndef VFH_H -#define VFH_H - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include "math.h" -#include "local_planner_alg.h" -#include "local_planner_utils.h" - -using namespace std; - - - class VFH : public local_planner_alg - { - private: - //　参数 - double inflate_distance; - double sensor_max_range; - double safe_distance; - bool has_local_map_; - bool has_odom_; - - double goalWeight, obstacle_weight; - double inflate_and_safe_distance; - double velocity; - - double *Hdata; - double Hres; - int Hcnt; // 直方图个数 - - pcl::PointCloud latest_local_pcl_; - sensor_msgs::PointCloud2ConstPtr local_map_ptr_; - nav_msgs::Odometry current_odom; - - bool isIgnored(float x, float y, float z, float ws); - int find_Hcnt(double angle); - double find_angle(int cnt); - double angle_error(double angle1, double angle2); - void generate_voxel_data(double angle_cen, double angle_range, double val); - int find_optimization_path(void); - - public: - virtual void set_odom(nav_msgs::Odometry cur_odom); - virtual void set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr); - virtual void set_local_map_pcl(pcl::PointCloud::Ptr &pcl_ptr); - virtual int compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel); - virtual void init(ros::NodeHandle &nh); - - VFH() {} - ~VFH() - { - delete Hdata; - } - - typedef shared_ptr Ptr; - }; - - - -#endif diff --git a/src/无人机端代码/Modules/motion_planning/local_planner/launch/sitl_apf_with_local_point.launch b/src/无人机端代码/Modules/motion_planning/local_planner/launch/sitl_apf_with_local_point.launch deleted file mode 100644 index 6947902c..00000000 --- a/src/无人机端代码/Modules/motion_planning/local_planner/launch/sitl_apf_with_local_point.launch +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/local_planner/launch/sitl_vfh_with_local_point.launch b/src/无人机端代码/Modules/motion_planning/local_planner/launch/sitl_vfh_with_local_point.launch deleted file mode 100644 index 8fa12651..00000000 --- a/src/无人机端代码/Modules/motion_planning/local_planner/launch/sitl_vfh_with_local_point.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/local_planner/local_planner.md b/src/无人机端代码/Modules/motion_planning/local_planner/local_planner.md deleted file mode 100644 index 1c985930..00000000 --- a/src/无人机端代码/Modules/motion_planning/local_planner/local_planner.md +++ /dev/null @@ -1,31 +0,0 @@ -## local_planner - - - -#### 情况讨论 - - - 局部点云情况 - - 对应真实情况:D435i等RGBD相机,三维激光雷达 - - map_generator生成全局点云,模拟得到局部点云信息 - - 无人机不需要搭载传感器 - - PX4动力学 & fake_odom模块 - -roslaunch prometheus_simulator_utils map_generator_with_fake_odom.launch -roslaunch prometheus_local_planner sitl_apf_with_local_point.launch - - - 2dlidar情况 - - 对应真实情况:二维激光雷达 - - projector_.projectLaser(*local_point, input_laser_scan)将scan转换为点云,即对应回到局部点云情况 - - map_generator生成全局点云,模拟得到局部点云信息 - - 无人机不需要搭载传感器 - - PX4动力学 & fake_odom模块 - -roslaunch prometheus_simulator_utils map_generator_with_fake_odom.launch -roslaunch prometheus_local_planner sitl_global_planner_with_2dlidar.launch - - - 多机情况 - - 建议使用 全局点云情况 + 多个无人机 - - fake_odom模块 - -## 运行 - diff --git a/src/无人机端代码/Modules/motion_planning/local_planner/package.xml b/src/无人机端代码/Modules/motion_planning/local_planner/package.xml deleted file mode 100644 index af2fe83b..00000000 --- a/src/无人机端代码/Modules/motion_planning/local_planner/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - prometheus_local_planner - 0.0.0 - The prometheus_local_planner package - - Yuhua Qi - - TODO - - catkin - roscpp - rospy - std_msgs - sensor_msgs - geometry_msgs - nav_msgs - prometheus_msgs - roscpp - rospy - std_msgs - sensor_msgs - geometry_msgs - nav_msgs - prometheus_msgs - roscpp - rospy - std_msgs - sensor_msgs - geometry_msgs - nav_msgs - prometheus_msgs - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/local_planner/src/apf.cpp b/src/无人机端代码/Modules/motion_planning/local_planner/src/apf.cpp deleted file mode 100644 index 817eb86e..00000000 --- a/src/无人机端代码/Modules/motion_planning/local_planner/src/apf.cpp +++ /dev/null @@ -1,183 +0,0 @@ -#include "apf.h" - - - - void APF::init(ros::NodeHandle &nh) - { - - // 【参数】障碍物膨胀距离 - nh.param("apf/inflate_distance", inflate_distance, 0.2); - // 【参数】感知障碍物距离 - nh.param("apf/sensor_max_range", sensor_max_range, 5.0); - // 【参数】障碍物排斥力增益 - nh.param("apf/k_repulsion", k_repulsion, 0.8); - // 【参数】目标吸引力增益 - nh.param("apf/k_attraction", k_attraction, 0.4); - // 【参数】最小避障距离 - nh.param("apf/min_dist", min_dist, 0.2); - // 【参数】最大吸引距离（相对于目标） - nh.param("apf/max_att_dist", max_att_dist, 5.0); - // 【参数】地面高度 - nh.param("apf/ground_height", ground_height, 0.1); - // 【参数】地面安全距离，低于地面高度，则不考虑该点的排斥力 - nh.param("apf/ground_safe_height", ground_safe_height, 0.2); - // 【参数】安全停止距离 - nh.param("apf/safe_distance", safe_distance, 0.15); - has_local_map_ = false; - } - - // 设置局部地图 - void APF::set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr) - { - local_map_ptr_ = local_map_ptr; - pcl::fromROSMsg(*local_map_ptr, latest_local_pcl_); - has_local_map_ = true; - } - - // 设置局部点云 - void APF::set_local_map_pcl(pcl::PointCloud::Ptr &pcl_ptr) - { - latest_local_pcl_ = *pcl_ptr; - has_local_map_ = true; - } - - // 设置本地位置 - void APF::set_odom(nav_msgs::Odometry cur_odom) - { - current_odom = cur_odom; - has_odom_ = true; - } - - // 计算输出 - int APF::compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel) - { - // 规划器返回的状态值：0 for not init; 1 for safe; 2 for dangerous - int local_planner_state = 0; - int safe_cnt = 0; - - if (!has_local_map_ || !has_odom_) - return 0; - - if ((int)latest_local_pcl_.points.size() == 0) - return 0; - - if (isnan(goal(0)) || isnan(goal(1)) || isnan(goal(2))) - return 0; - - //　当前位置 - Eigen::Vector3d current_pos; - current_pos[0] = current_odom.pose.pose.position.x; - current_pos[1] = current_odom.pose.pose.position.y; - current_pos[2] = current_odom.pose.pose.position.z; - - ros::Time begin_collision = ros::Time::now(); - - // 引力 - Eigen::Vector3d uav2goal = goal - current_pos; - // 不考虑高度影响 - uav2goal(2) = 0.0; - double dist_att = uav2goal.norm(); - if (dist_att > max_att_dist) - { - uav2goal = max_att_dist * uav2goal / dist_att; - } - //　计算吸引力 - attractive_force = k_attraction * uav2goal; - - // 排斥力 - double uav_height = current_odom.pose.pose.position.z; - repulsive_force = Eigen::Vector3d(0.0, 0.0, 0.0); - - Eigen::Vector3d p3d; - vector obstacles; - - //　根据局部点云计算排斥力（是否可以考虑对点云进行降采样？） - for (size_t i = 0; i < latest_local_pcl_.points.size(); ++i) - { - p3d(0) = latest_local_pcl_.points[i].x; - p3d(1) = latest_local_pcl_.points[i].y; - p3d(2) = latest_local_pcl_.points[i].z; - - Eigen::Vector3d current_pos_local(0.0, 0.0, 0.0); - - //　低于地面高度，则不考虑该点的排斥力 - double point_height_global = uav_height + p3d(2); - if (fabs(point_height_global) < ground_height) - continue; - - //　超出最大感知距离，则不考虑该点的排斥力 - double dist_push = (current_pos_local - p3d).norm(); - if (dist_push > sensor_max_range || isnan(dist_push)) - continue; - - //　考虑膨胀距离 - dist_push = dist_push - inflate_distance; - - // 如果当前的观测点中，包含小于安全停止距离的点，进行计数 - if (dist_push < safe_distance) - { - safe_cnt++; - } - - //　小于最小距离时，则增大该距离，从而增大排斥力 - if (dist_push < min_dist) - { - dist_push = min_dist / 1.5; - } - - obstacles.push_back(p3d); - double push_gain = k_repulsion * (1 / dist_push - 1 / sensor_max_range) * 1.0 / (dist_push * dist_push); - - if (dist_att < 1.0) - { - push_gain *= dist_att; // to gaurantee to reach the goal. - } - - repulsive_force += push_gain * (current_pos_local - p3d) / dist_push; - } - - //　平均排斥力 - if (obstacles.size() != 0) - { - repulsive_force = repulsive_force / obstacles.size(); - } - - Eigen::Quaterniond cur_rotation_local_to_global(current_odom.pose.pose.orientation.w, - current_odom.pose.pose.orientation.x, - current_odom.pose.pose.orientation.y, - current_odom.pose.pose.orientation.z); - - Eigen::Matrix rotation_mat_local_to_global = cur_rotation_local_to_global.toRotationMatrix(); - - repulsive_force = rotation_mat_local_to_global * repulsive_force; - - // 合力 - desired_vel = repulsive_force + attractive_force; - - // 由于定高飞行，设置期望Z轴速度为0 - desired_vel[2] = 0.0; - - // 如果不安全的点超出， - if (safe_cnt > 10) - { - local_planner_state = 2; //成功规划，但是飞机不安全 - } - else - { - local_planner_state = 1; //成功规划， 安全 - } - - static int exec_num = 0; - exec_num++; - - // 此处改为根据循环时间计算的数值 - if (exec_num == 50) - { - cout << GREEN << NODE_NAME << "APF calculate take: " << (ros::Time::now() - begin_collision).toSec() << " [s] " << TAIL << endl; - exec_num = 0; - } - - return local_planner_state; - } - - diff --git a/src/无人机端代码/Modules/motion_planning/local_planner/src/local_planner.cpp b/src/无人机端代码/Modules/motion_planning/local_planner/src/local_planner.cpp deleted file mode 100644 index 1a3f3bc9..00000000 --- a/src/无人机端代码/Modules/motion_planning/local_planner/src/local_planner.cpp +++ /dev/null @@ -1,432 +0,0 @@ -#include "local_planner.h" - -// 初始化函数 -LocalPlanner::LocalPlanner(ros::NodeHandle &nh) -{ - // 【参数】编号，从1开始编号 - nh.param("uav_id", uav_id, 0); - // 【参数】是否为仿真模式 - nh.param("local_planner/sim_mode", sim_mode, false); - // 【参数】根据参数 planning/algorithm_mode 选择局部避障算法: 0为APF,1为VFH - nh.param("local_planner/algorithm_mode", algorithm_mode, 0); - // 【参数】激光雷达模型,0代表3d雷达,1代表2d雷达 - // 3d雷达输入类型为 2d雷达输入类型为 - nh.param("local_planner/map_input_source", map_input_source, 0); - // 【参数】定高高度 - nh.param("local_planner/fly_height", fly_height, 1.0); - // 【参数】最大速度 - nh.param("local_planner/max_planning_vel", max_planning_vel, 0.4); - - //【订阅】订阅目标点 - goal_sub = nh.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/motion_planning/goal", 1, &LocalPlanner::goal_cb, this); - - //【订阅】无人机状态信息 - uav_state_sub = nh.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/state", - 1, - &LocalPlanner::uav_state_cb, this); - - uav_control_state_sub = nh.subscribe("/uav" + std::to_string(uav_id) + "/prometheus/control_state", - 1, - &LocalPlanner::uav_control_state_cb, this); - string uav_name = "/uav" + std::to_string(uav_id); - // 订阅传感器点云信息,该话题名字可在launch文件中任意指定 - if (map_input_source == 0) - { - nh.getParam("local_planner/local_pcl_topic_name", local_pcl_topic_name); - cout << GREEN << "Local pcl mode, subscirbe to " << uav_name << local_pcl_topic_name << TAIL << endl; - local_point_cloud_sub = nh.subscribe("/uav" + std::to_string(uav_id) + local_pcl_topic_name, 1, &LocalPlanner::pcl_cb, this); - } - else if (map_input_source == 1) - { - nh.getParam("global_planner/local_pcl_topic_name", local_pcl_topic_name); - cout << GREEN << "Laser scan mode, subscirbe to " << uav_name << local_pcl_topic_name << TAIL << endl; - local_point_cloud_sub = nh.subscribe("/uav" + std::to_string(uav_id) + local_pcl_topic_name, 1, &LocalPlanner::laserscan_cb, this); - } - - // 【发布】控制指令 - uav_cmd_pub = nh.advertise("/uav" + std::to_string(uav_id) + "/prometheus/command", 1); - - // 【发布】速度用于显示 - rviz_vel_pub = nh.advertise("/uav" + std::to_string(uav_id) + "/prometheus/local_planner/desired_vel", 0 ); - - // 【定时器】执行周期为1Hz - mainloop_timer = nh.createTimer(ros::Duration(0.2), &LocalPlanner::mainloop_cb, this); - - // 【定时器】控制定时器 - control_timer = nh.createTimer(ros::Duration(0.05), &LocalPlanner::control_cb, this); - - // 选择避障算法 - if (algorithm_mode == 0) - { - local_alg_ptr.reset(new APF); - local_alg_ptr->init(nh); - cout << GREEN << NODE_NAME << "APF init. " << TAIL << endl; - } - else if (algorithm_mode == 1) - { - local_alg_ptr.reset(new VFH); - local_alg_ptr->init(nh); - cout << GREEN << NODE_NAME << "VFH init. " << TAIL << endl; - } - - // 规划器状态参数初始化 - exec_state = EXEC_STATE::WAIT_GOAL; - odom_ready = false; - drone_ready = false; - goal_ready = false; - sensor_ready = false; - path_ok = false; - - // 初始化发布的指令 - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Init_Pos_Hover; - uav_command.position_ref[0] = 0; - uav_command.position_ref[1] = 0; - uav_command.position_ref[2] = 0; - uav_command.yaw_ref = 0; - desired_yaw = 0.0; - - // 地图初始化 - sensor_msgs::PointCloud2ConstPtr init_local_map(new sensor_msgs::PointCloud2()); - local_map_ptr_ = init_local_map; -} -void LocalPlanner::debug_info() -{ - //固定的浮点显示 - cout.setf(ios::fixed); - // setprecision(n) 设显示小数精度为n位 - cout << setprecision(2); - //左对齐 - cout.setf(ios::left); - // 强制显示小数点 - cout.setf(ios::showpoint); - // 强制显示符号 - cout.setf(ios::showpos); - cout << GREEN << "--------------> Local Planner <------------- " << TAIL << endl; - cout << GREEN << "[ ID: " << uav_id << "] " << TAIL; - if (drone_ready) - { - cout << GREEN << "[ drone ready ] " << TAIL << endl; - } - else - { - cout << RED << "[ drone not ready ] " << TAIL << endl; - } - - if (odom_ready) - { - cout << GREEN << "[ odom ready ] " << TAIL << endl; - } - else - { - cout << RED << "[ odom not ready ] " << TAIL << endl; - } - - if (sensor_ready) - { - cout << GREEN << "[ sensor ready ] " << TAIL << endl; - } - else - { - cout << RED << "[ sensor not ready ] " << TAIL << endl; - } - - if (exec_state == EXEC_STATE::WAIT_GOAL) - { - cout << GREEN << "[ WAIT_GOAL ] " << TAIL << endl; - if (uav_control_state.control_state != prometheus_msgs::UAVControlState::COMMAND_CONTROL) - { - cout << YELLOW << "Please switch to COMMAND_CONTROL mode." << TAIL << endl; - } - if (!goal_ready) - { - cout << YELLOW << "Waiting for a new goal." << TAIL << endl; - } - } - else if (exec_state == EXEC_STATE::PLANNING) - { - cout << GREEN << "[ PLANNING ] " << TAIL << endl; - - if (planner_state == 1) - { - cout << GREEN << NODE_NAME << "local planning desired vel [XY]:" << desired_vel(0) << "[m/s]" << desired_vel(1) << "[m/s]" << TAIL << endl; - } - else if (planner_state == 2) - { - cout << YELLOW << NODE_NAME << "Dangerous!" << TAIL << endl; - } - distance_to_goal = (uav_pos - goal_pos).norm(); - cout << GREEN << "---->distance_to_goal:" << distance_to_goal << TAIL << endl; - } - else if (exec_state == EXEC_STATE::LANDING) - { - cout << GREEN << "[ LANDING ] " << TAIL << endl; - } -} - -void LocalPlanner::mainloop_cb(const ros::TimerEvent &e) -{ - static int exec_num = 0; - exec_num++; - - if (exec_num == 10) - { - debug_info(); - exec_num = 0; - } - - // 检查当前状态，不满足规划条件则直接退出主循环 - if (!odom_ready || !drone_ready || !sensor_ready) - { - return; - } - else - { - // 对检查的状态进行重置 - odom_ready = false; - drone_ready = false; - sensor_ready = false; - } - - switch (exec_state) - { - case EXEC_STATE::WAIT_GOAL: - path_ok = false; - if (!goal_ready) - { - if (exec_num == 20) - { - cout << GREEN << NODE_NAME << "Waiting for a new goal." << TAIL << endl; - exec_num = 0; - } - } - else - { - // 获取到目标点后，生成新轨迹 - exec_state = EXEC_STATE::PLANNING; - goal_ready = false; - } - - break; - case EXEC_STATE::PLANNING: - // desired_vel是返回的规划速度；返回值为2时,飞机不安全(距离障碍物太近) - planner_state = local_alg_ptr->compute_force(goal_pos, desired_vel); - - path_ok = true; - - //　对规划的速度进行限幅处理 - if (desired_vel.norm() > max_planning_vel) - { - desired_vel = desired_vel / desired_vel.norm() * max_planning_vel; - } - - break; - case EXEC_STATE::LANDING: - - uav_command.header.stamp = ros::Time::now(); - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Land; - uav_command.Command_ID = uav_command.Command_ID + 1; - - uav_cmd_pub.publish(uav_command); - break; - } -} - - -void LocalPlanner::goal_cb(const geometry_msgs::PoseStampedConstPtr &msg) -{ - // 2D定高飞行 - goal_pos << msg->pose.position.x, msg->pose.position.y, fly_height; - goal_vel.setZero(); - goal_ready = true; - - cout << GREEN << NODE_NAME << "Get a new goal point:" << goal_pos(0) << " [m] " << goal_pos(1) << " [m] " << goal_pos(2) << " [m] " << TAIL << endl; - - if (goal_pos(0) == 99 && goal_pos(1) == 99) - { - path_ok = false; - goal_ready = false; - exec_state = EXEC_STATE::LANDING; - cout << GREEN << NODE_NAME << "Land " << TAIL << endl; - } -} - -//无人机控制状态回调函数 -void LocalPlanner::uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg) -{ - uav_control_state = *msg; -} - -void LocalPlanner::uav_state_cb(const prometheus_msgs::UAVState::ConstPtr &msg) -{ - uav_state = *msg; - if (uav_state.connected == true && uav_state.armed == true) - { - drone_ready = true; - } - else - { - drone_ready = false; - } - - if (uav_state.odom_valid) - { - odom_ready = true; - } - else - { - odom_ready = false; - } - - uav_odom.header = uav_state.header; - uav_odom.child_frame_id = "base_link"; - - uav_odom.pose.pose.position.x = uav_state.position[0]; - uav_odom.pose.pose.position.y = uav_state.position[1]; - uav_odom.pose.pose.position.z = fly_height; - - uav_odom.pose.pose.orientation = uav_state.attitude_q; - uav_odom.twist.twist.linear.x = uav_state.velocity[0]; - uav_odom.twist.twist.linear.y = uav_state.velocity[1]; - uav_odom.twist.twist.linear.z = uav_state.velocity[2]; - - uav_pos = Eigen::Vector3d(msg->position[0], msg->position[1], fly_height); - uav_vel = Eigen::Vector3d(msg->velocity[0], msg->velocity[1], msg->velocity[2]); - uav_yaw = msg->attitude[2]; - - local_alg_ptr->set_odom(uav_odom); -} - -void LocalPlanner::laserscan_cb(const sensor_msgs::LaserScanConstPtr &msg) -{ - if (!odom_ready) - { - return; - } - - pcl::PointCloud _pointcloud; - - _pointcloud.clear(); - pcl::PointXYZ newPoint; - double newPointAngle; - - int beamNum = msg->ranges.size(); - for (int i = 0; i < beamNum; i++) - { - newPointAngle = msg->angle_min + msg->angle_increment * i; - newPoint.x = msg->ranges[i] * cos(newPointAngle); - newPoint.y = msg->ranges[i] * sin(newPointAngle); - newPoint.z = uav_odom.pose.pose.position.z; - _pointcloud.push_back(newPoint); - } - - pcl_ptr = _pointcloud.makeShared(); - local_alg_ptr->set_local_map_pcl(pcl_ptr); - latest_local_pcl_ = *pcl_ptr; // 没用 - - sensor_ready = true; -} - -void LocalPlanner::pcl_cb(const sensor_msgs::PointCloud2ConstPtr &msg) -{ - if (!odom_ready) - { - return; - } - - local_map_ptr_ = msg; - local_alg_ptr->set_local_map(local_map_ptr_); - pcl::fromROSMsg(*msg, latest_local_pcl_); // 没用 - - sensor_ready = true; -} - -void LocalPlanner::control_cb(const ros::TimerEvent &e) -{ - if (!path_ok) - { - return; - } - - distance_to_goal = (uav_pos - goal_pos).norm(); - - // 抵达终点 - if (distance_to_goal < MIN_DIS) - { - uav_command.header.stamp = ros::Time::now(); - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - uav_command.Command_ID = uav_command.Command_ID + 1; - - uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS; - uav_command.position_ref[0] = goal_pos[0]; - uav_command.position_ref[1] = goal_pos[1]; - uav_command.position_ref[2] = goal_pos[2]; - - uav_command.yaw_ref = desired_yaw; - uav_cmd_pub.publish(uav_command); - - cout << GREEN << NODE_NAME << "Reach the goal! " << TAIL << endl; - // 停止执行 - path_ok = false; - // 转换状态为等待目标 - exec_state = EXEC_STATE::WAIT_GOAL; - return; - } - - uav_command.header.stamp = ros::Time::now(); - uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; - uav_command.Command_ID = uav_command.Command_ID + 1; - - uav_command.Move_mode = prometheus_msgs::UAVCommand::XY_VEL_Z_POS; - uav_command.position_ref[2] = fly_height; - uav_command.velocity_ref[0] = desired_vel[0]; - uav_command.velocity_ref[1] = desired_vel[1]; - uav_command.yaw_ref = desired_yaw; - - uav_cmd_pub.publish(uav_command); - - //　发布rviz显示 rviz怎么显示速度方向? - - visualization_msgs::Marker marker; - marker.header.frame_id = "world"; - marker.header.stamp = ros::Time(); - marker.ns = "my_namespace"; - marker.id = 0; - marker.type = visualization_msgs::Marker::ARROW; - marker.action = visualization_msgs::Marker::ADD; - // marker.pose.position.x = 1; - // marker.pose.position.y = 1; - // marker.pose.position.z = 1; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - // marker.scale.x = 1; - // marker.scale.y = 0.1; - // marker.scale.z = 0.1; - // marker.color.a = 1.0; // Don't forget to set the alpha! - // marker.color.r = 0.0; - // marker.color.g = 1.0; - // marker.color.b = 0.0; - -marker.scale.x = 0.1; -marker.scale.y = 0.1; -marker.scale.z = 0.15; -// 点为绿色 -marker.color.g = 1.0f; -marker.color.a = 1.0; -geometry_msgs::Point p1, p2; -p1.x = uav_pos(0); -p1.y = uav_pos(1); -p1.z = uav_pos(2); -p2.x = uav_pos(0) + desired_vel(0); -p2.y = uav_pos(1) + desired_vel(1); -p2.z = uav_pos(2) + desired_vel(2); -marker.points.push_back(p1); -marker.points.push_back(p2); - - - //only if using a MESH_RESOURCE marker type: - // marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae"; - - rviz_vel_pub.publish(marker); -} diff --git a/src/无人机端代码/Modules/motion_planning/local_planner/src/local_planner_node.cpp b/src/无人机端代码/Modules/motion_planning/local_planner/src/local_planner_node.cpp deleted file mode 100644 index 7c40663a..00000000 --- a/src/无人机端代码/Modules/motion_planning/local_planner/src/local_planner_node.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include -#include - -#include "local_planner.h" - - - -void mySigintHandler(int sig) -{ - ROS_INFO("[local_planner_node] exit..."); - ros::shutdown(); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "local_planner_node"); - ros::NodeHandle nh("~"); - - signal(SIGINT, mySigintHandler); - ros::Duration(1.0).sleep(); - - LocalPlanner local_planner(nh); - - ros::spin(); - - return 0; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/local_planner/src/vfh.cpp b/src/无人机端代码/Modules/motion_planning/local_planner/src/vfh.cpp deleted file mode 100644 index fc110b21..00000000 --- a/src/无人机端代码/Modules/motion_planning/local_planner/src/vfh.cpp +++ /dev/null @@ -1,286 +0,0 @@ -#include "vfh.h" - - - - void VFH::init(ros::NodeHandle &nh) - { - // 【参数】障碍物膨胀距离 - nh.param("vfh/inflate_distance", inflate_distance, 0.2); - // 【参数】感知障碍物距离 - nh.param("vfh/sensor_max_range", sensor_max_range, 5.0); - // 【参数】目标权重 - nh.param("vfh/goalWeight", goalWeight, 0.2); - // 【参数】直方图 个数 - nh.param("vfh/h_res", Hcnt, 180); - // 【参数】障碍物权重 - nh.param("vfh/obstacle_weight", obstacle_weight, 0.0); - // 【参数】安全停止距离 - nh.param("vfh/safe_distance", safe_distance, 0.2); - // 【参数】设定速度 - nh.param("vfh/velocity", velocity, 1.0); - - inflate_and_safe_distance = safe_distance + inflate_distance; - Hres = 2 * M_PI / Hcnt; - Hdata = new double[Hcnt](); - for (int i(0); i < Hcnt; i++) - { - Hdata[i] = 0.0; - } - - has_local_map_ = false; - } - - // get the map - void VFH::set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr) - { - local_map_ptr_ = local_map_ptr; - pcl::fromROSMsg(*local_map_ptr, latest_local_pcl_); - has_local_map_ = true; - } - - void VFH::set_local_map_pcl(pcl::PointCloud::Ptr &pcl_ptr) - { - latest_local_pcl_ = *pcl_ptr; - has_local_map_ = true; - } - - void VFH::set_odom(nav_msgs::Odometry cur_odom) - { - current_odom = cur_odom; - has_odom_ = true; - } - - int VFH::compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel) - { - // 0 for not init; 1for safe; 2 for dangerous - int local_planner_state = 0; - int safe_cnt = 0; - - if (!has_local_map_ || !has_odom_) - return 0; - - if ((int)latest_local_pcl_.points.size() == 0) - return 0; - - if (isnan(goal(0)) || isnan(goal(1)) || isnan(goal(2))) - return 0; - - // clear the Hdata - for (int i = 0; i < Hcnt; i++) - { - Hdata[i] = 0; - } - - ros::Time begin_collision = ros::Time::now(); - - // 计算障碍物相关cost - Eigen::Vector3d p3d; - vector obstacles; - Eigen::Vector3d p3d_gloabl_rot; - - // 排斥力 - Eigen::Quaterniond cur_rotation_local_to_global(current_odom.pose.pose.orientation.w, current_odom.pose.pose.orientation.x, current_odom.pose.pose.orientation.y, current_odom.pose.pose.orientation.z); - - Eigen::Matrix rotation_mat_local_to_global = cur_rotation_local_to_global.toRotationMatrix(); - Eigen::Vector3d eulerAngle_yrp = rotation_mat_local_to_global.eulerAngles(2, 1, 0); - rotation_mat_local_to_global = Eigen::AngleAxisd(eulerAngle_yrp(0), Eigen::Vector3d::UnitZ()).toRotationMatrix(); - - // 遍历点云中的所有点 - for (size_t i = 0; i < latest_local_pcl_.points.size(); ++i) - { - // 提取障碍物点 - p3d(0) = latest_local_pcl_.points[i].x; - p3d(1) = latest_local_pcl_.points[i].y; - p3d(2) = latest_local_pcl_.points[i].z; - - // 将本地点云转化为全局点云点(主要是yaw角) - p3d_gloabl_rot = rotation_mat_local_to_global * p3d; - - // sensor_max_range为感知距离,只考虑感知距离内的障碍 - if (isIgnored(p3d_gloabl_rot(0), p3d_gloabl_rot(1), p3d_gloabl_rot(2), sensor_max_range)) - { - continue; - } - - double obs_dist = p3d_gloabl_rot.norm(); - double obs_angle = atan2(p3d_gloabl_rot(1), p3d_gloabl_rot(0)); - double angle_range; - if (obs_dist > inflate_and_safe_distance) - { - angle_range = asin(inflate_and_safe_distance / obs_dist); - } - else if (obs_dist <= inflate_and_safe_distance) - { - safe_cnt++; // 非常危险 - continue; - } - - double obstacle_cost = obstacle_weight * (1 / obs_dist - 1 / sensor_max_range) * 1.0 / (obs_dist * obs_dist); - generate_voxel_data(obs_angle, angle_range, obstacle_cost); - - obstacles.push_back(p3d); - } - - // 与目标点相关cost计算 - //　当前位置 - Eigen::Vector3d current_pos; - current_pos[0] = current_odom.pose.pose.position.x; - current_pos[1] = current_odom.pose.pose.position.y; - current_pos[2] = current_odom.pose.pose.position.z; - Eigen::Vector3d uav2goal = goal - current_pos; - // 不考虑高度影响 - uav2goal(2) = 0.0; - double dist_att = uav2goal.norm(); - double goal_heading = atan2(uav2goal(1), uav2goal(0)); - - for (int i = 0; i < Hcnt; i++) - { - // Hdata; - // angle_i 为当前角度 - double angle_i = find_angle(i); - - double goal_cost = 0; - double angle_er = angle_error(angle_i, goal_heading); - float goal_gain; - if (dist_att > 3.0) - { - goal_gain = 3.0; - } - else if (dist_att < 0.5) - { - goal_gain = 0.5; - } - else - { - goal_gain = dist_att; - } - // 当前角度与目标角度差的越多,则该代价越大 - goal_cost = goalWeight * angle_er * goal_gain; - - Hdata[i] += goal_cost; - } - - // 寻找cost最小的路径 - int best_ind = find_optimization_path(); // direction - // 提取最优路径的航向角 - double best_heading = find_angle(best_ind); - - desired_vel(0) = cos(best_heading) * velocity; - desired_vel(1) = sin(best_heading) * velocity; - // 定高飞行 - desired_vel(2) = 0.0; - - // 如果不安全的点超出指定数量 - if (safe_cnt > 5) - { - local_planner_state = 2; //成功规划，但是飞机不安全 - } - else - { - local_planner_state = 1; //成功规划， 安全 - } - - static int exec_num = 0; - exec_num++; - - // 此处改为根据循环时间计算的数值 - if (exec_num == 50) - { - cout << GREEN << NODE_NAME << "VFH calculate take: " << (ros::Time::now() - begin_collision).toSec() << " [s] " << TAIL << endl; - exec_num = 0; - } - - return local_planner_state; - } - - // 寻找最小 - int VFH::find_optimization_path(void) - { - int bset_ind = 10000; - double best_cost = 100000; - for (int i = 0; i < Hcnt; i++) - { - if (Hdata[i] < best_cost) - { - best_cost = Hdata[i]; - bset_ind = i; - } - } - return bset_ind; - } - - bool VFH::isIgnored(float x, float y, float z, float ws) - { - z = 0; - if (isnan(x) || isnan(y) || isnan(z)) - return true; - - if (x * x + y * y + z * z > ws) - return true; - - return false; - } - - void VFH::generate_voxel_data(double angle_cen, double angle_range, double val) // set the map obstacle into the H data - { - double angle_max = angle_cen + angle_range; - double angle_min = angle_cen - angle_range; - int cnt_min = find_Hcnt(angle_min); - int cnt_max = find_Hcnt(angle_max); - if (cnt_min > cnt_max) - { - for (int i = cnt_min; i < Hcnt; i++) - { - Hdata[i] = +val; - } - for (int i = 0; i < cnt_max; i++) - { - Hdata[i] += val; - } - } - else if (cnt_max >= cnt_min) - { - for (int i = cnt_min; i <= cnt_max; i++) - { - Hdata[i] += val; - } - } - } - - // angle: deg - int VFH::find_Hcnt(double angle) - { - if (angle < 0) - { - angle += 2 * M_PI; - } - if (angle > 2 * M_PI) - { - angle -= 2 * M_PI; - } - int cnt = floor(angle / Hres); - return cnt; - } - - double VFH::find_angle(int cnt) - { - double angle = (cnt + 0.5) * Hres; - return angle; - } - - double VFH::angle_error(double angle1, double angle2) - { - double angle_er = angle1 - angle2; - while (angle_er > M_PI) - { - angle_er = angle_er - 2 * M_PI; - } - - while (angle_er < -M_PI) - { - angle_er = angle_er + 2 * M_PI; - } - angle_er = fabs(angle_er); - return angle_er; - } - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap.sh b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap.sh deleted file mode 100644 index 24b78d56..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap.sh +++ /dev/null @@ -1,7 +0,0 @@ -#roscore & sleep 5; -roslaunch min_snap real.launch & sleep 5; -roslaunch vicon_bridge vicon.launch & sleep 3; -roslaunch mavros px4.launch & sleep 3; -roslaunch ekf PX4_vicon.launch & sleep 3; -roslaunch px4ctrl run_ctrl.launch -#rosbag record /vicon_imu_ekf_odom /debugPx4ctrl diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/CMakeLists.txt b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/CMakeLists.txt deleted file mode 100644 index b6b3bb13..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/CMakeLists.txt +++ /dev/null @@ -1,221 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(min_snap) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages - -find_package(Eigen3 REQUIRED) - -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - geometry_msgs - quadrotor_msgs - mini_snap_traj_utils -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include -# LIBRARIES min_snap - CATKIN_DEPENDS roscpp rospy std_msgs mini_snap_traj_utils -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/min_snap.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/min_snap_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) -add_library( minsnapCloseform - include/min_snap/min_snap_closeform.h - src/min_snap_closeform.cpp -) -target_link_libraries(minsnapCloseform ${catkin_LIBRARIES}) - -add_executable(min_snap_generator src/min_snap_generator.cpp) -target_link_libraries(min_snap_generator minsnapCloseform ${catkin_LIBRARIES}) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_min_snap.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/include/min_snap/min_snap_closeform.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/include/min_snap/min_snap_closeform.h deleted file mode 100644 index 95744dd0..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/include/min_snap/min_snap_closeform.h +++ /dev/null @@ -1,42 +0,0 @@ -#ifndef _MIN_SNAP_CLOSEFORM_H_ -#define _MIN_SNAP_CLOSEFORM_H_ - -#include -#include - -using std::vector; -using namespace std; - -namespace my_planner -{ - class minsnapCloseform - { - private: - vector wps; - int n_order, n_seg, n_per_seg; - double mean_vel; - Eigen::VectorXd ts; - Eigen::VectorXd poly_coef_x, poly_coef_y, poly_coef_z; - Eigen::VectorXd dec_vel_x, dec_vel_y, dec_vel_z; - Eigen::MatrixXd Q, M, Ct; - - int fact(int n); - void init_ts(int init_type); - std::pair MinSnapCloseFormServer(const Eigen::VectorXd &wp); - Eigen::VectorXd calDecVel(const Eigen::VectorXd decvel); - void calQ(); - void calM(); - void calCt(); - - public: - minsnapCloseform(){}; - ~minsnapCloseform(){}; - minsnapCloseform(const vector &waypoints, double meanvel = 1.0); - void Init(const vector &waypoints, double meanvel = 1.0); - void calMinsnap_polycoef(); - Eigen::MatrixXd getPolyCoef(); - Eigen::MatrixXd getDecVel(); - Eigen::VectorXd getTime(); - }; -} -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/launch/default.rviz b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/launch/default.rviz deleted file mode 100644 index 231314b3..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/launch/default.rviz +++ /dev/null @@ -1,312 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Planning1 - - /Planning1/jerk_dir1 - - /Planning1/vel_dir1 - - /Planning1/acc_dir1 - - /Planning1/Goal_point1 - - /Planning1/plan_traj1 - - /Mapping1/simulation_map1/Autocompute Value Bounds1 - - /Simulation1/robot1 - Splitter Ratio: 0.5 - Tree Height: 865 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /3D Nav Goal1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - - /ThirdPersonFollower1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 1 - Class: rviz/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 40 - Reference Frame: - Value: true - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /min_snap_visual/jerk_dir - Name: jerk_dir - Namespaces: - "": true - Queue Size: 1 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /min_snap_visual/vel_dir - Name: vel_dir - Namespaces: - "": true - Queue Size: 1 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /min_snap_visual/acc_dir - Name: acc_dir - Namespaces: - "": true - Queue Size: 1 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /min_snap_visual/goal_point - Name: Goal_point - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /min_snap_visual/poly_traj - Name: plan_traj - Namespaces: - "": true - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.029999999329447746 - Name: drone_path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /odom_visualization/path - Unreliable: false - Value: true - Enabled: true - Name: Planning - - Class: rviz/Group - Displays: - - Alpha: 0.20000000298023224 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 85; 170; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: simulation_map - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Boxes - Topic: /map_generator/global_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.3399999141693115 - Min Value: 0.03999999910593033 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: map inflate - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /grid_map/occupancy_inflate - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.7592508792877197 - Min Value: -0.9228500127792358 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 187; 187; 187 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: real_map - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Boxes - Topic: /grid_map/occupancy - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: false - Name: Mapping - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization/robot - Name: robot - Namespaces: - mesh: true - Queue Size: 100 - Value: true - Enabled: true - Name: Simulation - Enabled: true - Global Options: - Background Color: 255; 253; 224 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz_plugins/Goal3DTool - Topic: /rviz/3d_nav_goal - Value: true - Views: - Current: - Class: rviz/ThirdPersonFollower - Distance: 9.049065589904785 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 2.164052724838257 - Y: -1.1675572395324707 - Z: 2.1457672119140625e-06 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.7747965455055237 - Target Frame: world - Yaw: 4.195437908172607 - Saved: - - Class: rviz/ThirdPersonFollower - Distance: 17.48538589477539 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: -16.308002471923828 - Y: 0.4492051601409912 - Z: 8.589673598180525e-06 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: ThirdPersonFollower - Near Clip Distance: 0.009999999776482582 - Pitch: 1.0347968339920044 - Target Frame: - Yaw: 3.150407314300537 -Window Geometry: - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000000a005600690065007700730000000114000001c9000000a400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000001d1000000b50000000000000000fb0000000a0049006d00610067006502000001a2000001e1000000f8000000b5fb0000000a0064006500700074006800000002da000001010000000000000000fb0000000a0049006d0061006700650100000415000000f80000000000000000fb0000000a0049006d00610067006501000003f4000001190000000000000000fb0000000a006400650070007400680100000459000000f50000000000000000000000010000010f00000385fc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006100000003bfc0100000002fb0000000800540069006d0065000000000000000610000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000005dc0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/launch/min_snap.launch b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/launch/min_snap.launch deleted file mode 100644 index ba2be9d9..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/launch/min_snap.launch +++ /dev/null @@ -1,50 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/launch/real.launch b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/launch/real.launch deleted file mode 100644 index b4caab8f..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/launch/real.launch +++ /dev/null @@ -1,50 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/launch/rviz.launch b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/launch/rviz.launch deleted file mode 100644 index af5eb8ba..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/launch/rviz.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/package.xml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/package.xml deleted file mode 100644 index 3f7026f2..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/package.xml +++ /dev/null @@ -1,71 +0,0 @@ - - - min_snap - 0.0.0 - The min_snap package - - - - - mywang - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - - mini_snap_traj_utils - mini_snap_traj_utils - - - - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/src/min_snap_closeform.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/src/min_snap_closeform.cpp deleted file mode 100644 index f84fdeeb..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/src/min_snap_closeform.cpp +++ /dev/null @@ -1,237 +0,0 @@ -#include - -namespace my_planner -{ - minsnapCloseform::minsnapCloseform(const vector &waypoints, double meanvel) - { - n_order = 7; - wps = waypoints; - n_seg = int(wps.size()) - 1; - n_per_seg = n_order + 1; - mean_vel = meanvel; - } - - void minsnapCloseform::Init(const vector &waypoints, double meanvel) - { - n_order = 7; - wps = waypoints; - n_seg = int(wps.size()) - 1; - n_per_seg = n_order + 1; - mean_vel = meanvel; - } - - // 0 means each segment time is one second. - void minsnapCloseform::init_ts(int init_type) - { - const double dist_min = 2.0; - ts = Eigen::VectorXd::Zero(n_seg); - if (init_type) - { - Eigen::VectorXd dist(n_seg); - double dist_sum = 0, t_sum = 0; - for (int i = 0; i < n_seg; i++) - { - dist(i) = 0; - for (int j = 0; j < 3; j++) - { - dist(i) += pow(wps[i + 1](j) - wps[i](j), 2); - } - dist(i) = sqrt(dist(i)); - if ((dist(i)) < dist_min) - { - dist(i) = sqrt(dist(i)) * 2; - } - dist_sum += dist(i); - } - dist(0) += 1; - dist(n_seg - 1) += 1; - dist_sum += 2; - double T = dist_sum / mean_vel; - for (int i = 0; i < n_seg - 1; i++) - { - ts(i) = dist(i) / dist_sum * T; - t_sum += ts(i); - } - ts(n_seg - 1) = T - t_sum; - } - else - { - for (int i = 0; i < n_seg; i++) - { - ts(i) = 1; - } - } - cout << "ts: " << ts.transpose() << endl; - } - - int minsnapCloseform::fact(int n) - { - if (n < 0) - { - cout << "ERROR fact(" << n << ")" << endl; - return 0; - } - else if (n == 0) - { - return 1; - } - else - { - return n * fact(n - 1); - } - } - - Eigen::VectorXd minsnapCloseform::calDecVel(const Eigen::VectorXd decvel) - { - Eigen::VectorXd temp(Eigen::VectorXd::Zero((n_seg + 1) * 4)); - for (int i = 0; i < (n_seg + 1) / 2; i++) - { - temp.segment(i * 8, 8) = decvel.segment((i * 2) * 8, 8); - } - if (n_seg % 2 != 1) - { - temp.tail(4) = decvel.tail(4); - } - - return temp; - } - - void minsnapCloseform::calQ() - { - Q = Eigen::MatrixXd::Zero(n_seg * (n_order + 1), n_seg * (n_order + 1)); - for (int k = 0; k < n_seg; k++) - { - Eigen::MatrixXd Q_k(Eigen::MatrixXd::Zero(n_order + 1, n_order + 1)); - for (int i = 5; i <= n_order + 1; i++) - { - for (int j = 5; j <= n_order + 1; j++) - { - Q_k(i - 1, j - 1) = fact(i) / fact(i - 4) * - fact(j) / fact(j - 4) / - (i + j - 7) * pow(ts(k), i + j - 7); - } - } - Q.block(k * (n_order + 1), k * (n_order + 1), n_order + 1, n_order + 1) = Q_k; - } - } - - void minsnapCloseform::calM() - { - M = Eigen::MatrixXd::Zero(n_seg * (n_order + 1), n_seg * (n_order + 1)); - for (int k = 0; k < n_seg; k++) - { - Eigen::MatrixXd M_k(Eigen::MatrixXd::Zero(n_order + 1, n_order + 1)); - M_k(0, 0) = 1; - M_k(1, 1) = 1; - M_k(2, 2) = 2; - M_k(3, 3) = 6; - for (int i = 0; i <= n_order; i++) - { - for (int j = 0; j <= 3; j++) - { - if (i >= j) - { - M_k(j + 4, i) = fact(i) / fact(i - j) * pow(ts(k), i - j); - } - } - } - M.block(k * (n_order + 1), k * (n_order + 1), n_order + 1, n_order + 1) = M_k; - } - } - - void minsnapCloseform::calCt() - { - int m = n_seg * (n_order + 1); - int n = 4 * (n_seg + 1); - Ct = Eigen::MatrixXd::Zero(m, n); - for (int i = 0; i < n_seg; i++) - { - Ct.block(i * (n_order + 1), i * 4, n_order + 1, n_order + 1) = - Eigen::MatrixXd::Identity(n_order + 1, n_order + 1); - } - - Eigen::MatrixXd dF_Ct = Eigen::MatrixXd::Zero(m, 8 + (n_seg - 1)); - Eigen::MatrixXd dP_Ct = Eigen::MatrixXd::Zero(m, (n_seg - 1) * 3); - dF_Ct.leftCols(4) = Ct.leftCols(4); - for (int i = 0; i < n_seg - 1; i++) - { - dF_Ct.col(i + 4) = Ct.col(i * 4 + 4); - dP_Ct.middleCols(i * 3, 3) = Ct.middleCols(i * 4 + 5, 3); - } - dF_Ct.rightCols(4) = Ct.rightCols(4); - Ct << dF_Ct, dP_Ct; - } - - std::pair minsnapCloseform::MinSnapCloseFormServer(const Eigen::VectorXd &wp) - { - std::pair return_vel; - Eigen::VectorXd poly_coef, dec_vel; - Eigen::VectorXd dF(n_seg + 7), dP(3 * (n_seg - 1)); - Eigen::VectorXd start_cond(4), end_cond(4), d(4 * n_seg + 4); - Eigen::MatrixXd R, R_pp, R_fp; - start_cond << wp.head(1), 0, 0, 0; - end_cond << wp.tail(1), 0, 0, 0; - init_ts(1); - - calQ(); - calM(); - calCt(); - - - R = Ct.transpose() * M.inverse().transpose() * Q * M.inverse() * Ct; - R_pp = R.bottomRightCorner(3 * (n_seg - 1), 3 * (n_seg - 1)); - R_fp = R.topRightCorner(n_seg + 7, 3 * (n_seg - 1)); - - dF << start_cond, wp.segment(1, n_seg - 1), end_cond; - dP = -R_pp.inverse() * R_fp.transpose() * dF; - d << dF, dP; - - dec_vel = Ct * d; - poly_coef = M.inverse() * dec_vel; - - return_vel.first = poly_coef; - return_vel.second = dec_vel; - return return_vel; - } - - void minsnapCloseform::calMinsnap_polycoef() - { - Eigen::VectorXd wps_x(n_seg + 1), wps_y(n_seg + 1), wps_z(n_seg + 1); - for (int i = 0; i < n_seg + 1; i++) - { - wps_x(i) = wps[i](0); - wps_y(i) = wps[i](1); - wps_z(i) = wps[i](2); - } - std::pair return_vel; - return_vel = MinSnapCloseFormServer(wps_x); - poly_coef_x = return_vel.first; - dec_vel_x = calDecVel(return_vel.second); - return_vel = MinSnapCloseFormServer(wps_y); - poly_coef_y = return_vel.first; - dec_vel_y = calDecVel(return_vel.second); - return_vel = MinSnapCloseFormServer(wps_z); - poly_coef_z = return_vel.first; - dec_vel_z = calDecVel(return_vel.second); - } - - Eigen::MatrixXd minsnapCloseform::getPolyCoef() - { - Eigen::MatrixXd poly_coef(poly_coef_x.size(), 3); - poly_coef << poly_coef_x, poly_coef_y, poly_coef_z; - return poly_coef; - } - - Eigen::MatrixXd minsnapCloseform::getDecVel() - { - Eigen::MatrixXd dec_vel(dec_vel_x.size(), 3); - dec_vel << dec_vel_x, dec_vel_y, dec_vel_z; - return dec_vel; - } - - Eigen::VectorXd minsnapCloseform::getTime() - { - return ts; - } - -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/src/min_snap_generator.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/src/min_snap_generator.cpp deleted file mode 100644 index 36f9cd26..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/min_snap/src/min_snap_generator.cpp +++ /dev/null @@ -1,137 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include "min_snap/min_snap_closeform.h" - -ros::Publisher goal_list_pub; -ros::Publisher poly_coef_pub; -ros::Subscriber rviz_goal_sub; -ros::Subscriber odom_sub; - -int id = 0; -double mean_vel = 0; -const int GOAL_HEIGHT = 1; -nav_msgs::Odometry odom; -geometry_msgs::Pose goal_pt; -geometry_msgs::PoseArray goal_list; -my_planner::minsnapCloseform minsnap_solver; -std::vector waypoints; -quadrotor_msgs::PolynomialTrajectory poly_pub_topic; - -void pub_poly_coefs() -{ - Eigen::MatrixXd poly_coef = minsnap_solver.getPolyCoef(); - Eigen::MatrixXd dec_vel = minsnap_solver.getDecVel(); - Eigen::VectorXd time = minsnap_solver.getTime(); - - poly_pub_topic.num_segment = goal_list.poses.size() - 1; - poly_pub_topic.coef_x.clear(); - poly_pub_topic.coef_y.clear(); - poly_pub_topic.coef_z.clear(); - poly_pub_topic.time.clear(); - poly_pub_topic.trajectory_id = id; - - // display decision variable - ROS_WARN("decision variable:"); - for (int i = 0; i < goal_list.poses.size(); i++) - { - cout << "Point number = " << i + 1 << endl - << dec_vel.middleRows(i * 4, 4) << endl; - } - - for (int i = 0; i < time.size(); i++) - { - for (int j = (i + 1) * 8 - 1; j >= i * 8; j--) - { - poly_pub_topic.coef_x.push_back(poly_coef(j, 0)); - poly_pub_topic.coef_y.push_back(poly_coef(j, 1)); - poly_pub_topic.coef_z.push_back(poly_coef(j, 2)); - } - poly_pub_topic.time.push_back(time(i)); - } - - poly_pub_topic.header.frame_id = "world"; - poly_pub_topic.header.stamp = ros::Time::now(); - - poly_coef_pub.publish(poly_pub_topic); -} - -void solve_min_snap() -{ - Eigen::Vector3d wp; - waypoints.clear(); - for (int i = 0; i < int(goal_list.poses.size()); i++) - { - wp << goal_list.poses[i].position.x, goal_list.poses[i].position.y, goal_list.poses[i].position.z; - waypoints.push_back(wp); - } - minsnap_solver.Init(waypoints, mean_vel); - ROS_INFO("Init success"); - minsnap_solver.calMinsnap_polycoef(); - pub_poly_coefs(); -} - -void rviz_goal_cb(const geometry_msgs::PoseStamped::ConstPtr &msg) -{ - goal_pt = msg->pose; - if (goal_pt.position.z < 0) - { - goal_pt.position.z = GOAL_HEIGHT; - goal_list.poses.push_back(goal_pt); - goal_pt.position = odom.pose.pose.position; - goal_list.poses.insert(goal_list.poses.begin(), goal_pt); - goal_list.header.stamp = ros::Time::now(); - goal_list.header.frame_id = "world"; - goal_list.header.seq = id++; - goal_list_pub.publish(goal_list); - solve_min_snap(); - ROS_INFO("solver finished"); - goal_list.poses.clear(); - } - else - { - goal_pt.position.z = GOAL_HEIGHT; - goal_list.poses.push_back(goal_pt); - } -} - -void odom_goal_cb(const nav_msgs::Odometry::ConstPtr &msg) -{ - odom = *msg; -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "min_snap_generator"); - ros::NodeHandle nh("~"); - ros::Rate rate(10); - - // 【订阅】里程计（此处注意话题名称替换） - odom_sub = nh.subscribe("/odom_topic", 10, odom_goal_cb); - // 【订阅】RVIZ目标点 - rviz_goal_sub = nh.subscribe("/rviz_goal", 10, rviz_goal_cb); - // 【发布】目标点，用于目标点显示 - goal_list_pub = nh.advertise("/goal_list", 10); - // 发布多项式轨迹 - poly_coef_pub = nh.advertise("/poly_coefs", 10); - - poly_pub_topic.num_order = 7; - poly_pub_topic.start_yaw = 0; - poly_pub_topic.final_yaw = 0; - poly_pub_topic.mag_coeff = 0; - poly_pub_topic.order.push_back(0); - - ros::param::get("/min_snap_generator/meanvel", mean_vel); - - while (ros::ok()) - { - ros::spinOnce(); - rate.sleep(); - } - - return 0; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/readme.pdf b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/readme.pdf deleted file mode 100644 index fff19f07..00000000 Binary files a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/readme.pdf and /dev/null differ diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_server/CMakeLists.txt b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_server/CMakeLists.txt deleted file mode 100644 index 4dbb93c4..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_server/CMakeLists.txt +++ /dev/null @@ -1,217 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(traj_server) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages - -find_package(Eigen3 REQUIRED) - -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - geometry_msgs - quadrotor_msgs - mini_snap_traj_utils -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES traj_server -# CATKIN_DEPENDS roscpp rospy std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/traj_server.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/traj_server_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -add_executable(my_traj_server src/my_traj_server.cpp) -target_link_libraries(my_traj_server ${catkin_LIBRARIES}) -# add_dependencies(my_traj_server ${${PROJECT_NAME}_EXPORTED_TARGETS}) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_traj_server.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_server/launch/my_sim.xml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_server/launch/my_sim.xml deleted file mode 100644 index bd32ade7..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_server/launch/my_sim.xml +++ /dev/null @@ -1,140 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_server/launch/real_traj_server.xml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_server/launch/real_traj_server.xml deleted file mode 100644 index 6c56f0ed..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_server/launch/real_traj_server.xml +++ /dev/null @@ -1,110 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_server/package.xml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_server/package.xml deleted file mode 100644 index 45d9012d..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_server/package.xml +++ /dev/null @@ -1,70 +0,0 @@ - - - traj_server - 0.0.0 - The traj_server package - - - - - mywang - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - - mini_snap_traj_utils - mini_snap_traj_utils - - - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_server/src/my_traj_server.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_server/src/my_traj_server.cpp deleted file mode 100644 index 5ef4a4a7..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_server/src/my_traj_server.cpp +++ /dev/null @@ -1,264 +0,0 @@ -#include -#include "quadrotor_msgs/PositionCommand.h" -#include "quadrotor_msgs/PolynomialTrajectory.h" -#include "mini_snap_traj_utils/polynomial_traj.h" -#include - -ros::Publisher pose_cmd_pub; -ros::Publisher traj_pts_pub; -ros::Subscriber poly_traj_sub; - -PolynomialTraj Poly_traj; -quadrotor_msgs::PositionCommand cmd; -ros::Time start_time; -double pos_gain[3] = {0, 0, 0}; -double vel_gain[3] = {0, 0, 0}; - -double last_yaw_, last_yaw_dot_; -bool receive_traj_ = false; -int traj_id_; -double traj_duration_, time_forward_; - -std::pair calculate_yaw(double t_cur, Eigen::Vector3d &pos, ros::Time &time_now, ros::Time &time_last) -{ - constexpr double PI = 3.1415926; - constexpr double YAW_DOT_MAX_PER_SEC = PI; - // constexpr double YAW_DOT_DOT_MAX_PER_SEC = PI; - std::pair yaw_yawdot(0, 0); - double yaw = 0; - double yawdot = 0; - - // Eigen::Vector3d dir = t_cur + time_forward_ <= traj_duration_ ? traj_[0].evaluateDeBoorT(t_cur + time_forward_) - pos : traj_[0].evaluateDeBoorT(traj_duration_) - pos; - Eigen::Vector3d dir = t_cur + time_forward_ <= traj_duration_ ? Poly_traj.evaluate(t_cur + time_forward_) - pos : Poly_traj.evaluate(traj_duration_) - pos; - double yaw_temp = dir.norm() > 0.1 ? atan2(dir(1), dir(0)) : last_yaw_; - double max_yaw_change = YAW_DOT_MAX_PER_SEC * (time_now - time_last).toSec(); - if (yaw_temp - last_yaw_ > PI) - { - if (yaw_temp - last_yaw_ - 2 * PI < -max_yaw_change) - { - yaw = last_yaw_ - max_yaw_change; - if (yaw < -PI) - yaw += 2 * PI; - - yawdot = -YAW_DOT_MAX_PER_SEC; - } - else - { - yaw = yaw_temp; - if (yaw - last_yaw_ > PI) - yawdot = -YAW_DOT_MAX_PER_SEC; - else - yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec(); - } - } - else if (yaw_temp - last_yaw_ < -PI) - { - if (yaw_temp - last_yaw_ + 2 * PI > max_yaw_change) - { - yaw = last_yaw_ + max_yaw_change; - if (yaw > PI) - yaw -= 2 * PI; - - yawdot = YAW_DOT_MAX_PER_SEC; - } - else - { - yaw = yaw_temp; - if (yaw - last_yaw_ < -PI) - yawdot = YAW_DOT_MAX_PER_SEC; - else - yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec(); - } - } - else - { - if (yaw_temp - last_yaw_ < -max_yaw_change) - { - yaw = last_yaw_ - max_yaw_change; - if (yaw < -PI) - yaw += 2 * PI; - - yawdot = -YAW_DOT_MAX_PER_SEC; - } - else if (yaw_temp - last_yaw_ > max_yaw_change) - { - yaw = last_yaw_ + max_yaw_change; - if (yaw > PI) - yaw -= 2 * PI; - - yawdot = YAW_DOT_MAX_PER_SEC; - } - else - { - yaw = yaw_temp; - if (yaw - last_yaw_ > PI) - yawdot = -YAW_DOT_MAX_PER_SEC; - else if (yaw - last_yaw_ < -PI) - yawdot = YAW_DOT_MAX_PER_SEC; - else - yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec(); - } - } - - if (fabs(yaw - last_yaw_) <= max_yaw_change) - yaw = 0.5 * last_yaw_ + 0.5 * yaw; // nieve LPF - yawdot = 0.5 * last_yaw_dot_ + 0.5 * yawdot; - last_yaw_ = yaw; - last_yaw_dot_ = yawdot; - - yaw_yawdot.first = yaw; - yaw_yawdot.second = yawdot; - - return yaw_yawdot; -} - -void polyTrajCallback(quadrotor_msgs::PolynomialTrajectory::ConstPtr msg) -{ - ROS_INFO("[my Traj server]:receive poly_traj"); - - Poly_traj.reset(); - int idx; - for (int i = 0; i < msg->num_segment; i++) - { - vector cx, cy, cz; - for (int j = 0; j < msg->num_order + 1; j++) - { - idx = i * (msg->num_order + 1) + j; - cx.push_back(msg->coef_x[idx]); - cy.push_back(msg->coef_y[idx]); - cz.push_back(msg->coef_z[idx]); - } - Poly_traj.addSegment(cx, cy, cz, msg->time[i]); - } - Poly_traj.init(); - - // ROS_INFO("[my traj server]:time=%f", Poly_traj.getTimes().front()); - - start_time = ros::Time::now(); - traj_id_ = msg->trajectory_id; - traj_duration_ = Poly_traj.getTimeSum(); - receive_traj_ = true; -} - -void pub_traj(double t_cur) -{ - static int old_cnt = 0; - int cnt = (int)((traj_duration_ - t_cur) * 2) + 1; - - if (cnt != old_cnt) - { - geometry_msgs::PoseArray traj_pts; - geometry_msgs::Pose traj_pt; - Eigen::Vector3d opt_pt(Eigen::Vector3d::Zero()); - - traj_pts.header.stamp = ros::Time::now(); - for (int i = 0; i < cnt + 1; i++) - { - opt_pt = Poly_traj.evaluate(std::min(t_cur + i * 0.5, traj_duration_)); - traj_pt.orientation.w = 1.0; - traj_pt.position.x = opt_pt(0); - traj_pt.position.y = opt_pt(1); - traj_pt.position.z = opt_pt(2); - traj_pts.poses.push_back(traj_pt); - } - traj_pts_pub.publish(traj_pts); - } - old_cnt = cnt; -} - -void cmdCallback(const ros::TimerEvent &e) -{ - if (!receive_traj_) - return; - - Eigen::Vector3d pos(Eigen::Vector3d::Zero()); - Eigen::Vector3d vel(Eigen::Vector3d::Zero()); - Eigen::Vector3d acc(Eigen::Vector3d::Zero()); - Eigen::Vector3d jerk(Eigen::Vector3d::Zero()); - std::pair yaw_yawdot(0, 0); - - ros::Time time_now = ros::Time::now(); - double t_cur = (time_now - start_time).toSec(); - // ROS_INFO("time = %f", t_cur); - static ros::Time time_last = ros::Time::now(); - - if (t_cur < traj_duration_ && t_cur >= 0.0) - { - pos = Poly_traj.evaluate(t_cur); - vel = Poly_traj.evaluateVel(t_cur); - acc = Poly_traj.evaluateAcc(t_cur); - jerk = Poly_traj.evaluateJerk(t_cur); - yaw_yawdot = calculate_yaw(t_cur, pos, time_now, time_last); - pub_traj(t_cur + 0.5); - } - else if (t_cur >= traj_duration_) - { - pos = Poly_traj.evaluate(traj_duration_); - yaw_yawdot.first = last_yaw_; - yaw_yawdot.second = 0; - } - else - { - ROS_WARN("[my Traj server]: invalid time."); - } - time_last = time_now; - - cmd.header.stamp = time_now; - cmd.header.frame_id = "world"; - cmd.trajectory_id = traj_id_; - - cmd.position.x = pos(0); - cmd.position.y = pos(1); - cmd.position.z = pos(2); - - cmd.velocity.x = vel(0); - cmd.velocity.y = vel(1); - cmd.velocity.z = vel(2); - - cmd.acceleration.x = acc(0); - cmd.acceleration.y = acc(1); - cmd.acceleration.z = acc(2); - - cmd.jerk.x = jerk(0); - cmd.jerk.y = jerk(1); - cmd.jerk.z = jerk(2); - - cmd.yaw = yaw_yawdot.first; - cmd.yaw_dot = yaw_yawdot.second; - - last_yaw_ = cmd.yaw; - - pose_cmd_pub.publish(cmd); -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "my_traj_server"); - ros::NodeHandle nh("~"); - - pose_cmd_pub = nh.advertise("/position_cmd", 50); - traj_pts_pub = nh.advertise("/traj_pts", 50); - poly_traj_sub = nh.subscribe("/poly_coefs", 10, polyTrajCallback); - - ros::Timer cmd_timer = nh.createTimer(ros::Duration(0.01), cmdCallback); - - /* control parameter */ - cmd.kx[0] = pos_gain[0]; - cmd.kx[1] = pos_gain[1]; - cmd.kx[2] = pos_gain[2]; - - cmd.kv[0] = vel_gain[0]; - cmd.kv[1] = vel_gain[1]; - cmd.kv[2] = vel_gain[2]; - last_yaw_ = 0.0; - last_yaw_dot_ = 0.0; - time_forward_ = 1.0; - - ros::Duration(1.0).sleep(); - - ROS_INFO("[my Traj server]: ready."); - - ros::spin(); - - return 0; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/CMakeLists.txt b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/CMakeLists.txt deleted file mode 100644 index 7e57d4b1..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(mini_snap_traj_utils) - -set(CMAKE_BUILD_TYPE "Release") -set(CMAKE_CXX_FLAGS "-std=c++11") -set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") - -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - cv_bridge -) - -find_package(Eigen3 REQUIRED) -find_package(PCL 1.7 REQUIRED) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES mini_snap_traj_utils -# DEPENDS system_lib -) - -include_directories( - SYSTEM - include - ${catkin_INCLUDE_DIRS} - ${Eigen3_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -link_directories(${PCL_LIBRARY_DIRS}) - -add_library( mini_snap_traj_utils - src/planning_visualization.cpp - src/polynomial_traj.cpp - ) -target_link_libraries( mini_snap_traj_utils - ${catkin_LIBRARIES} - ) diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/include/mini_snap_traj_utils/planning_visualization.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/include/mini_snap_traj_utils/planning_visualization.h deleted file mode 100644 index a38f479f..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/include/mini_snap_traj_utils/planning_visualization.h +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef _PLANNING_VISUALIZATION_H_ -#define _PLANNING_VISUALIZATION_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using std::vector; -namespace my_planner -{ - class PlanningVisualization - { - private: - ros::NodeHandle node; - - ros::Publisher goal_point_pub; - ros::Publisher global_list_pub; - ros::Publisher init_list_pub; - ros::Publisher optimal_list_pub; - ros::Publisher a_star_list_pub; - ros::Publisher guide_vector_pub; - ros::Publisher intermediate_state_pub; - - public: - PlanningVisualization(/* args */) {} - ~PlanningVisualization() {} - PlanningVisualization(ros::NodeHandle &nh); - - void Init(ros::NodeHandle &nh); - typedef std::shared_ptr Ptr; - - void displayMarkerList(ros::Publisher &pub, const vector &list, double scale, - Eigen::Vector4d color, int id); - void generatePathDisplayArray(visualization_msgs::MarkerArray &array, - const vector &list, double scale, Eigen::Vector4d color, int id); - void generateArrowDisplayArray(visualization_msgs::MarkerArray &array, - const vector &list, double scale, Eigen::Vector4d color, int id); - void displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id); - void displayGlobalPathList(vector global_pts, const double scale, int id); - void displayInitPathList(vector init_pts, const double scale, int id); - void displayOptimalList(Eigen::MatrixXd optimal_pts, int id); - void displayAStarList(std::vector> a_star_paths, int id); - void displayArrowList(ros::Publisher &pub, const vector &list, double scale, Eigen::Vector4d color, int id); - // void displayIntermediateState(ros::Publisher& intermediate_pub, ego_planner::BsplineOptimizer::Ptr optimizer, double sleep_time, const int start_iteration); - // void displayNewArrow(ros::Publisher& guide_vector_pub, ego_planner::BsplineOptimizer::Ptr optimizer); - }; -} // namespace ego_planner -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/include/mini_snap_traj_utils/polynomial_traj.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/include/mini_snap_traj_utils/polynomial_traj.h deleted file mode 100644 index fe9f1610..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/include/mini_snap_traj_utils/polynomial_traj.h +++ /dev/null @@ -1,368 +0,0 @@ -#ifndef _POLYNOMIAL_TRAJ_H -#define _POLYNOMIAL_TRAJ_H - -#include -#include - -using std::vector; - -class PolynomialTraj -{ -private: - vector times; // time of each segment - vector> cxs; // coefficient of x of each segment, from n-1 -> 0 - vector> cys; // coefficient of y of each segment - vector> czs; // coefficient of z of each segment - - double time_sum; - int num_seg; - - /* evaluation */ - vector traj_vec3d; - double length; - -public: - PolynomialTraj(/* args */) - { - } - ~PolynomialTraj() - { - } - - void reset() - { - times.clear(), cxs.clear(), cys.clear(), czs.clear(); - time_sum = 0.0, num_seg = 0; - } - - void addSegment(vector cx, vector cy, vector cz, double t) - { - cxs.push_back(cx), cys.push_back(cy), czs.push_back(cz), times.push_back(t); - } - - void init() - { - num_seg = times.size(); - time_sum = 0.0; - for (int i = 0; i < times.size(); ++i) - { - time_sum += times[i]; - } - } - - vector getTimes() - { - return times; - } - - vector> getCoef(int axis) - { - switch (axis) - { - case 0: - return cxs; - case 1: - return cys; - case 2: - return czs; - default: - std::cout << "\033[31mIllegal axis!\033[0m" << std::endl; - } - - vector> empty; - return empty; - } - - Eigen::Vector3d evaluate(double t) - { - /* detetrmine segment num */ - int idx = 0; - while (times[idx] + 1e-4 < t) - { - t -= times[idx]; - ++idx; - } - - /* evaluation */ - int order = cxs[idx].size(); - Eigen::VectorXd cx(order), cy(order), cz(order), tv(order); - for (int i = 0; i < order; ++i) - { - cx(i) = cxs[idx][i], cy(i) = cys[idx][i], cz(i) = czs[idx][i]; - tv(order - 1 - i) = std::pow(t, double(i)); - } - - Eigen::Vector3d pt; - pt(0) = tv.dot(cx), pt(1) = tv.dot(cy), pt(2) = tv.dot(cz); - return pt; - } - - Eigen::Vector3d evaluateVel(double t) - { - /* detetrmine segment num */ - int idx = 0; - while (times[idx] + 1e-4 < t) - { - t -= times[idx]; - ++idx; - } - - /* evaluation */ - int order = cxs[idx].size(); - Eigen::VectorXd vx(order - 1), vy(order - 1), vz(order - 1); - - /* coef of vel */ - for (int i = 0; i < order - 1; ++i) - { - vx(i) = double(i + 1) * cxs[idx][order - 2 - i]; - vy(i) = double(i + 1) * cys[idx][order - 2 - i]; - vz(i) = double(i + 1) * czs[idx][order - 2 - i]; - } - double ts = t; - Eigen::VectorXd tv(order - 1); - for (int i = 0; i < order - 1; ++i) - tv(i) = pow(ts, i); - - Eigen::Vector3d vel; - vel(0) = tv.dot(vx), vel(1) = tv.dot(vy), vel(2) = tv.dot(vz); - return vel; - } - - Eigen::Vector3d evaluateAcc(double t) - { - /* detetrmine segment num */ - int idx = 0; - while (times[idx] + 1e-4 < t) - { - t -= times[idx]; - ++idx; - } - - /* evaluation */ - int order = cxs[idx].size(); - Eigen::VectorXd ax(order - 2), ay(order - 2), az(order - 2); - - /* coef of vel */ - for (int i = 0; i < order - 2; ++i) - { - ax(i) = double((i + 2) * (i + 1)) * cxs[idx][order - 3 - i]; - ay(i) = double((i + 2) * (i + 1)) * cys[idx][order - 3 - i]; - az(i) = double((i + 2) * (i + 1)) * czs[idx][order - 3 - i]; - } - double ts = t; - Eigen::VectorXd tv(order - 2); - for (int i = 0; i < order - 2; ++i) - tv(i) = pow(ts, i); - - Eigen::Vector3d acc; - acc(0) = tv.dot(ax), acc(1) = tv.dot(ay), acc(2) = tv.dot(az); - return acc; - } - - Eigen::Vector3d evaluateJerk(double t) - { - /* detetrmine segment num */ - int idx = 0; - while (times[idx] + 1e-4 < t) - { - t -= times[idx]; - ++idx; - } - - /* evaluation */ - int order = cxs[idx].size(); - Eigen::VectorXd jx(order - 3), jy(order - 3), jz(order - 3); - - /* coef of vel */ - for (int i = 0; i < order - 3; ++i) - { - jx(i) = double((i + 3) * (i + 2) * (i + 1)) * cxs[idx][order - 4 - i]; - jy(i) = double((i + 3) * (i + 2) * (i + 1)) * cys[idx][order - 4 - i]; - jz(i) = double((i + 3) * (i + 2) * (i + 1)) * czs[idx][order - 4 - i]; - } - double ts = t; - Eigen::VectorXd tv(order - 3); - for (int i = 0; i < order - 3; ++i) - tv(i) = pow(ts, i); - - Eigen::Vector3d jerk; - jerk(0) = tv.dot(jx), jerk(1) = tv.dot(jy), jerk(2) = tv.dot(jz); - return jerk; - } - - /* for evaluating traj, should be called in sequence!!! */ - double getTimeSum() - { - return this->time_sum; - } - - vector getTraj() - { - double eval_t = 0.0; - traj_vec3d.clear(); - while (eval_t < time_sum) - { - Eigen::Vector3d pt = evaluate(eval_t); - traj_vec3d.push_back(pt); - eval_t += 0.01; - } - return traj_vec3d; - } - - double getLength() - { - length = 0.0; - - Eigen::Vector3d p_l = traj_vec3d[0], p_n; - for (int i = 1; i < traj_vec3d.size(); ++i) - { - p_n = traj_vec3d[i]; - length += (p_n - p_l).norm(); - p_l = p_n; - } - return length; - } - - double getMeanVel() - { - double mean_vel = length / time_sum; - return mean_vel; - } - - double getAccCost() - { - double cost = 0.0; - int order = cxs[0].size(); - - for (int s = 0; s < times.size(); ++s) - { - Eigen::Vector3d um; - um(0) = 2 * cxs[s][order - 3], um(1) = 2 * cys[s][order - 3], um(2) = 2 * czs[s][order - 3]; - cost += um.squaredNorm() * times[s]; - } - - return cost; - } - - double getJerk() - { - double jerk = 0.0; - - /* evaluate jerk */ - for (int s = 0; s < times.size(); ++s) - { - Eigen::VectorXd cxv(cxs[s].size()), cyv(cys[s].size()), czv(czs[s].size()); - /* convert coefficient */ - int order = cxs[s].size(); - for (int j = 0; j < order; ++j) - { - cxv(j) = cxs[s][order - 1 - j], cyv(j) = cys[s][order - 1 - j], czv(j) = czs[s][order - 1 - j]; - } - double ts = times[s]; - - /* jerk matrix */ - Eigen::MatrixXd mat_jerk(order, order); - mat_jerk.setZero(); - for (double i = 3; i < order; i += 1) - for (double j = 3; j < order; j += 1) - { - mat_jerk(i, j) = - i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) * pow(ts, i + j - 5) / (i + j - 5); - } - - jerk += (cxv.transpose() * mat_jerk * cxv)(0, 0); - jerk += (cyv.transpose() * mat_jerk * cyv)(0, 0); - jerk += (czv.transpose() * mat_jerk * czv)(0, 0); - } - - return jerk; - } - - void getMeanAndMaxVel(double &mean_v, double &max_v) - { - int num = 0; - mean_v = 0.0, max_v = -1.0; - for (int s = 0; s < times.size(); ++s) - { - int order = cxs[s].size(); - Eigen::VectorXd vx(order - 1), vy(order - 1), vz(order - 1); - - /* coef of vel */ - for (int i = 0; i < order - 1; ++i) - { - vx(i) = double(i + 1) * cxs[s][order - 2 - i]; - vy(i) = double(i + 1) * cys[s][order - 2 - i]; - vz(i) = double(i + 1) * czs[s][order - 2 - i]; - } - double ts = times[s]; - - double eval_t = 0.0; - while (eval_t < ts) - { - Eigen::VectorXd tv(order - 1); - for (int i = 0; i < order - 1; ++i) - tv(i) = pow(ts, i); - Eigen::Vector3d vel; - vel(0) = tv.dot(vx), vel(1) = tv.dot(vy), vel(2) = tv.dot(vz); - double vn = vel.norm(); - mean_v += vn; - if (vn > max_v) - max_v = vn; - ++num; - - eval_t += 0.01; - } - } - - mean_v = mean_v / double(num); - } - - void getMeanAndMaxAcc(double &mean_a, double &max_a) - { - int num = 0; - mean_a = 0.0, max_a = -1.0; - for (int s = 0; s < times.size(); ++s) - { - int order = cxs[s].size(); - Eigen::VectorXd ax(order - 2), ay(order - 2), az(order - 2); - - /* coef of acc */ - for (int i = 0; i < order - 2; ++i) - { - ax(i) = double((i + 2) * (i + 1)) * cxs[s][order - 3 - i]; - ay(i) = double((i + 2) * (i + 1)) * cys[s][order - 3 - i]; - az(i) = double((i + 2) * (i + 1)) * czs[s][order - 3 - i]; - } - double ts = times[s]; - - double eval_t = 0.0; - while (eval_t < ts) - { - Eigen::VectorXd tv(order - 2); - for (int i = 0; i < order - 2; ++i) - tv(i) = pow(ts, i); - Eigen::Vector3d acc; - acc(0) = tv.dot(ax), acc(1) = tv.dot(ay), acc(2) = tv.dot(az); - double an = acc.norm(); - mean_a += an; - if (an > max_a) - max_a = an; - ++num; - - eval_t += 0.01; - } - } - - mean_a = mean_a / double(num); - } - - static PolynomialTraj minSnapTraj(const Eigen::MatrixXd &Pos, const Eigen::Vector3d &start_vel, - const Eigen::Vector3d &end_vel, const Eigen::Vector3d &start_acc, - const Eigen::Vector3d &end_acc, const Eigen::VectorXd &Time); - - static PolynomialTraj one_segment_traj_gen(const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc, - const Eigen::Vector3d &end_pt, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc, - double t); -}; - -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/package.xml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/package.xml deleted file mode 100644 index 6773176b..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/package.xml +++ /dev/null @@ -1,67 +0,0 @@ - - - mini_snap_traj_utils - 0.0.0 - The mini_snap_traj_utils package - - - - - bzhouai - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - std_msgs - - roscpp - std_msgs - - roscpp - std_msgs - - - - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/src/planning_visualization.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/src/planning_visualization.cpp deleted file mode 100644 index 3604f493..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/src/planning_visualization.cpp +++ /dev/null @@ -1,253 +0,0 @@ -#include - -using std::cout; -using std::endl; -namespace my_planner -{ - PlanningVisualization::PlanningVisualization(ros::NodeHandle &nh) - { - node = nh; - - goal_point_pub = nh.advertise("goal_point", 2); - global_list_pub = nh.advertise("global_list", 2); - init_list_pub = nh.advertise("init_list", 2); - optimal_list_pub = nh.advertise("optimal_list", 2); - a_star_list_pub = nh.advertise("a_star_list", 20); - } - - void PlanningVisualization::Init(ros::NodeHandle &nh) - { - node = nh; - - goal_point_pub = nh.advertise("goal_point", 2); - global_list_pub = nh.advertise("global_list", 2); - init_list_pub = nh.advertise("init_list", 2); - optimal_list_pub = nh.advertise("optimal_list", 2); - a_star_list_pub = nh.advertise("a_star_list", 20); - } - - // // real ids used: {id, id+1000} - void PlanningVisualization::displayMarkerList(ros::Publisher &pub, const vector &list, double scale, - Eigen::Vector4d color, int id) - { - visualization_msgs::Marker sphere, line_strip; - sphere.header.frame_id = line_strip.header.frame_id = "world"; - sphere.header.stamp = line_strip.header.stamp = ros::Time::now(); - sphere.type = visualization_msgs::Marker::SPHERE_LIST; - line_strip.type = visualization_msgs::Marker::LINE_STRIP; - sphere.action = line_strip.action = visualization_msgs::Marker::ADD; - sphere.id = id; - line_strip.id = id + 1000; - - sphere.pose.orientation.w = line_strip.pose.orientation.w = 1.0; - sphere.color.r = line_strip.color.r = color(0); - sphere.color.g = line_strip.color.g = color(1); - sphere.color.b = line_strip.color.b = color(2); - sphere.color.a = line_strip.color.a = color(3) > 1e-5 ? color(3) : 1.0; - sphere.scale.x = scale; - sphere.scale.y = scale; - sphere.scale.z = scale; - line_strip.scale.x = scale / 2; - geometry_msgs::Point pt; - for (int i = 0; i < int(list.size()); i++) - { - pt.x = list[i](0); - pt.y = list[i](1); - pt.z = list[i](2); - sphere.points.push_back(pt); - line_strip.points.push_back(pt); - } - pub.publish(sphere); - pub.publish(line_strip); - } - - // real ids used: {id, id+1} - void PlanningVisualization::generatePathDisplayArray(visualization_msgs::MarkerArray &array, - const vector &list, double scale, Eigen::Vector4d color, int id) - { - visualization_msgs::Marker sphere, line_strip; - sphere.header.frame_id = line_strip.header.frame_id = "map"; - sphere.header.stamp = line_strip.header.stamp = ros::Time::now(); - sphere.type = visualization_msgs::Marker::SPHERE_LIST; - line_strip.type = visualization_msgs::Marker::LINE_STRIP; - sphere.action = line_strip.action = visualization_msgs::Marker::ADD; - sphere.id = id; - line_strip.id = id + 1; - - sphere.pose.orientation.w = line_strip.pose.orientation.w = 1.0; - sphere.color.r = line_strip.color.r = color(0); - sphere.color.g = line_strip.color.g = color(1); - sphere.color.b = line_strip.color.b = color(2); - sphere.color.a = line_strip.color.a = color(3) > 1e-5 ? color(3) : 1.0; - sphere.scale.x = scale; - sphere.scale.y = scale; - sphere.scale.z = scale; - line_strip.scale.x = scale / 3; - geometry_msgs::Point pt; - for (int i = 0; i < int(list.size()); i++) - { - pt.x = list[i](0); - pt.y = list[i](1); - pt.z = list[i](2); - sphere.points.push_back(pt); - line_strip.points.push_back(pt); - } - array.markers.push_back(sphere); - array.markers.push_back(line_strip); - } - - // real ids used: {1000*id ~ (arrow nums)+1000*id} - void PlanningVisualization::generateArrowDisplayArray(visualization_msgs::MarkerArray &array, - const vector &list, double scale, Eigen::Vector4d color, int id) - { - visualization_msgs::Marker arrow; - arrow.header.frame_id = "map"; - arrow.header.stamp = ros::Time::now(); - arrow.type = visualization_msgs::Marker::ARROW; - arrow.action = visualization_msgs::Marker::ADD; - - // geometry_msgs::Point start, end; - // arrow.points - - arrow.color.r = color(0); - arrow.color.g = color(1); - arrow.color.b = color(2); - arrow.color.a = color(3) > 1e-5 ? color(3) : 1.0; - arrow.scale.x = scale; - arrow.scale.y = 2 * scale; - arrow.scale.z = 2 * scale; - - geometry_msgs::Point start, end; - for (int i = 0; i < int(list.size() / 2); i++) - { - // arrow.color.r = color(0) / (1+i); - // arrow.color.g = color(1) / (1+i); - // arrow.color.b = color(2) / (1+i); - - start.x = list[2 * i](0); - start.y = list[2 * i](1); - start.z = list[2 * i](2); - end.x = list[2 * i + 1](0); - end.y = list[2 * i + 1](1); - end.z = list[2 * i + 1](2); - arrow.points.clear(); - arrow.points.push_back(start); - arrow.points.push_back(end); - arrow.id = i + id * 1000; - - array.markers.push_back(arrow); - } - } - - void PlanningVisualization::displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id) - { - visualization_msgs::Marker sphere; - sphere.header.frame_id = "world"; - sphere.header.stamp = ros::Time::now(); - sphere.type = visualization_msgs::Marker::SPHERE; - sphere.action = visualization_msgs::Marker::ADD; - sphere.id = id; - - sphere.pose.orientation.w = 1.0; - sphere.color.r = color(0); - sphere.color.g = color(1); - sphere.color.b = color(2); - sphere.color.a = color(3); - sphere.scale.x = scale; - sphere.scale.y = scale; - sphere.scale.z = scale; - sphere.pose.position.x = goal_point(0); - sphere.pose.position.y = goal_point(1); - sphere.pose.position.z = goal_point(2); - - goal_point_pub.publish(sphere); - } - - void PlanningVisualization::displayGlobalPathList(vector init_pts, const double scale, int id) - { - - if (global_list_pub.getNumSubscribers() == 0) - { - return; - } - - Eigen::Vector4d color(0, 0.5, 0.5, 1); - displayMarkerList(global_list_pub, init_pts, scale, color, id); - } - - void PlanningVisualization::displayInitPathList(vector init_pts, const double scale, int id) - { - - if (init_list_pub.getNumSubscribers() == 0) - { - return; - } - - Eigen::Vector4d color(0, 0, 1, 1); - displayMarkerList(init_list_pub, init_pts, scale, color, id); - } - - void PlanningVisualization::displayOptimalList(Eigen::MatrixXd optimal_pts, int id) - { - - if (optimal_list_pub.getNumSubscribers() == 0) - { - return; - } - - vector list; - for (int i = 0; i < optimal_pts.cols(); i++) - { - Eigen::Vector3d pt = optimal_pts.col(i).transpose(); - list.push_back(pt); - } - Eigen::Vector4d color(1, 0, 0, 1); - displayMarkerList(optimal_list_pub, list, 0.15, color, id); - } - - void PlanningVisualization::displayAStarList(std::vector> a_star_paths, int id /* = Eigen::Vector4d(0.5,0.5,0,1)*/) - { - - if (a_star_list_pub.getNumSubscribers() == 0) - { - return; - } - - int i = 0; - vector list; - - Eigen::Vector4d color = Eigen::Vector4d(0.5 + ((double)rand() / RAND_MAX / 2), 0.5 + ((double)rand() / RAND_MAX / 2), 0, 1); // make the A star pathes different every time. - double scale = 0.05 + (double)rand() / RAND_MAX / 10; - - // for ( int i=0; i<10; i++ ) - // { - // //Eigen::Vector4d color(1,1,0,0); - // displayMarkerList(a_star_list_pub, list, scale, color, id+i); - // } - - for (auto block : a_star_paths) - { - list.clear(); - for (auto pt : block) - { - list.push_back(pt); - } - //Eigen::Vector4d color(0.5,0.5,0,1); - displayMarkerList(a_star_list_pub, list, scale, color, id + i); // real ids used: [ id ~ id+a_star_paths.size() ] - i++; - } - } - - void PlanningVisualization::displayArrowList(ros::Publisher &pub, const vector &list, double scale, Eigen::Vector4d color, int id) - { - visualization_msgs::MarkerArray array; - // clear - pub.publish(array); - - generateArrowDisplayArray(array, list, scale, color, id); - - pub.publish(array); - } - - // PlanningVisualization:: -} // namespace ego_planner \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/src/polynomial_traj.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/src/polynomial_traj.cpp deleted file mode 100644 index 4960544b..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/traj_utils/src/polynomial_traj.cpp +++ /dev/null @@ -1,224 +0,0 @@ -#include -#include - -PolynomialTraj PolynomialTraj::minSnapTraj(const Eigen::MatrixXd &Pos, const Eigen::Vector3d &start_vel, - const Eigen::Vector3d &end_vel, const Eigen::Vector3d &start_acc, - const Eigen::Vector3d &end_acc, const Eigen::VectorXd &Time) -{ - int seg_num = Time.size(); - Eigen::MatrixXd poly_coeff(seg_num, 3 * 6); - Eigen::VectorXd Px(6 * seg_num), Py(6 * seg_num), Pz(6 * seg_num); - - int num_f, num_p; // number of fixed and free variables - int num_d; // number of all segments' derivatives - - const static auto Factorial = [](int x) { - int fac = 1; - for (int i = x; i > 0; i--) - fac = fac * i; - return fac; - }; - - /* ---------- end point derivative ---------- */ - Eigen::VectorXd Dx = Eigen::VectorXd::Zero(seg_num * 6); - Eigen::VectorXd Dy = Eigen::VectorXd::Zero(seg_num * 6); - Eigen::VectorXd Dz = Eigen::VectorXd::Zero(seg_num * 6); - - for (int k = 0; k < seg_num; k++) - { - /* position to derivative */ - Dx(k * 6) = Pos(0, k); - Dx(k * 6 + 1) = Pos(0, k + 1); - Dy(k * 6) = Pos(1, k); - Dy(k * 6 + 1) = Pos(1, k + 1); - Dz(k * 6) = Pos(2, k); - Dz(k * 6 + 1) = Pos(2, k + 1); - - if (k == 0) - { - Dx(k * 6 + 2) = start_vel(0); - Dy(k * 6 + 2) = start_vel(1); - Dz(k * 6 + 2) = start_vel(2); - - Dx(k * 6 + 4) = start_acc(0); - Dy(k * 6 + 4) = start_acc(1); - Dz(k * 6 + 4) = start_acc(2); - } - else if (k == seg_num - 1) - { - Dx(k * 6 + 3) = end_vel(0); - Dy(k * 6 + 3) = end_vel(1); - Dz(k * 6 + 3) = end_vel(2); - - Dx(k * 6 + 5) = end_acc(0); - Dy(k * 6 + 5) = end_acc(1); - Dz(k * 6 + 5) = end_acc(2); - } - } - - /* ---------- Mapping Matrix A ---------- */ - Eigen::MatrixXd Ab; - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(seg_num * 6, seg_num * 6); - - for (int k = 0; k < seg_num; k++) - { - Ab = Eigen::MatrixXd::Zero(6, 6); - for (int i = 0; i < 3; i++) - { - Ab(2 * i, i) = Factorial(i); - for (int j = i; j < 6; j++) - Ab(2 * i + 1, j) = Factorial(j) / Factorial(j - i) * pow(Time(k), j - i); - } - A.block(k * 6, k * 6, 6, 6) = Ab; - } - - /* ---------- Produce Selection Matrix C' ---------- */ - Eigen::MatrixXd Ct, C; - - num_f = 2 * seg_num + 4; // 3 + 3 + (seg_num - 1) * 2 = 2m + 4 - num_p = 2 * seg_num - 2; //(seg_num - 1) * 2 = 2m - 2 - num_d = 6 * seg_num; - Ct = Eigen::MatrixXd::Zero(num_d, num_f + num_p); - Ct(0, 0) = 1; - Ct(2, 1) = 1; - Ct(4, 2) = 1; // stack the start point - Ct(1, 3) = 1; - Ct(3, 2 * seg_num + 4) = 1; - Ct(5, 2 * seg_num + 5) = 1; - - Ct(6 * (seg_num - 1) + 0, 2 * seg_num + 0) = 1; - Ct(6 * (seg_num - 1) + 1, 2 * seg_num + 1) = 1; // Stack the end point - Ct(6 * (seg_num - 1) + 2, 4 * seg_num + 0) = 1; - Ct(6 * (seg_num - 1) + 3, 2 * seg_num + 2) = 1; // Stack the end point - Ct(6 * (seg_num - 1) + 4, 4 * seg_num + 1) = 1; - Ct(6 * (seg_num - 1) + 5, 2 * seg_num + 3) = 1; // Stack the end point - - for (int j = 2; j < seg_num; j++) - { - Ct(6 * (j - 1) + 0, 2 + 2 * (j - 1) + 0) = 1; - Ct(6 * (j - 1) + 1, 2 + 2 * (j - 1) + 1) = 1; - Ct(6 * (j - 1) + 2, 2 * seg_num + 4 + 2 * (j - 2) + 0) = 1; - Ct(6 * (j - 1) + 3, 2 * seg_num + 4 + 2 * (j - 1) + 0) = 1; - Ct(6 * (j - 1) + 4, 2 * seg_num + 4 + 2 * (j - 2) + 1) = 1; - Ct(6 * (j - 1) + 5, 2 * seg_num + 4 + 2 * (j - 1) + 1) = 1; - } - - C = Ct.transpose(); - - Eigen::VectorXd Dx1 = C * Dx; - Eigen::VectorXd Dy1 = C * Dy; - Eigen::VectorXd Dz1 = C * Dz; - - /* ---------- minimum snap matrix ---------- */ - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(seg_num * 6, seg_num * 6); - - for (int k = 0; k < seg_num; k++) - { - for (int i = 3; i < 6; i++) - { - for (int j = 3; j < 6; j++) - { - Q(k * 6 + i, k * 6 + j) = - i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) / (i + j - 5) * pow(Time(k), (i + j - 5)); - } - } - } - - /* ---------- R matrix ---------- */ - Eigen::MatrixXd R = C * A.transpose().inverse() * Q * A.inverse() * Ct; - - Eigen::VectorXd Dxf(2 * seg_num + 4), Dyf(2 * seg_num + 4), Dzf(2 * seg_num + 4); - - Dxf = Dx1.segment(0, 2 * seg_num + 4); - Dyf = Dy1.segment(0, 2 * seg_num + 4); - Dzf = Dz1.segment(0, 2 * seg_num + 4); - - Eigen::MatrixXd Rff(2 * seg_num + 4, 2 * seg_num + 4); - Eigen::MatrixXd Rfp(2 * seg_num + 4, 2 * seg_num - 2); - Eigen::MatrixXd Rpf(2 * seg_num - 2, 2 * seg_num + 4); - Eigen::MatrixXd Rpp(2 * seg_num - 2, 2 * seg_num - 2); - - Rff = R.block(0, 0, 2 * seg_num + 4, 2 * seg_num + 4); - Rfp = R.block(0, 2 * seg_num + 4, 2 * seg_num + 4, 2 * seg_num - 2); - Rpf = R.block(2 * seg_num + 4, 0, 2 * seg_num - 2, 2 * seg_num + 4); - Rpp = R.block(2 * seg_num + 4, 2 * seg_num + 4, 2 * seg_num - 2, 2 * seg_num - 2); - - /* ---------- close form solution ---------- */ - - Eigen::VectorXd Dxp(2 * seg_num - 2), Dyp(2 * seg_num - 2), Dzp(2 * seg_num - 2); - Dxp = -(Rpp.inverse() * Rfp.transpose()) * Dxf; - Dyp = -(Rpp.inverse() * Rfp.transpose()) * Dyf; - Dzp = -(Rpp.inverse() * Rfp.transpose()) * Dzf; - - Dx1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dxp; - Dy1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dyp; - Dz1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dzp; - - Px = (A.inverse() * Ct) * Dx1; - Py = (A.inverse() * Ct) * Dy1; - Pz = (A.inverse() * Ct) * Dz1; - - for (int i = 0; i < seg_num; i++) - { - poly_coeff.block(i, 0, 1, 6) = Px.segment(i * 6, 6).transpose(); - poly_coeff.block(i, 6, 1, 6) = Py.segment(i * 6, 6).transpose(); - poly_coeff.block(i, 12, 1, 6) = Pz.segment(i * 6, 6).transpose(); - } - - /* ---------- use polynomials ---------- */ - PolynomialTraj poly_traj; - for (int i = 0; i < poly_coeff.rows(); ++i) - { - vector cx(6), cy(6), cz(6); - for (int j = 0; j < 6; ++j) - { - cx[j] = poly_coeff(i, j), cy[j] = poly_coeff(i, j + 6), cz[j] = poly_coeff(i, j + 12); - } - reverse(cx.begin(), cx.end()); - reverse(cy.begin(), cy.end()); - reverse(cz.begin(), cz.end()); - double ts = Time(i); - poly_traj.addSegment(cx, cy, cz, ts); - } - - return poly_traj; -} - -PolynomialTraj PolynomialTraj::one_segment_traj_gen(const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc, - const Eigen::Vector3d &end_pt, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc, - double t) -{ - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(6, 6), Crow(1, 6); - Eigen::VectorXd Bx(6), By(6), Bz(6); - - C(0, 5) = 1; - C(1, 4) = 1; - C(2, 3) = 2; - Crow << pow(t, 5), pow(t, 4), pow(t, 3), pow(t, 2), t, 1; - C.row(3) = Crow; - Crow << 5 * pow(t, 4), 4 * pow(t, 3), 3 * pow(t, 2), 2 * t, 1, 0; - C.row(4) = Crow; - Crow << 20 * pow(t, 3), 12 * pow(t, 2), 6 * t, 2, 0, 0; - C.row(5) = Crow; - - Bx << start_pt(0), start_vel(0), start_acc(0), end_pt(0), end_vel(0), end_acc(0); - By << start_pt(1), start_vel(1), start_acc(1), end_pt(1), end_vel(1), end_acc(1); - Bz << start_pt(2), start_vel(2), start_acc(2), end_pt(2), end_vel(2), end_acc(2); - - Eigen::VectorXd Cofx = C.colPivHouseholderQr().solve(Bx); - Eigen::VectorXd Cofy = C.colPivHouseholderQr().solve(By); - Eigen::VectorXd Cofz = C.colPivHouseholderQr().solve(Bz); - - vector cx(6), cy(6), cz(6); - for (int i = 0; i < 6; i++) - { - cx[i] = Cofx(i); - cy[i] = Cofy(i); - cz[i] = Cofz(i); - } - - PolynomialTraj poly_traj; - poly_traj.addSegment(cx, cy, cz, t); - - return poly_traj; -} diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/CMakeLists.txt b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/CMakeLists.txt deleted file mode 100644 index a510ed1c..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(cmake_utils) - -find_package(catkin REQUIRED COMPONENTS roscpp) - -set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake) - -file(GLOB CMAKE_UILTS_FILES cmake/*.cmake) - -catkin_package( - CFG_EXTRAS ${CMAKE_UILTS_FILES} -) - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/arch.cmake b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/arch.cmake deleted file mode 100644 index 9f1dcc8f..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/arch.cmake +++ /dev/null @@ -1,126 +0,0 @@ -set(archdetect_c_code " -#if defined(__arm__) || defined(__TARGET_ARCH_ARM) - #if defined(__ARM_ARCH_7__) \\ - || defined(__ARM_ARCH_7A__) \\ - || defined(__ARM_ARCH_7R__) \\ - || defined(__ARM_ARCH_7M__) \\ - || (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 7) - #error cmake_ARCH armv7 - #elif defined(__ARM_ARCH_6__) \\ - || defined(__ARM_ARCH_6J__) \\ - || defined(__ARM_ARCH_6T2__) \\ - || defined(__ARM_ARCH_6Z__) \\ - || defined(__ARM_ARCH_6K__) \\ - || defined(__ARM_ARCH_6ZK__) \\ - || defined(__ARM_ARCH_6M__) \\ - || (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 6) - #error cmake_ARCH armv6 - #elif defined(__ARM_ARCH_5TEJ__) \\ - || (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 5) - #error cmake_ARCH armv5 - #else - #error cmake_ARCH arm - #endif -#elif defined(__i386) || defined(__i386__) || defined(_M_IX86) - #error cmake_ARCH i386 -#elif defined(__x86_64) || defined(__x86_64__) || defined(__amd64) || defined(_M_X64) - #error cmake_ARCH x86_64 -#elif defined(__ia64) || defined(__ia64__) || defined(_M_IA64) - #error cmake_ARCH ia64 -#elif defined(__ppc__) || defined(__ppc) || defined(__powerpc__) \\ - || defined(_ARCH_COM) || defined(_ARCH_PWR) || defined(_ARCH_PPC) \\ - || defined(_M_MPPC) || defined(_M_PPC) - #if defined(__ppc64__) || defined(__powerpc64__) || defined(__64BIT__) - #error cmake_ARCH ppc64 - #else - #error cmake_ARCH ppc - #endif -#endif -#error cmake_ARCH unknown -") - -# Set ppc_support to TRUE before including this file or ppc and ppc64 -# will be treated as invalid architectures since they are no longer supported by Apple - -function(target_architecture output_var) - if(APPLE AND CMAKE_OSX_ARCHITECTURES) - # On OS X we use CMAKE_OSX_ARCHITECTURES *if* it was set - # First let's normalize the order of the values - - # Note that it's not possible to compile PowerPC applications if you are using - # the OS X SDK version 10.6 or later - you'll need 10.4/10.5 for that, so we - # disable it by default - # See this page for more information: - # http://stackoverflow.com/questions/5333490/how-can-we-restore-ppc-ppc64-as-well-as-full-10-4-10-5-sdk-support-to-xcode-4 - - # Architecture defaults to i386 or ppc on OS X 10.5 and earlier, depending on the CPU type detected at runtime. - # On OS X 10.6+ the default is x86_64 if the CPU supports it, i386 otherwise. - - foreach(osx_arch ${CMAKE_OSX_ARCHITECTURES}) - if("${osx_arch}" STREQUAL "ppc" AND ppc_support) - set(osx_arch_ppc TRUE) - elseif("${osx_arch}" STREQUAL "i386") - set(osx_arch_i386 TRUE) - elseif("${osx_arch}" STREQUAL "x86_64") - set(osx_arch_x86_64 TRUE) - elseif("${osx_arch}" STREQUAL "ppc64" AND ppc_support) - set(osx_arch_ppc64 TRUE) - else() - message(FATAL_ERROR "Invalid OS X arch name: ${osx_arch}") - endif() - endforeach() - - # Now add all the architectures in our normalized order - if(osx_arch_ppc) - list(APPEND ARCH ppc) - endif() - - if(osx_arch_i386) - list(APPEND ARCH i386) - endif() - - if(osx_arch_x86_64) - list(APPEND ARCH x86_64) - endif() - - if(osx_arch_ppc64) - list(APPEND ARCH ppc64) - endif() - else() - file(WRITE "${CMAKE_BINARY_DIR}/arch.c" "${archdetect_c_code}") - - enable_language(C) - - # Detect the architecture in a rather creative way... - # This compiles a small C program which is a series of ifdefs that selects a - # particular #error preprocessor directive whose message string contains the - # target architecture. The program will always fail to compile (both because - # file is not a valid C program, and obviously because of the presence of the - # #error preprocessor directives... but by exploiting the preprocessor in this - # way, we can detect the correct target architecture even when cross-compiling, - # since the program itself never needs to be run (only the compiler/preprocessor) - try_run( - run_result_unused - compile_result_unused - "${CMAKE_BINARY_DIR}" - "${CMAKE_BINARY_DIR}/arch.c" - COMPILE_OUTPUT_VARIABLE ARCH - CMAKE_FLAGS CMAKE_OSX_ARCHITECTURES=${CMAKE_OSX_ARCHITECTURES} - ) - - # Parse the architecture name from the compiler output - string(REGEX MATCH "cmake_ARCH ([a-zA-Z0-9_]+)" ARCH "${ARCH}") - - # Get rid of the value marker leaving just the architecture name - string(REPLACE "cmake_ARCH " "" ARCH "${ARCH}") - - # If we are compiling with an unknown architecture this variable should - # already be set to "unknown" but in the case that it's empty (i.e. due - # to a typo in the code), then set it to unknown - if (NOT ARCH) - set(ARCH unknown) - endif() - endif() - - set(${output_var} "${ARCH}" PARENT_SCOPE) -endfunction() diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/cmake_modules.cmake b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/cmake_modules.cmake deleted file mode 100644 index 4370b380..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/cmake_modules.cmake +++ /dev/null @@ -1,2 +0,0 @@ -list(APPEND CMAKE_MODULE_PATH "${cmake_utils_SOURCE_PREFIX}/cmake_modules") -link_directories( ${cmake_utils_SOURCE_PREFIX}/lib/mosek8 ) diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/color.cmake b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/color.cmake deleted file mode 100644 index bada383c..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/color.cmake +++ /dev/null @@ -1,17 +0,0 @@ -string(ASCII 27 Esc) -set(ColourReset "${Esc}[m") -set(ColourBold "${Esc}[1m") -set(Red "${Esc}[31m") -set(Green "${Esc}[32m") -set(Yellow "${Esc}[33m") -set(Blue "${Esc}[34m") -set(Magenta "${Esc}[35m") -set(Cyan "${Esc}[36m") -set(White "${Esc}[37m") -set(BoldRed "${Esc}[1;31m") -set(BoldGreen "${Esc}[1;32m") -set(BoldYellow "${Esc}[1;33m") -set(BoldBlue "${Esc}[1;34m") -set(BoldMagenta "${Esc}[1;35m") -set(BoldCyan "${Esc}[1;36m") -set(BoldWhite "${Esc}[1;37m") diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindEigen.cmake b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindEigen.cmake deleted file mode 100644 index d45fad7c..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindEigen.cmake +++ /dev/null @@ -1,173 +0,0 @@ -# Ceres Solver - A fast non-linear least squares minimizer -# Copyright 2015 Google Inc. All rights reserved. -# http://ceres-solver.org/ -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# * Neither the name of Google Inc. nor the names of its contributors may be -# used to endorse or promote products derived from this software without -# specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: alexs.mac@gmail.com (Alex Stewart) -# - -# FindEigen.cmake - Find Eigen library, version >= 3. -# -# This module defines the following variables: -# -# EIGEN_FOUND: TRUE iff Eigen is found. -# EIGEN_INCLUDE_DIRS: Include directories for Eigen. -# -# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h -# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 -# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 -# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 -# -# The following variables control the behaviour of this module: -# -# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to -# search for eigen includes, e.g: /timbuktu/eigen3. -# -# The following variables are also defined by this module, but in line with -# CMake recommended FindPackage() module style should NOT be referenced directly -# by callers (use the plural variables detailed above instead). These variables -# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which -# are NOT re-called (i.e. search for library is not repeated) if these variables -# are set with valid values _in the CMake cache_. This means that if these -# variables are set directly in the cache, either by the user in the CMake GUI, -# or by the user passing -DVAR=VALUE directives to CMake when called (which -# explicitly defines a cache variable), then they will be used verbatim, -# bypassing the HINTS variables and other hard-coded search locations. -# -# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the -# include directory of any dependencies. - -# Called if we failed to find Eigen or any of it's required dependencies, -# unsets all public (designed to be used externally) variables and reports -# error message at priority depending upon [REQUIRED/QUIET/] argument. -macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) - unset(EIGEN_FOUND) - unset(EIGEN_INCLUDE_DIRS) - # Make results of search visible in the CMake GUI if Eigen has not - # been found so that user does not have to toggle to advanced view. - mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) - # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() - # use the camelcase library name, not uppercase. - if (Eigen_FIND_QUIETLY) - message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) - elseif (Eigen_FIND_REQUIRED) - message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) - else() - # Neither QUIETLY nor REQUIRED, use no priority which emits a message - # but continues configuration and allows generation. - message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) - endif () - return() -endmacro(EIGEN_REPORT_NOT_FOUND) - -# Protect against any alternative find_package scripts for this library having -# been called previously (in a client project) which set EIGEN_FOUND, but not -# the other variables we require / set here which could cause the search logic -# here to fail. -unset(EIGEN_FOUND) - -# Search user-installed locations first, so that we prefer user installs -# to system installs where both exist. -# -# TODO: Add standard Windows search locations for Eigen. -list(APPEND EIGEN_CHECK_INCLUDE_DIRS - /usr/local/include - /usr/local/homebrew/include # Mac OS X - /opt/local/var/macports/software # Mac OS X. - /opt/local/include - /usr/include) -# Additional suffixes to try appending to each search path. -list(APPEND EIGEN_CHECK_PATH_SUFFIXES - eigen3 # Default root directory for Eigen. - Eigen/include/eigen3 ) # Windows (for C:/Program Files prefix). - -# Search supplied hint directories first if supplied. -find_path(EIGEN_INCLUDE_DIR - NAMES Eigen/Core - PATHS ${EIGEN_INCLUDE_DIR_HINTS} - ${EIGEN_CHECK_INCLUDE_DIRS} - PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) - -if (NOT EIGEN_INCLUDE_DIR OR - NOT EXISTS ${EIGEN_INCLUDE_DIR}) - eigen_report_not_found( - "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " - "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") -endif (NOT EIGEN_INCLUDE_DIR OR - NOT EXISTS ${EIGEN_INCLUDE_DIR}) - -# Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets -# if called. -set(EIGEN_FOUND TRUE) - -# Extract Eigen version from Eigen/src/Core/util/Macros.h -if (EIGEN_INCLUDE_DIR) - set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) - if (NOT EXISTS ${EIGEN_VERSION_FILE}) - eigen_report_not_found( - "Could not find file: ${EIGEN_VERSION_FILE} " - "containing version information in Eigen install located at: " - "${EIGEN_INCLUDE_DIR}.") - else (NOT EXISTS ${EIGEN_VERSION_FILE}) - file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) - - string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" - EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") - string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" - EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") - - string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" - EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") - string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" - EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") - - string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" - EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") - string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" - EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") - - # This is on a single line s/t CMake does not interpret it as a list of - # elements and insert ';' separators which would result in 3.;2.;0 nonsense. - set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") - endif (NOT EXISTS ${EIGEN_VERSION_FILE}) -endif (EIGEN_INCLUDE_DIR) - -# Set standard CMake FindPackage variables if found. -if (EIGEN_FOUND) - set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) -endif (EIGEN_FOUND) - -# Handle REQUIRED / QUIET optional arguments and version. -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(Eigen - REQUIRED_VARS EIGEN_INCLUDE_DIRS - VERSION_VAR EIGEN_VERSION) - -# Only mark internal variables as advanced if we found Eigen, otherwise -# leave it visible in the standard GUI for the user to set manually. -if (EIGEN_FOUND) - mark_as_advanced(FORCE EIGEN_INCLUDE_DIR) -endif (EIGEN_FOUND) diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindGSL.cmake b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindGSL.cmake deleted file mode 100644 index 797b2455..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindGSL.cmake +++ /dev/null @@ -1,135 +0,0 @@ -# Try to find gnu scientific library GSL -# See -# http://www.gnu.org/software/gsl/ and -# http://gnuwin32.sourceforge.net/packages/gsl.htm -# -# Based on a script of Felix Woelk and Jan Woetzel -# (www.mip.informatik.uni-kiel.de) -# -# It defines the following variables: -# GSL_FOUND - system has GSL lib -# GSL_INCLUDE_DIRS - where to find headers -# GSL_LIBRARIES - full path to the libraries -# GSL_LIBRARY_DIRS, the directory where the PLplot library is found. - -# CMAKE_GSL_CXX_FLAGS = Unix compiler flags for GSL, essentially "`gsl-config --cxxflags`" -# GSL_LINK_DIRECTORIES = link directories, useful for rpath on Unix -# GSL_EXE_LINKER_FLAGS = rpath on Unix - -set( GSL_FOUND OFF ) -set( GSL_CBLAS_FOUND OFF ) - -# Windows, but not for Cygwin and MSys where gsl-config is available -if( WIN32 AND NOT CYGWIN AND NOT MSYS ) - # look for headers - find_path( GSL_INCLUDE_DIR - NAMES gsl/gsl_cdf.h gsl/gsl_randist.h - ) - if( GSL_INCLUDE_DIR ) - # look for gsl library - find_library( GSL_LIBRARY - NAMES gsl - ) - if( GSL_LIBRARY ) - set( GSL_INCLUDE_DIRS ${GSL_INCLUDE_DIR} ) - get_filename_component( GSL_LIBRARY_DIRS ${GSL_LIBRARY} PATH ) - set( GSL_FOUND ON ) - endif( GSL_LIBRARY ) - - # look for gsl cblas library - find_library( GSL_CBLAS_LIBRARY - NAMES gslcblas - ) - if( GSL_CBLAS_LIBRARY ) - set( GSL_CBLAS_FOUND ON ) - endif( GSL_CBLAS_LIBRARY ) - - set( GSL_LIBRARIES ${GSL_LIBRARY} ${GSL_CBLAS_LIBRARY} ) - endif( GSL_INCLUDE_DIR ) - - mark_as_advanced( - GSL_INCLUDE_DIR - GSL_LIBRARY - GSL_CBLAS_LIBRARY - ) -else( WIN32 AND NOT CYGWIN AND NOT MSYS ) - if( UNIX OR MSYS ) - find_program( GSL_CONFIG_EXECUTABLE gsl-config - /usr/bin/ - /usr/local/bin - ) - - if( GSL_CONFIG_EXECUTABLE ) - set( GSL_FOUND ON ) - - # run the gsl-config program to get cxxflags - execute_process( - COMMAND sh "${GSL_CONFIG_EXECUTABLE}" --cflags - OUTPUT_VARIABLE GSL_CFLAGS - RESULT_VARIABLE RET - ERROR_QUIET - ) - if( RET EQUAL 0 ) - string( STRIP "${GSL_CFLAGS}" GSL_CFLAGS ) - separate_arguments( GSL_CFLAGS ) - - # parse definitions from cflags; drop -D* from CFLAGS - string( REGEX MATCHALL "-D[^;]+" - GSL_DEFINITIONS "${GSL_CFLAGS}" ) - string( REGEX REPLACE "-D[^;]+;" "" - GSL_CFLAGS "${GSL_CFLAGS}" ) - - # parse include dirs from cflags; drop -I prefix - string( REGEX MATCHALL "-I[^;]+" - GSL_INCLUDE_DIRS "${GSL_CFLAGS}" ) - string( REPLACE "-I" "" - GSL_INCLUDE_DIRS "${GSL_INCLUDE_DIRS}") - string( REGEX REPLACE "-I[^;]+;" "" - GSL_CFLAGS "${GSL_CFLAGS}") - - message("GSL_DEFINITIONS=${GSL_DEFINITIONS}") - message("GSL_INCLUDE_DIRS=${GSL_INCLUDE_DIRS}") - message("GSL_CFLAGS=${GSL_CFLAGS}") - else( RET EQUAL 0 ) - set( GSL_FOUND FALSE ) - endif( RET EQUAL 0 ) - - # run the gsl-config program to get the libs - execute_process( - COMMAND sh "${GSL_CONFIG_EXECUTABLE}" --libs - OUTPUT_VARIABLE GSL_LIBRARIES - RESULT_VARIABLE RET - ERROR_QUIET - ) - if( RET EQUAL 0 ) - string(STRIP "${GSL_LIBRARIES}" GSL_LIBRARIES ) - separate_arguments( GSL_LIBRARIES ) - - # extract linkdirs (-L) for rpath (i.e., LINK_DIRECTORIES) - string( REGEX MATCHALL "-L[^;]+" - GSL_LIBRARY_DIRS "${GSL_LIBRARIES}" ) - string( REPLACE "-L" "" - GSL_LIBRARY_DIRS "${GSL_LIBRARY_DIRS}" ) - else( RET EQUAL 0 ) - set( GSL_FOUND FALSE ) - endif( RET EQUAL 0 ) - - MARK_AS_ADVANCED( - GSL_CFLAGS - ) - message( STATUS "Using GSL from ${GSL_PREFIX}" ) - else( GSL_CONFIG_EXECUTABLE ) - message( STATUS "FindGSL: gsl-config not found.") - endif( GSL_CONFIG_EXECUTABLE ) - endif( UNIX OR MSYS ) -endif( WIN32 AND NOT CYGWIN AND NOT MSYS ) - -if( GSL_FOUND ) - if( NOT GSL_FIND_QUIETLY ) - message( STATUS "FindGSL: Found both GSL headers and library" ) - endif( NOT GSL_FIND_QUIETLY ) -else( GSL_FOUND ) - if( GSL_FIND_REQUIRED ) - message( FATAL_ERROR "FindGSL: Could not find GSL headers or library" ) - endif( GSL_FIND_REQUIRED ) -endif( GSL_FOUND ) diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindmvIMPACT.cmake b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindmvIMPACT.cmake deleted file mode 100644 index d1a61b64..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindmvIMPACT.cmake +++ /dev/null @@ -1,124 +0,0 @@ -macro(mvIMPACT_REPORT_NOT_FOUND REASON_MSG) - unset(mvIMPACT_FOUND) - unset(mvIMPACT_INCLUDE_DIRS) - unset(mvIMPACT_LIBRARIES) - unset(mvIMPACT_WORLD_VERSION) - unset(mvIMPACT_MAJOR_VERSION) - unset(mvIMPACT_MINOR_VERSION) - # Make results of search visible in the CMake GUI if mvimpact has not - # been found so that user does not have to toggle to advanced view. - mark_as_advanced(CLEAR mvIMPACT_INCLUDE_DIR) - # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() - # use the camelcase library name, not uppercase. - if(Mvimpact_FIND_QUIETLY) - message(STATUS "Failed to find mvimpact - " ${REASON_MSG} ${ARGN}) - elseif(Mvimpact_FIND_REQUIRED) - message(FATAL_ERROR "Failed to find mvimpact - " ${REASON_MSG} ${ARGN}) - else() - # Neither QUIETLY nor REQUIRED, use no priority which emits a message - # but continues configuration and allows generation. - message("-- Failed to find mvimpact - " ${REASON_MSG} ${ARGN}) - endif() -endmacro(mvIMPACT_REPORT_NOT_FOUND) - -# Search user-installed locations first, so that we prefer user installs -# to system installs where both exist. -get_filename_component(BLUEFOX2_DIR ${CMAKE_CURRENT_SOURCE_DIR} REALPATH) -list(APPEND mvIMPACT_CHECK_INCLUDE_DIRS - /opt/mvIMPACT_acquire - /opt/mvIMPACT_Acquire - ) -execute_process(COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCH) - -if(NOT ARCH) -set(ARCH "arm64") -elseif(${ARCH} STREQUAL "aarch64") -set(ARCH "arm64") -endif() - -list(APPEND mvIMPACT_CHECK_LIBRARY_DIRS - /opt/mvIMPACT_acquire/lib/${ARCH} - /opt/mvIMPACT_Acquire/lib/${ARCH} - ) - -# Check general hints -if(mvIMPACT_HINTS AND EXISTS ${mvIMPACT_HINTS}) - set(mvIMPACT_INCLUDE_DIR_HINTS ${mvIMPACT_HINTS}/include) - set(mvIMPACT_LIBRARY_DIR_HINTS ${mvIMPACT_HINTS}/lib) -endif() - -# Search supplied hint directories first if supplied. -# Find include directory for mvimpact -find_path(mvIMPACT_INCLUDE_DIR - NAMES mvIMPACT_CPP/mvIMPACT_acquire.h - PATHS ${mvIMPACT_INCLUDE_DIR_HINTS} - ${mvIMPACT_CHECK_INCLUDE_DIRS} - NO_DEFAULT_PATH) -if(NOT mvIMPACT_INCLUDE_DIR OR NOT EXISTS ${mvIMPACT_INCLUDE_DIR}) - mvIMPACT_REPORT_NOT_FOUND( - "Could not find mvimpact include directory, set mvIMPACT_INCLUDE_DIR to " - "path to mvimpact include directory," - "e.g. /opt/mvIMPACT_acquire.") -else() - message(STATUS "mvimpact include dir found: " ${mvIMPACT_INCLUDE_DIR}) -endif() - -# Find library directory for mvimpact -find_library(mvIMPACT_LIBRARY - NAMES libmvBlueFOX.so - PATHS ${mvIMPACT_LIBRARY_DIR_HINTS} - ${mvIMPACT_CHECK_LIBRARY_DIRS} - NO_DEFAULT_PATH) -if(NOT mvIMPACT_LIBRARY OR NOT EXISTS ${mvIMPACT_LIBRARY}) - mvIMPACT_REPORT_NOT_FOUND( - "Could not find mvimpact library, set mvIMPACT_LIBRARY " - "to full path to mvimpact library direcotory.${mvIMPACT_CHECK_LIBRARY_DIRS}~~") -else() - # TODO: need to fix this hacky solution for getting mvIMPACT_LIBRARY_DIR - string(REGEX MATCH ".*/" mvIMPACT_LIBRARY_DIR ${mvIMPACT_LIBRARY}) - message(STATUS "mvimpact library dir found: " ${mvIMPACT_LIBRARY_DIR}) - -endif() - -# Mark internally as found, then verify. mvIMPACT_REPORT_NOT_FOUND() unsets if -# called. -set(mvIMPACT_FOUND TRUE) - -# Extract mvimpact version -if(mvIMPACT_LIBRARY_DIR) - file(GLOB mvIMPACT_LIBS - RELATIVE ${mvIMPACT_LIBRARY_DIR} - ${mvIMPACT_LIBRARY_DIR}/libmvBlueFOX.so.[0-9].[0-9].[0-9]) - # TODO: add version support - # string(REGEX MATCH "" - # mvIMPACT_WORLD_VERSION ${mvIMPACT_PVBASE}) - # message(STATUS "mvimpact world version: " ${mvIMPACT_WORLD_VERSION}) -endif() - -# Catch case when caller has set mvIMPACT_INCLUDE_DIR in the cache / GUI and -# thus FIND_[PATH/LIBRARY] are not called, but specified locations are -# invalid, otherwise we would report the library as found. -if(NOT mvIMPACT_INCLUDE_DIR AND NOT EXISTS ${mvIMPACT_INCLUDE_DIR}/mvIMPACT_CPP/mvIMPACT_acquire.h) - mvIMPACT_REPORT_NOT_FOUND("Caller defined mvIMPACT_INCLUDE_DIR: " - ${mvIMPACT_INCLUDE_DIR} - " does not contain mvIMPACT_CPP/mvIMPACT_acquire.h header.") -endif() - -# Set standard CMake FindPackage variables if found. -if(mvIMPACT_FOUND) - set(mvIMPACT_INCLUDE_DIRS ${mvIMPACT_INCLUDE_DIR}) - file(GLOB mvIMPACT_LIBRARIES ${mvIMPACT_LIBRARY_DIR}lib*.so) -endif() - -# Handle REQUIRED / QUIET optional arguments. -include(FindPackageHandleStandardArgs) -if(mvIMPACT_FOUND) - FIND_PACKAGE_HANDLE_STANDARD_ARGS(Mvimpact DEFAULT_MSG - mvIMPACT_INCLUDE_DIRS mvIMPACT_LIBRARIES) -endif() - -# Only mark internal variables as advanced if we found mvimpact, otherwise -# leave it visible in the standard GUI for the user to set manually. -if(mvIMPACT_FOUND) - mark_as_advanced(FORCE mvIMPACT_INCLUDE_DIR mvIMPACT_LIBRARY) -endif() diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/package.xml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/package.xml deleted file mode 100644 index d745d2d3..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - cmake_utils - 0.0.0 - - Once you used this package, - then you do not need to copy cmake files among packages - - - William.Wu - - LGPLv3 - - catkin - roscpp - cmake_modules - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/CMakeLists.txt b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/CMakeLists.txt deleted file mode 100644 index a89dd726..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/CMakeLists.txt +++ /dev/null @@ -1,222 +0,0 @@ -#cmake_minimum_required(VERSION 2.4.6) -#include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -# Set the build type. Options are: -# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage -# Debug : w/ debug symbols, w/o optimization -# Release : w/o debug symbols, w/ optimization -# RelWithDebInfo : w/ debug symbols, w/ optimization -# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries -#set(ROS_BUILD_TYPE Release) - -#rosbuild_init() - -#set the default path for built executables to the "bin" directory -#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -#set the default path for built libraries to the "lib" directory -#set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) - -#uncomment if you have defined messages -#rosbuild_genmsg() -#uncomment if you have defined services -#rosbuild_gensrv() - -#common commands for building c++ executables and libraries -#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) -#target_link_libraries(${PROJECT_NAME} another_library) -#rosbuild_add_executable(example examples/example.cpp) -#target_link_libraries(example ${PROJECT_NAME}) -#rosbuild_add_boost_directories() - - -#rosbuild_add_executable(multi_map_server src/multi_map_server.cc) -#target_link_libraries(multi_map_server pose_utils) - -#rosbuild_add_executable(multi_map_visualization src/multi_map_visualization.cc) -#target_link_libraries(multi_map_visualization pose_utils) - -#---------------------------------- -cmake_minimum_required(VERSION 2.8.3) -project(multi_map_server) - -set(CMAKE_BUILD_TYPE "Release") -set(CMAKE_CXX_FLAGS "-std=c++11") -set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - nav_msgs - pose_utils - message_generation - roscpp - visualization_msgs - tf - std_srvs - laser_geometry - pose_utils - quadrotor_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependencies might have been -## pulled in transitively but can be declared for certainty nonetheless: -## * add a build_depend tag for "message_generation" -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -add_message_files( - FILES - MultiOccupancyGrid.msg - MultiSparseMap3D.msg - SparseMap3D.msg - VerticalOccupancyGridList.msg - #plan_cmd.msg -) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - geometry_msgs - nav_msgs -) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES irobot_msgs - CATKIN_DEPENDS geometry_msgs nav_msgs quadrotor_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### -find_package(Armadillo REQUIRED) -include_directories(${ARMADILLO_INCLUDE_DIRS}) - - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a cpp library -# add_library(irobot_msgs -# src/${PROJECT_NAME}/irobot_msgs.cpp -# ) - -## Declare a cpp executable -add_executable(multi_map_visualization src/multi_map_visualization.cc) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -add_dependencies(multi_map_visualization multi_map_server_messages_cpp) - -## Specify libraries to link a library or executable target against -target_link_libraries(multi_map_visualization - ${catkin_LIBRARIES} - ${ARMADILLO_LIBRARIES} - pose_utils -) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS irobot_msgs irobot_msgs_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_irobot_msgs.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/Makefile b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/Makefile deleted file mode 100644 index b75b928f..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map2D.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map2D.h deleted file mode 100644 index cee4d65a..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map2D.h +++ /dev/null @@ -1,242 +0,0 @@ -#ifndef MAP2D_H -#define MAP2D_H - -#include -#include -#include -#include - -using namespace std; - -class Map2D -{ - -private: - nav_msgs::OccupancyGrid map; - int expandStep; - int binning; - bool isBinningSet; - bool updated; - -public: - Map2D() - { - map.data.resize(0); - map.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0); - expandStep = 200; - binning = 1; - isBinningSet = false; - updated = false; - } - - Map2D(int _binning) - { - map.data.resize(0); - map.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0); - expandStep = 200; - binning = _binning; - isBinningSet = true; - updated = false; - } - - ~Map2D() {} - - double GetResolution() { return map.info.resolution; } - double GetMinX() { return map.info.origin.position.x; } - double GetMinY() { return map.info.origin.position.y; } - double GetMaxX() { return map.info.origin.position.x + map.info.width * map.info.resolution; } - double GetMaxY() { return map.info.origin.position.y + map.info.height * map.info.resolution; } - bool Updated() { return updated; } - void Reset() { map = nav_msgs::OccupancyGrid(); } - - void SetBinning(int _binning) - { - if (!isBinningSet) - binning = _binning; - } - - // Get occupancy value, 0: unknown; +ve: occupied; -ve: free - signed char GetOccupiedFromWorldFrame(double x, double y) - { - int xm = (x - map.info.origin.position.x) / map.info.resolution; - int ym = (y - map.info.origin.position.y) / map.info.resolution; - if (xm < 0 || xm > (int)(map.info.width-1) || ym < 0 || ym > (int)(map.info.height-1)) - return 0; - else - return map.data[ym*map.info.width+xm]; - } - - void Replace(nav_msgs::OccupancyGrid m) - { - // Check data - if (m.data.size() == 0) - return; - isBinningSet = true; - // Binning, conservative, take maximum - if (binning > 1) - { - int _width = m.info.width; - m.info.width /= binning; - m.info.height /= binning; - m.info.resolution *= binning; - vector data(m.info.width * m.info.height); - for (uint32_t i = 0; i < m.info.height; i++) - { - for (uint32_t j = 0; j < m.info.width; j++) - { - int val = -0xff; - for (int _i = 0; _i < binning; _i++) - { - for (int _j = 0; _j < binning; _j++) - { - int v = m.data[(i*binning + _i) * _width + (j*binning + _j)]; - val = (v > val)?v:val; - } - } - data[i * m.info.width + j] = val; - } - } - m.data = data; - } - // Replace map - map = m; - updated = true; - } - - // Merge submap - void Update(nav_msgs::OccupancyGrid m) - { - // Check data - if (m.data.size() == 0) - return; - isBinningSet = true; - // Binning, conservative, take maximum - if (binning > 1) - { - int _width = m.info.width; - m.info.width /= binning; - m.info.height /= binning; - m.info.resolution *= binning; - vector data(m.info.width * m.info.height); - for (uint32_t i = 0; i < m.info.height; i++) - { - for (uint32_t j = 0; j < m.info.width; j++) - { - int val = -0xff; - for (int _i = 0; _i < binning; _i++) - { - for (int _j = 0; _j < binning; _j++) - { - int v = m.data[(i*binning + _i) * _width + (j*binning + _j)]; - val = (v > val)?v:val; - } - } - data[i * m.info.width + j] = val; - } - } - m.data = data; - } - // Initialize and check resolution - if (map.info.resolution == 0) - map.info.resolution = m.info.resolution; - else if (m.info.resolution != map.info.resolution) - return; - // Get Info - double ox = m.info.origin.position.x; - double oy = m.info.origin.position.y; - double oyaw = tf::getYaw(m.info.origin.orientation); - double syaw = sin(oyaw); - double cyaw = cos(oyaw); - int mx = m.info.width; - int my = m.info.height; - double xs[4]; - double ys[4]; - xs[0] = cyaw * (0) - syaw * (0) + ox; - xs[1] = cyaw * (mx*map.info.resolution) - syaw * (0) + ox; - xs[2] = cyaw * (0) - syaw * (mx*map.info.resolution) + ox; - xs[3] = cyaw * (mx*map.info.resolution) - syaw * (my*map.info.resolution) + ox; - ys[0] = syaw * (0) + cyaw * (0) + oy; - ys[1] = syaw * (mx*map.info.resolution) + cyaw * (0) + oy; - ys[2] = syaw * (0) + cyaw * (my*map.info.resolution) + oy; - ys[3] = syaw * (mx*map.info.resolution) + cyaw * (my*map.info.resolution) + oy; - double minx = xs[0]; - double maxx = xs[0]; - double miny = ys[0]; - double maxy = ys[0]; - for (int k = 0; k < 4; k++) - { - minx = (xs[k] < minx)?xs[k]:minx; - miny = (ys[k] < miny)?ys[k]:miny; - maxx = (xs[k] > maxx)?xs[k]:maxx; - maxy = (ys[k] > maxy)?ys[k]:maxy; - } - // Check whether resize map is needed - bool mapResetFlag = false; - double prevOriginX = map.info.origin.position.x; - double prevOriginY = map.info.origin.position.y; - int prevMapX = map.info.width; - int prevMapY = map.info.height; - while (map.info.origin.position.x > minx) - { - map.info.width += expandStep; - map.info.origin.position.x -= expandStep*map.info.resolution; - mapResetFlag = true; - } - while (map.info.origin.position.y > miny) - { - map.info.height += expandStep; - map.info.origin.position.y -= expandStep*map.info.resolution; - mapResetFlag = true; - } - while (map.info.origin.position.x + map.info.width*map.info.resolution < maxx) - { - map.info.width += expandStep; - mapResetFlag = true; - } - while (map.info.origin.position.y + map.info.height*map.info.resolution < maxy) - { - map.info.height += expandStep; - mapResetFlag = true; - } - // Resize map - if (mapResetFlag) - { - mapResetFlag = false; - vector _data = map.data; - map.data.clear(); - map.data.resize(map.info.width*map.info.height,0); - for (int x = 0; x < prevMapX; x++) - { - for(int y = 0; y < prevMapY; y++) - { - int xn = x + round((prevOriginX - map.info.origin.position.x) / map.info.resolution); - int yn = y + round((prevOriginY - map.info.origin.position.y) / map.info.resolution); - map.data[yn*map.info.width+xn] = _data[y*prevMapX+x]; - } - } - } - // Merge map - for (int x = 0; x < mx; x++) - { - for(int y = 0; y < my; y++) - { - int xn = (cyaw*(x*map.info.resolution)-syaw*(y*map.info.resolution)+ox-map.info.origin.position.x) / map.info.resolution; - int yn = (syaw*(x*map.info.resolution)+cyaw*(y*map.info.resolution)+oy-map.info.origin.position.y) / map.info.resolution; - if (abs((int)(map.data[yn*map.info.width+xn]) + (int)(m.data[y*mx+x])) <= 127) - map.data[yn*map.info.width+xn] += m.data[y*mx+x]; - } - } - updated = true; - } - - const nav_msgs::OccupancyGrid& GetMap() - { - map.header.stamp = ros::Time::now(); - map.info.map_load_time = ros::Time::now(); - map.header.frame_id = string("/map"); - updated = false; - return map; - } -}; - -#endif diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map3D.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map3D.h deleted file mode 100644 index 7f8aa7c0..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map3D.h +++ /dev/null @@ -1,608 +0,0 @@ -#ifndef MAP3D_H -#define MAP3D_H - -#include -#include -#include -#include -#include - -using namespace std; - -// Occupancy probability of a sensor -#define PROB_OCCUPIED 0.95 - -// Free space probability of a sensor -#define PROB_FREE 0.01 - -// Threshold that determine a cell is occupied -#define PROB_OCCUPIED_THRESHOLD 0.75 - -// If beyond this threshold, the occupancy(occupied) of this cell is fixed, no decay -#define PROB_OCCUPIED_FIXED_THRESHOLD 0.95 - -// Threshold that determine a cell is free -#define PROB_FREE_THRESHOLD 0.25 - -// If bwlow this threshold, the occupancy(free) of this cell is fixed, no decay -#define PROB_FREE_FIXED_THRESHOLD 0.05 - -// Used integer value to store occupancy, create a large scale factor to enable small scale decay -#define LOG_ODD_SCALE_FACTOR 10000 - -// Decay factor per second -#define LOG_ODD_DECAY_RATE 1.00 - -// Binary value of the occupancy output -#define OCCUPIED 1 -#define FREE -1 - -// Cell Struct ----------------------------------------- -struct OccupancyGrid -{ - int upper; - int lower; - int mass; -}; - -// Occupancy Grids List -------------------------------- -class OccupancyGridList -{ -public: - - OccupancyGridList() { updateCounter = 0; } - - ~OccupancyGridList() { } - - void PackMsg(multi_map_server::VerticalOccupancyGridList &msg) - { - msg.x = x; - msg.y = y; - for (list::iterator k = grids.begin(); k != grids.end(); k++) - { - msg.upper.push_back(k->upper); - msg.lower.push_back(k->lower); - msg.mass.push_back(k->mass); - } - } - - void UnpackMsg(const multi_map_server::VerticalOccupancyGridList &msg) - { - x = msg.x; - y = msg.y; - updateCounter = 0; - grids.clear(); - for (unsigned int k = 0; k < msg.mass.size(); k++) - { - OccupancyGrid c; - c.upper = msg.upper[k]; - c.lower = msg.lower[k]; - c.mass = msg.mass[k]; - grids.push_back(c); - } - } - - void GetOccupancyGrids(vector& _grids) - { - _grids.clear(); - for (list::iterator k = grids.begin(); k != grids.end(); k++) - _grids.push_back((*k)); - } - - inline int GetUpdateCounter() { return updateCounter; } - - inline void SetUpdateCounterXY(int _updateCounter, double _x, double _y) - { - updateCounter = _updateCounter; - x = _x; - y = _y; - } - - inline int GetOccupancyValue(int mz) - { - for (list::iterator k = grids.begin(); k != grids.end(); k++) - if (mz <= k->upper && mz >= k->lower) - return k->mass / (k->upper - k->lower + 1); - return 0; - } - - inline void DeleteOccupancyGrid(int mz) - { - for (list::iterator k = grids.begin(); k != grids.end(); k++) - { - if (mz <= k->upper && mz >= k->lower) - { - grids.erase(k); - return; - } - } - return; - } - - inline void SetOccupancyValue(int mz, int value) - { - OccupancyGrid grid; - grid.upper = mz; - grid.lower = mz; - grid.mass = value; - - list::iterator gend = grids.end(); - gend--; - - if (grids.size() == 0) // Empty case - { - grids.push_back(grid); - return; - } - else if (mz - grids.begin()->upper > 1) // Beyond highest - { - grids.push_front(grid); - return; - } - else if (mz - grids.begin()->upper == 1) // Next to highest - { - grids.begin()->upper += 1; - grids.begin()->mass += value; - return; - } - else if (gend->lower - mz > 1) // Below lowest - { - grids.push_back(grid); - return; - } - else if (gend->lower - mz == 1) // Next to lowest - { - grids.end()->lower -= 1; - grids.end()->mass += value; - return; - } - else // General case - { - for (list::iterator k = grids.begin(); k != grids.end(); k++) - { - if (mz <= k->upper && mz >= k->lower) // Within a grid - { - k->mass += value; - return; - } - else if (k != gend) - { - list::iterator j = k; - j++; - if (k->lower - mz == 1 && mz - j->upper > 1) // ###*--### - { - k->lower -= 1; - k->mass += value; - return; - } - else if (k->lower - mz > 1 && mz - j->upper == 1) // ###--*### - { - j->upper += 1; - j->mass += value; - return; - } - else if (k->lower - mz == 1 && mz - j->upper == 1) // ###*### - { - k->lower = j->lower; - k->mass += j->mass + value; - grids.erase(j); - return; - } - else if (k->lower - mz > 1 && mz - j->upper > 1) // ###-*-### - { - grids.insert(j, grid); - return; - } - } - } - } - } - - // Merging two columns, merge the grids in input "gridList" into current column - inline void Merge(const OccupancyGridList& gridsIn) - { - // Create a sorted list containing both upper and lower values - list > lp; - for (list::const_iterator k = grids.begin(); k != grids.end(); k++) - { - lp.push_back( make_pair(k->upper, k->mass)); - lp.push_back( make_pair(k->lower, -1)); - } - list > lp2; - for (list::const_iterator k = gridsIn.grids.begin(); k != gridsIn.grids.end(); k++) - { - lp2.push_back( make_pair(k->upper, k->mass)); - lp2.push_back( make_pair(k->lower, -1)); - } - lp.merge(lp2, ComparePair()); - // Manipulate this list to get a minimum size list - grids.clear(); - int currUpper = 0; - int currLower = 0; - int currMass = 0; - int upperCnt = 0; - int lowerCnt = 0; - list >::iterator lend = lp.end(); - lend--; - for (list >::iterator k = lp.begin(); k != lp.end(); k++) - { - if (k->second > 0) - { - if (upperCnt == 0) currUpper = k->first; - currMass = (k->second > currMass)?k->second:currMass; - upperCnt++; - } - if (k->second < 0) - { - currLower = k->first; - lowerCnt++; - } - if (lowerCnt == upperCnt && k != lend) - { - list >::iterator j = k; - if (k->first - (++j)->first == 1) continue; - } - if (lowerCnt == upperCnt) - { - OccupancyGrid c; - c.upper = currUpper; - c.lower = currLower; - c.mass = currMass; - grids.push_back(c); - upperCnt = lowerCnt = currUpper = currLower = currMass = 0; - } - } - } - - inline void Decay(int upThr, int lowThr, double factor) - { - for (list::iterator k = grids.begin(); k != grids.end(); k++) - { - int val = k->mass / (k->upper - k->lower + 1); - if (val < upThr && val > lowThr) - k->mass *= factor; - } - } - -private: - - struct ComparePair - { - bool operator()(pair p1, pair p2) - { - if (p1.first != p2.first) - return (p1.first > p2.first); - else - return (p1.second > p2.second); - } - }; - - // List of vertical occupancy values - list grids; - // Location of the list in world frame - double x; - double y; - // Update indicator - int updateCounter; - -}; - -// 3D Map Object --------------------------------- -class Map3D -{ -public: - - Map3D() - { - resolution = 0.1; - decayInterval = -1; - originX = -5; - originY = -5; - originZ = 0; - mapX = 200; - mapY = 200; - expandStep = 200; - updated = false; - updateCounter = 1; - updateList.clear(); - mapBase.clear(); - mapBase.resize(mapX*mapY, NULL); - logOddOccupied = log(PROB_OCCUPIED/(1.0-PROB_OCCUPIED)) * LOG_ODD_SCALE_FACTOR; - logOddFree = log(PROB_FREE/(1.0-PROB_FREE)) * LOG_ODD_SCALE_FACTOR; - logOddOccupiedThr = log(1.0/(1.0-PROB_OCCUPIED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR; - logOddOccupiedFixedThr = log(1.0/(1.0-PROB_OCCUPIED_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR; - logOddFreeThr = log(1.0/(1.0-PROB_FREE_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR; - logOddFreeFixedThr = log(1.0/(1.0-PROB_FREE_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR; - } - - Map3D(const Map3D& _map3d) - { - resolution = _map3d.resolution; - decayInterval = _map3d.decayInterval; - originX = _map3d.originX; - originY = _map3d.originY; - originZ = _map3d.originZ; - mapX = _map3d.mapX; - mapY = _map3d.mapY; - expandStep = _map3d.expandStep; - updated = _map3d.updated; - updateCounter = _map3d.updateCounter; - updateList = _map3d.updateList; - mapBase = _map3d.mapBase; - for (unsigned int k = 0; k < mapBase.size(); k++) - { - if (mapBase[k]) - { - mapBase[k] = new OccupancyGridList; - *mapBase[k] = *_map3d.mapBase[k]; - } - } - logOddOccupied = log(PROB_OCCUPIED/(1.0-PROB_OCCUPIED)) * LOG_ODD_SCALE_FACTOR; - logOddFree = log(PROB_FREE/(1.0-PROB_FREE)) * LOG_ODD_SCALE_FACTOR; - logOddOccupiedThr = log(1.0/(1.0-PROB_OCCUPIED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR; - logOddOccupiedFixedThr = log(1.0/(1.0-PROB_OCCUPIED_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR; - logOddFreeThr = log(1.0/(1.0-PROB_FREE_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR; - logOddFreeFixedThr = log(1.0/(1.0-PROB_FREE_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR; - } - - ~Map3D() - { - for (unsigned int k = 0; k < mapBase.size(); k++) - { - if (mapBase[k]) - { - delete mapBase[k]; - mapBase[k] = NULL; - } - } - } - - void PackMsg(multi_map_server::SparseMap3D &msg) - { - // Basic map info - msg.header.stamp = ros::Time::now(); - msg.header.frame_id = string("/map"); - msg.info.map_load_time = ros::Time::now(); - msg.info.resolution = resolution; - msg.info.origin.position.x = originX; - msg.info.origin.position.y = originY; - msg.info.origin.position.z = originZ; - msg.info.width = mapX; - msg.info.height = mapY; - msg.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0); - // Pack columns into message - msg.lists.clear(); - for (unsigned int k = 0; k < updateList.size(); k++) - { - multi_map_server::VerticalOccupancyGridList c; - updateList[k]->PackMsg(c); - msg.lists.push_back(c); - } - updateList.clear(); - updateCounter++; - } - - void UnpackMsg(const multi_map_server::SparseMap3D &msg) - { - // Unpack column msgs, Replace the whole column - for (unsigned int k = 0; k < msg.lists.size(); k++) - { - int mx, my, mz; - WorldFrameToMapFrame(msg.lists[k].x, msg.lists[k].y, 0, mx, my, mz); - ResizeMapBase(mx, my); - if (mapBase[my*mapX+mx]) delete mapBase[my*mapX+mx]; - mapBase[my*mapX+mx] = new OccupancyGridList; - mapBase[my*mapX+mx]->UnpackMsg(msg.lists[k]); - } - CheckDecayMap(); - updated = true; - } - - inline void SetOccupancyFromWorldFrame(double x, double y, double z, double p = PROB_OCCUPIED) - { - // Find occupancy value to be set - int value = 0; - if (p == PROB_OCCUPIED) value = logOddOccupied; - else if (p == PROB_FREE) value = logOddFree; - else return; - // Update occupancy value, return the column that have been changed - int mx, my, mz; - WorldFrameToMapFrame(x, y, z, mx, my, mz); - ResizeMapBase(mx, my); - if (!mapBase[my*mapX+mx]) - mapBase[my*mapX+mx] = new OccupancyGridList; - mapBase[my*mapX+mx]->SetOccupancyValue(mz, value); - // Also record the column that have been changed in another list, for publish incremental map - if (mapBase[my*mapX+mx]->GetUpdateCounter() != updateCounter) - { - updateList.push_back(mapBase[my*mapX+mx]); - mapBase[my*mapX+mx]->SetUpdateCounterXY(updateCounter, x, y); - } - updated = true; - } - - inline int GetOccupancyFromWorldFrame(double x, double y, double z) - { - int mx, my, mz; - WorldFrameToMapFrame(x, y, z, mx, my, mz); - if (mx < 0 || my < 0 || mx >= mapX || my >= mapY) - return 0; - if (!mapBase[my*mapX+mx]) - return 0; - else - { - if (mapBase[my*mapX+mx]->GetOccupancyValue(mz) > logOddOccupiedThr) - return 1; - else if (mapBase[my*mapX+mx]->GetOccupancyValue(mz) < logOddFreeThr) - return -1; - else - return 0; - } - } - - inline void DeleteFromWorldFrame(double x, double y, double z) - { - int mx, my, mz; - WorldFrameToMapFrame(x, y, z, mx, my, mz); - if (mx < 0 || my < 0 || mx >= mapX || my >= mapY) - return; - if (mapBase[my*mapX+mx]) - mapBase[my*mapX+mx]->DeleteOccupancyGrid(mz); - } - - vector& GetOccupancyWorldFrame(int type = OCCUPIED) - { - pts.clear(); - for (int mx = 0; mx < mapX; mx++) - { - for (int my = 0; my < mapY; my++) - { - if (mapBase[my*mapX+mx]) - { - vector grids; - mapBase[my*mapX+mx]->GetOccupancyGrids(grids); - for (unsigned int k = 0; k < grids.size(); k++) - { - if ( (grids[k].mass / (grids[k].upper - grids[k].lower + 1) > logOddOccupiedThr && type == OCCUPIED) || - (grids[k].mass / (grids[k].upper - grids[k].lower + 1) < logOddFreeThr && type == FREE) ) - { - for (int mz = grids[k].lower; mz <= grids[k].upper; mz++) - { - double x, y, z; - MapFrameToWorldFrame(mx, my, mz, x, y, z); - arma::colvec pt(3); - pt(0) = x; - pt(1) = y; - pt(2) = z; - pts.push_back(pt); - } - } - } - } - } - } - return pts; - } - - // Do not allow setting parameters if at least one update received - void SetResolution(double _resolution) - { - if (!updated) - resolution = _resolution; - } - - void SetDecayInterval(double _decayInterval) - { - if (!updated && _decayInterval > 0) - decayInterval = _decayInterval; - } - - inline double GetResolution() { return resolution; } - inline double GetMaxX() { return originX + mapX*resolution; } - inline double GetMinX() { return originX; } - inline double GetMaxY() { return originY + mapY*resolution; } - inline double GetMinY() { return originY; } - inline bool Updated() { return updated; } - -private: - - inline void WorldFrameToMapFrame(double x, double y, double z, int& mx, int& my, int& mz) - { - mx = (x - originX) / resolution; - my = (y - originY) / resolution; - mz = (z - originZ) / resolution; - } - - inline void MapFrameToWorldFrame(int mx, int my, int mz, double& x, double& y, double& z) - { - double r = 0.5*resolution; - x = mx * resolution + r + originX; - y = my * resolution + r + originY; - z = mz * resolution + r + originZ; - } - - inline void ResizeMapBase(int& mx, int& my) - { - if (mx < 0 || my < 0 || mx >= mapX || my >= mapY) - { - double prevOriginX = originX; - double prevOriginY = originY; - int prevMapX = mapX; - int prevMapY = mapY; - while(mx < 0) - { - mapX += expandStep; - mx += expandStep; - originX -= expandStep*resolution; - } - while(my < 0) - { - mapY += expandStep; - my += expandStep; - originY -= expandStep*resolution; - } - while(mx >= mapX) - { - mapX += expandStep; - } - while(my >= mapY) - { - mapY += expandStep; - } - vector _mapBase = mapBase; - mapBase.clear(); - mapBase.resize(mapX*mapY,NULL); - for (int _x = 0; _x < prevMapX; _x++) - { - for(int _y = 0; _y < prevMapY; _y++) - { - int x = _x + round((prevOriginX - originX) / resolution); - int y = _y + round((prevOriginY - originY) / resolution); - mapBase[y*mapX+x] = _mapBase[_y*prevMapX+_x]; - } - } - } - } - - void CheckDecayMap() - { - if (decayInterval < 0) - return; - // Check whether to decay - static ros::Time prevDecayT = ros::Time::now(); - ros::Time t = ros::Time::now(); - double dt = (t - prevDecayT).toSec(); - if (dt > decayInterval) - { - double r = pow(LOG_ODD_DECAY_RATE, dt); - for (int mx = 0; mx < mapX; mx++) - for (int my = 0; my < mapY; my++) - if (mapBase[my*mapX+mx]) - mapBase[my*mapX+mx]->Decay(logOddOccupiedFixedThr, logOddFreeFixedThr, r); - prevDecayT = t; - } - } - - double resolution; - double decayInterval; - - int logOddOccupied; - int logOddFree; - int logOddOccupiedThr; - int logOddOccupiedFixedThr; - int logOddFreeThr; - int logOddFreeFixedThr; - - bool updated; - int updateCounter; - vector updateList; - - double originX, originY, originZ; - int mapX, mapY; - int expandStep; - vector mapBase; - - vector pts; - -}; -#endif diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/mainpage.dox b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/mainpage.dox deleted file mode 100644 index 3a1fb201..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/mainpage.dox +++ /dev/null @@ -1,26 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b multi_map_server is ... - - - - -\section codeapi Code API - - - - -*/ diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/MultiOccupancyGrid.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/MultiOccupancyGrid.msg deleted file mode 100644 index d7b020d3..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/MultiOccupancyGrid.msg +++ /dev/null @@ -1,2 +0,0 @@ -nav_msgs/OccupancyGrid[] maps -geometry_msgs/Pose[] origins diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/MultiSparseMap3D.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/MultiSparseMap3D.msg deleted file mode 100644 index c7be2d78..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/MultiSparseMap3D.msg +++ /dev/null @@ -1,2 +0,0 @@ -SparseMap3D[] maps -geometry_msgs/Pose[] origins diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/SparseMap3D.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/SparseMap3D.msg deleted file mode 100644 index 777fb861..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/SparseMap3D.msg +++ /dev/null @@ -1,4 +0,0 @@ -Header header -nav_msgs/MapMetaData info -VerticalOccupancyGridList[] lists - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/VerticalOccupancyGridList.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/VerticalOccupancyGridList.msg deleted file mode 100644 index 379ffaae..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/VerticalOccupancyGridList.msg +++ /dev/null @@ -1,6 +0,0 @@ -float32 x -float32 y -int32[] upper -int32[] lower -int32[] mass - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/package.xml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/package.xml deleted file mode 100644 index f037f19b..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - multi_map_server - multi_map_server - 0.0.0 - Shaojie Shen - BSD - - http://ros.org/wiki/multi_map_server - - catkin - - roscpp - visualization_msgs - geometry_msgs - tf - nav_msgs - std_srvs - laser_geometry - pose_utils - message_generation - quadrotor_msgs - - roscpp - visualization_msgs - geometry_msgs - tf - nav_msgs - std_srvs - laser_geometry - pose_utils - message_runtime - quadrotor_msgs - - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/__init__.py b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/__init__.py deleted file mode 100644 index b23e43d7..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/__init__.py +++ /dev/null @@ -1 +0,0 @@ -#autogenerated by ROS python message generators \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/_MultiOccupancyGrid.py b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/_MultiOccupancyGrid.py deleted file mode 100644 index bf2de496..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/_MultiOccupancyGrid.py +++ /dev/null @@ -1,404 +0,0 @@ -"""autogenerated by genpy from multi_map_server/MultiOccupancyGrid.msg. Do not edit.""" -import sys -python3 = True if sys.hexversion > 0x03000000 else False -import genpy -import struct - -import geometry_msgs.msg -import nav_msgs.msg -import genpy -import std_msgs.msg - -class MultiOccupancyGrid(genpy.Message): - _md5sum = "61e63a291f11a6b1796a1edf79f34f72" - _type = "multi_map_server/MultiOccupancyGrid" - _has_header = False #flag to mark the presence of a Header object - _full_text = """nav_msgs/OccupancyGrid[] maps -geometry_msgs/Pose[] origins - -================================================================================ -MSG: nav_msgs/OccupancyGrid -# This represents a 2-D grid map, in which each cell represents the probability of -# occupancy. - -Header header - -#MetaData for the map -MapMetaData info - -# The map data, in row-major order, starting with (0,0). Occupancy -# probabilities are in the range [0,100]. Unknown is -1. -int8[] data - -================================================================================ -MSG: std_msgs/Header -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. -# -# sequence ID: consecutively increasing ID -uint32 seq -#Two-integer timestamp that is expressed as: -# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') -# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') -# time-handling sugar is provided by the client library -time stamp -#Frame this data is associated with -# 0: no frame -# 1: global frame -string frame_id - -================================================================================ -MSG: nav_msgs/MapMetaData -# This hold basic information about the characterists of the OccupancyGrid - -# The time at which the map was loaded -time map_load_time -# The map resolution [m/cell] -float32 resolution -# Map width [cells] -uint32 width -# Map height [cells] -uint32 height -# The origin of the map [m, m, rad]. This is the real-world pose of the -# cell (0,0) in the map. -geometry_msgs/Pose origin -================================================================================ -MSG: geometry_msgs/Pose -# A representation of pose in free space, composed of postion and orientation. -Point position -Quaternion orientation - -================================================================================ -MSG: geometry_msgs/Point -# This contains the position of a point in free space -float64 x -float64 y -float64 z - -================================================================================ -MSG: geometry_msgs/Quaternion -# This represents an orientation in free space in quaternion form. - -float64 x -float64 y -float64 z -float64 w - -""" - __slots__ = ['maps','origins'] - _slot_types = ['nav_msgs/OccupancyGrid[]','geometry_msgs/Pose[]'] - - def __init__(self, *args, **kwds): - """ - Constructor. Any message fields that are implicitly/explicitly - set to None will be assigned a default value. The recommend - use is keyword arguments as this is more robust to future message - changes. You cannot mix in-order arguments and keyword arguments. - - The available fields are: - maps,origins - - :param args: complete set of field values, in .msg order - :param kwds: use keyword arguments corresponding to message field names - to set specific fields. - """ - if args or kwds: - super(MultiOccupancyGrid, self).__init__(*args, **kwds) - #message fields cannot be None, assign default values for those that are - if self.maps is None: - self.maps = [] - if self.origins is None: - self.origins = [] - else: - self.maps = [] - self.origins = [] - - def _get_types(self): - """ - internal API method - """ - return self._slot_types - - def serialize(self, buff): - """ - serialize message into buffer - :param buff: buffer, ``StringIO`` - """ - try: - length = len(self.maps) - buff.write(_struct_I.pack(length)) - for val1 in self.maps: - _v1 = val1.header - buff.write(_struct_I.pack(_v1.seq)) - _v2 = _v1.stamp - _x = _v2 - buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) - _x = _v1.frame_id - length = len(_x) - if python3 or type(_x) == unicode: - _x = _x.encode('utf-8') - length = len(_x) - if python3: - buff.write(struct.pack(' 0x03000000 else False -import genpy -import struct - -import geometry_msgs.msg -import multi_map_server.msg -import nav_msgs.msg -import genpy -import std_msgs.msg - -class MultiSparseMap3D(genpy.Message): - _md5sum = "2e3d76c98ee3e2b23a422f64965f6418" - _type = "multi_map_server/MultiSparseMap3D" - _has_header = False #flag to mark the presence of a Header object - _full_text = """SparseMap3D[] maps -geometry_msgs/Pose[] origins - -================================================================================ -MSG: multi_map_server/SparseMap3D -Header header -nav_msgs/MapMetaData info -VerticalOccupancyGridList[] lists - - -================================================================================ -MSG: std_msgs/Header -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. -# -# sequence ID: consecutively increasing ID -uint32 seq -#Two-integer timestamp that is expressed as: -# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') -# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') -# time-handling sugar is provided by the client library -time stamp -#Frame this data is associated with -# 0: no frame -# 1: global frame -string frame_id - -================================================================================ -MSG: nav_msgs/MapMetaData -# This hold basic information about the characterists of the OccupancyGrid - -# The time at which the map was loaded -time map_load_time -# The map resolution [m/cell] -float32 resolution -# Map width [cells] -uint32 width -# Map height [cells] -uint32 height -# The origin of the map [m, m, rad]. This is the real-world pose of the -# cell (0,0) in the map. -geometry_msgs/Pose origin -================================================================================ -MSG: geometry_msgs/Pose -# A representation of pose in free space, composed of postion and orientation. -Point position -Quaternion orientation - -================================================================================ -MSG: geometry_msgs/Point -# This contains the position of a point in free space -float64 x -float64 y -float64 z - -================================================================================ -MSG: geometry_msgs/Quaternion -# This represents an orientation in free space in quaternion form. - -float64 x -float64 y -float64 z -float64 w - -================================================================================ -MSG: multi_map_server/VerticalOccupancyGridList -float32 x -float32 y -int32[] upper -int32[] lower -int32[] mass - - -""" - __slots__ = ['maps','origins'] - _slot_types = ['multi_map_server/SparseMap3D[]','geometry_msgs/Pose[]'] - - def __init__(self, *args, **kwds): - """ - Constructor. Any message fields that are implicitly/explicitly - set to None will be assigned a default value. The recommend - use is keyword arguments as this is more robust to future message - changes. You cannot mix in-order arguments and keyword arguments. - - The available fields are: - maps,origins - - :param args: complete set of field values, in .msg order - :param kwds: use keyword arguments corresponding to message field names - to set specific fields. - """ - if args or kwds: - super(MultiSparseMap3D, self).__init__(*args, **kwds) - #message fields cannot be None, assign default values for those that are - if self.maps is None: - self.maps = [] - if self.origins is None: - self.origins = [] - else: - self.maps = [] - self.origins = [] - - def _get_types(self): - """ - internal API method - """ - return self._slot_types - - def serialize(self, buff): - """ - serialize message into buffer - :param buff: buffer, ``StringIO`` - """ - try: - length = len(self.maps) - buff.write(_struct_I.pack(length)) - for val1 in self.maps: - _v1 = val1.header - buff.write(_struct_I.pack(_v1.seq)) - _v2 = _v1.stamp - _x = _v2 - buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) - _x = _v1.frame_id - length = len(_x) - if python3 or type(_x) == unicode: - _x = _x.encode('utf-8') - length = len(_x) - if python3: - buff.write(struct.pack(' 0x03000000 else False -import genpy -import struct - -import geometry_msgs.msg -import multi_map_server.msg -import nav_msgs.msg -import genpy -import std_msgs.msg - -class SparseMap3D(genpy.Message): - _md5sum = "a20102f0b3a02e95070dab4140b78fb5" - _type = "multi_map_server/SparseMap3D" - _has_header = True #flag to mark the presence of a Header object - _full_text = """Header header -nav_msgs/MapMetaData info -VerticalOccupancyGridList[] lists - - -================================================================================ -MSG: std_msgs/Header -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. -# -# sequence ID: consecutively increasing ID -uint32 seq -#Two-integer timestamp that is expressed as: -# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') -# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') -# time-handling sugar is provided by the client library -time stamp -#Frame this data is associated with -# 0: no frame -# 1: global frame -string frame_id - -================================================================================ -MSG: nav_msgs/MapMetaData -# This hold basic information about the characterists of the OccupancyGrid - -# The time at which the map was loaded -time map_load_time -# The map resolution [m/cell] -float32 resolution -# Map width [cells] -uint32 width -# Map height [cells] -uint32 height -# The origin of the map [m, m, rad]. This is the real-world pose of the -# cell (0,0) in the map. -geometry_msgs/Pose origin -================================================================================ -MSG: geometry_msgs/Pose -# A representation of pose in free space, composed of postion and orientation. -Point position -Quaternion orientation - -================================================================================ -MSG: geometry_msgs/Point -# This contains the position of a point in free space -float64 x -float64 y -float64 z - -================================================================================ -MSG: geometry_msgs/Quaternion -# This represents an orientation in free space in quaternion form. - -float64 x -float64 y -float64 z -float64 w - -================================================================================ -MSG: multi_map_server/VerticalOccupancyGridList -float32 x -float32 y -int32[] upper -int32[] lower -int32[] mass - - -""" - __slots__ = ['header','info','lists'] - _slot_types = ['std_msgs/Header','nav_msgs/MapMetaData','multi_map_server/VerticalOccupancyGridList[]'] - - def __init__(self, *args, **kwds): - """ - Constructor. Any message fields that are implicitly/explicitly - set to None will be assigned a default value. The recommend - use is keyword arguments as this is more robust to future message - changes. You cannot mix in-order arguments and keyword arguments. - - The available fields are: - header,info,lists - - :param args: complete set of field values, in .msg order - :param kwds: use keyword arguments corresponding to message field names - to set specific fields. - """ - if args or kwds: - super(SparseMap3D, self).__init__(*args, **kwds) - #message fields cannot be None, assign default values for those that are - if self.header is None: - self.header = std_msgs.msg.Header() - if self.info is None: - self.info = nav_msgs.msg.MapMetaData() - if self.lists is None: - self.lists = [] - else: - self.header = std_msgs.msg.Header() - self.info = nav_msgs.msg.MapMetaData() - self.lists = [] - - def _get_types(self): - """ - internal API method - """ - return self._slot_types - - def serialize(self, buff): - """ - serialize message into buffer - :param buff: buffer, ``StringIO`` - """ - try: - _x = self - buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) - _x = self.header.frame_id - length = len(_x) - if python3 or type(_x) == unicode: - _x = _x.encode('utf-8') - length = len(_x) - if python3: - buff.write(struct.pack(' 0x03000000 else False -import genpy -import struct - - -class VerticalOccupancyGridList(genpy.Message): - _md5sum = "7ef85cc95b82747f51eb01a16bd7c795" - _type = "multi_map_server/VerticalOccupancyGridList" - _has_header = False #flag to mark the presence of a Header object - _full_text = """float32 x -float32 y -int32[] upper -int32[] lower -int32[] mass - - -""" - __slots__ = ['x','y','upper','lower','mass'] - _slot_types = ['float32','float32','int32[]','int32[]','int32[]'] - - def __init__(self, *args, **kwds): - """ - Constructor. Any message fields that are implicitly/explicitly - set to None will be assigned a default value. The recommend - use is keyword arguments as this is more robust to future message - changes. You cannot mix in-order arguments and keyword arguments. - - The available fields are: - x,y,upper,lower,mass - - :param args: complete set of field values, in .msg order - :param kwds: use keyword arguments corresponding to message field names - to set specific fields. - """ - if args or kwds: - super(VerticalOccupancyGridList, self).__init__(*args, **kwds) - #message fields cannot be None, assign default values for those that are - if self.x is None: - self.x = 0. - if self.y is None: - self.y = 0. - if self.upper is None: - self.upper = [] - if self.lower is None: - self.lower = [] - if self.mass is None: - self.mass = [] - else: - self.x = 0. - self.y = 0. - self.upper = [] - self.lower = [] - self.mass = [] - - def _get_types(self): - """ - internal API method - """ - return self._slot_types - - def serialize(self, buff): - """ - serialize message into buffer - :param buff: buffer, ``StringIO`` - """ - try: - _x = self - buff.write(_struct_2f.pack(_x.x, _x.y)) - length = len(self.upper) - buff.write(_struct_I.pack(length)) - pattern = '<%si'%length - buff.write(struct.pack(pattern, *self.upper)) - length = len(self.lower) - buff.write(_struct_I.pack(length)) - pattern = '<%si'%length - buff.write(struct.pack(pattern, *self.lower)) - length = len(self.mass) - buff.write(_struct_I.pack(length)) - pattern = '<%si'%length - buff.write(struct.pack(pattern, *self.mass)) - except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x)))) - except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x)))) - - def deserialize(self, str): - """ - unpack serialized message in str into this message instance - :param str: byte array of serialized message, ``str`` - """ - try: - end = 0 - _x = self - start = end - end += 8 - (_x.x, _x.y,) = _struct_2f.unpack(str[start:end]) - start = end - end += 4 - (length,) = _struct_I.unpack(str[start:end]) - pattern = '<%si'%length - start = end - end += struct.calcsize(pattern) - self.upper = struct.unpack(pattern, str[start:end]) - start = end - end += 4 - (length,) = _struct_I.unpack(str[start:end]) - pattern = '<%si'%length - start = end - end += struct.calcsize(pattern) - self.lower = struct.unpack(pattern, str[start:end]) - start = end - end += 4 - (length,) = _struct_I.unpack(str[start:end]) - pattern = '<%si'%length - start = end - end += struct.calcsize(pattern) - self.mass = struct.unpack(pattern, str[start:end]) - return self - except struct.error as e: - raise genpy.DeserializationError(e) #most likely buffer underfill - - - def serialize_numpy(self, buff, numpy): - """ - serialize message with numpy array types into buffer - :param buff: buffer, ``StringIO`` - :param numpy: numpy python module - """ - try: - _x = self - buff.write(_struct_2f.pack(_x.x, _x.y)) - length = len(self.upper) - buff.write(_struct_I.pack(length)) - pattern = '<%si'%length - buff.write(self.upper.tostring()) - length = len(self.lower) - buff.write(_struct_I.pack(length)) - pattern = '<%si'%length - buff.write(self.lower.tostring()) - length = len(self.mass) - buff.write(_struct_I.pack(length)) - pattern = '<%si'%length - buff.write(self.mass.tostring()) - except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x)))) - except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x)))) - - def deserialize_numpy(self, str, numpy): - """ - unpack serialized message in str into this message instance using numpy for array types - :param str: byte array of serialized message, ``str`` - :param numpy: numpy python module - """ - try: - end = 0 - _x = self - start = end - end += 8 - (_x.x, _x.y,) = _struct_2f.unpack(str[start:end]) - start = end - end += 4 - (length,) = _struct_I.unpack(str[start:end]) - pattern = '<%si'%length - start = end - end += struct.calcsize(pattern) - self.upper = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) - start = end - end += 4 - (length,) = _struct_I.unpack(str[start:end]) - pattern = '<%si'%length - start = end - end += struct.calcsize(pattern) - self.lower = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) - start = end - end += 4 - (length,) = _struct_I.unpack(str[start:end]) - pattern = '<%si'%length - start = end - end += struct.calcsize(pattern) - self.mass = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) - return self - except struct.error as e: - raise genpy.DeserializationError(e) #most likely buffer underfill - -_struct_I = genpy.struct_I -_struct_2f = struct.Struct("<2f") diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/__init__.py b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/__init__.py deleted file mode 100644 index 29e57955..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/__init__.py +++ /dev/null @@ -1,4 +0,0 @@ -from ._SparseMap3D import * -from ._MultiOccupancyGrid import * -from ._MultiSparseMap3D import * -from ._VerticalOccupancyGridList import * diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_visualization.cc b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_visualization.cc deleted file mode 100644 index 7013fefc..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_visualization.cc +++ /dev/null @@ -1,90 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -ros::Publisher pub1; -ros::Publisher pub2; - -vector maps2d; -vector origins2d; -vector maps3d; -vector origins3d; - -void maps2d_callback(const multi_map_server::MultiOccupancyGrid::ConstPtr &msg) -{ - // Merge map - maps2d.resize(msg->maps.size(), Map2D(4)); - for (unsigned int k = 0; k < msg->maps.size(); k++) - maps2d[k].Replace(msg->maps[k]); - origins2d = msg->origins; - // Assemble and publish map - multi_map_server::MultiOccupancyGrid m; - m.maps.resize(maps2d.size()); - m.origins.resize(maps2d.size()); - for (unsigned int k = 0; k < maps2d.size(); k++) - { - m.maps[k] = maps2d[k].GetMap(); - m.origins[k] = origins2d[k]; - } - pub1.publish(m); -} - -void maps3d_callback(const multi_map_server::MultiSparseMap3D::ConstPtr &msg) -{ - // Update incremental map - maps3d.resize(msg->maps.size()); - for (unsigned int k = 0; k < msg->maps.size(); k++) - maps3d[k].UnpackMsg(msg->maps[k]); - origins3d = msg->origins; - // Publish - sensor_msgs::PointCloud m; - for (unsigned int k = 0; k < msg->maps.size(); k++) - { - colvec po(6); - po(0) = origins3d[k].position.x; - po(1) = origins3d[k].position.y; - po(2) = origins3d[k].position.z; - colvec poq(4); - poq(0) = origins3d[k].orientation.w; - poq(1) = origins3d[k].orientation.x; - poq(2) = origins3d[k].orientation.y; - poq(3) = origins3d[k].orientation.z; - po.rows(3,5) = R_to_ypr(quaternion_to_R(poq)); - colvec tpo = po.rows(0,2); - mat Rpo = ypr_to_R(po.rows(3,5)); - vector pts = maps3d[k].GetOccupancyWorldFrame(OCCUPIED); - for (unsigned int i = 0; i < pts.size(); i++) - { - colvec pt = Rpo * pts[i] + tpo; - geometry_msgs::Point32 _pt; - _pt.x = pt(0); - _pt.y = pt(1); - _pt.z = pt(2); - m.points.push_back(_pt); - } - } - // Publish - m.header.stamp = ros::Time::now(); - m.header.frame_id = string("/map"); - pub2.publish(m); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "multi_map_visualization"); - ros::NodeHandle n("~"); - - ros::Subscriber sub1 = n.subscribe("dmaps2d", 1, maps2d_callback); - ros::Subscriber sub2 = n.subscribe("dmaps3d", 1, maps3d_callback); - pub1 = n.advertise("maps2d", 1, true); - pub2 = n.advertise("map3d", 1, true); - - ros::spin(); - return 0; -} - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/unused/multi_map_server.cc b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/unused/multi_map_server.cc deleted file mode 100644 index a96d953b..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/unused/multi_map_server.cc +++ /dev/null @@ -1,192 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace arma; -using namespace std; -#define MAX_MAP_CNT 25 - -ros::Publisher pub1; -ros::Publisher pub2; - -// 2D Map -int maps2dCnt = 0; -Map2D maps2d[MAX_MAP_CNT]; -map_server_3d_new::MultiOccupancyGrid grids2d; -// 3D Map -int maps3dCnt = 0; -Map3D maps3d[MAX_MAP_CNT]; -// Map origin from UKF -vector maps_origin; - -void map2d_callback(const nav_msgs::OccupancyGrid::ConstPtr &msg) -{ - // Update msg and publish - if (grids2d.maps.size() == 0) - { - // Init msg - grids2d.maps.push_back(*msg); - maps2dCnt++; - } - else if (grids2d.maps.back().info.map_load_time != msg->info.map_load_time) - { - // Add Costmap - nav_msgs::OccupancyGrid m; - maps2d[maps2dCnt-1].get_map(m); - mapMatcher.AddCostMap(m); - // Increase msg size - grids2d.maps.back().data.clear(); - grids2d.maps.push_back(*msg); - maps2dCnt++; - } - else - { - grids2d.maps.back() = *msg; - } - pub1.publish(grids2d); - // Update internval map - maps2d[maps2dCnt-1].update(*msg); -} - -void scan_callback(const sensor_msgs::LaserScan::ConstPtr &msg) -{ - // Get Map Status - static bool isSLAMValid = false; - if (msg->intensities[10]) - { - if (!isSLAMValid) - { - maps3dCnt++; - mapLinks.push_back(zeros(3)); - } - isSLAMValid = true; - } - else - { - isSLAMValid = false; - return; - } - // Scan cnt - mapLinks.back()(2)++; - // Get Current Pose - colvec pose(6); - pose(0) = msg->intensities[0]; - pose(1) = msg->intensities[1]; - pose(2) = msg->intensities[2]; - pose(3) = msg->intensities[3]; - pose(4) = msg->intensities[4]; - pose(5) = msg->intensities[5]; - colvec pose2D(3); - pose2D(0) = msg->intensities[0]; - pose2D(1) = msg->intensities[1]; - pose2D(2) = msg->intensities[3]; - double currFloor = msg->intensities[7]; - double currLaserH = msg->intensities[8]; - // Horizontal laser scans - sensor_msgs::LaserScan scan = *msg; - scan.intensities.clear(); - laser_geometry::LaserProjection projector; - sensor_msgs::PointCloud scanCloud; - projector.projectLaser(scan, scanCloud); - mat scanMat(3, scanCloud.points.size()); - // Get scan in body frame - for (int k = 0; k < scanCloud.points.size(); k++) - { - scanMat(0,k) = scanCloud.points[k].x; - scanMat(1,k) = scanCloud.points[k].y; - scanMat(2,k) = 0.215 - 0.09; - } - // Downsample Laser scan to map resolution - double resolution = maps3d[maps3dCnt-1].GetResolution(); - double px = NUM_INF; - double py = NUM_INF; - int cnt = 0; - mat scanMatDown = zeros(3, scanMat.n_cols); - for (int k = 0; k < scanMat.n_cols; k++) - { - double x = scanMat(0,k); - double y = scanMat(1,k); - double dist = (x-px)*(x-px) + (y-py)*(y-py); - if (dist > resolution * resolution) - { - scanMatDown.col(cnt) = scanMat.col(k); - px = x; - py = y; - cnt++; - } - } - if (cnt > 0) - scanMat = scanMatDown.cols(0, cnt-1); - else - scanMat = scanMatDown.cols(0,cnt); - // Transform to map local frame - scanMat = ypr_to_R(pose.rows(3,5)) * scanMat + pose.rows(0,2)*ones(scanMat.n_cols); - // Update map - for (int k = 0; k < scanMat.n_cols; k++) - maps3d[maps3dCnt-1].SetOccupancyFromWorldFrame(scanMat(0,k), scanMat(1,k), scanMat(2,k), PROB_LASER_OCCUPIED); - // downward facing laser scans - if (currLaserH > 0) - { - colvec pt(3); - pt(0) = 0; - pt(1) = 0; - pt(2) = -currLaserH; - pt = ypr_to_R(pose.rows(3,5)) * pt + pose.rows(0,2); - double resolution = maps3d[maps3dCnt-1].GetResolution(); - for (double x = -0.1; x <= 0.1; x+=resolution) - for (double y = -0.1; y <= 0.1; y+=resolution) - maps3d[maps3dCnt-1].SetOccupancyFromWorldFrame(pt(0)+x, pt(1)+y, pt(2), PROB_LASER_OCCUPIED); - } - // Floor Levels - maps3d[maps3dCnt-1].SetFloor(currFloor); -} - -void maps_origin_callback(const geometry_msgs::PoseArray::ConstPtr &msg) -{ - maps_origin = msg->poses; -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "multi_map_server_3d"); - ros::NodeHandle n("~"); - - ros::Subscriber sub1 = n.subscribe("dmap2d", 100, map2d_callback); - ros::Subscriber sub2 = n.subscribe("scan", 100, scan_callback); - ros::Subscriber sub3 = n.subscribe("/maps_origin", 100, maps_origin_callback); - pub1 = n.advertise("dmaps2d", 10, true); - pub2 = n.advertise( "dmaps3d", 10, true); - - ros::Rate r(100.0); - int cnt = 0; - while (n.ok()) - { - cnt++; - if (cnt > 100) - { - cnt = 0; - map_server_3d_new::MultiSparseMap3D msg; - msg.maps_origin = maps_origin; - for (int k = 0; k < maps3dCnt; k++) - { - msg.maps_active.push_back((bool)(!mapLinks[k](0)) && mapLinks[k](2) > 50); - map_server_3d_new::SparseMap3D m; - maps3d[k].GetSparseMap3DMsg(m); - m.header.seq = k; - msg.maps.push_back(m); - } - pub2.publish(msg); - } - ros::spinOnce(); - r.sleep(); - } - return 0; -} diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/CMakeLists.txt b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/CMakeLists.txt deleted file mode 100644 index 396d4a58..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/CMakeLists.txt +++ /dev/null @@ -1,211 +0,0 @@ -#cmake_minimum_required(VERSION 2.4.6) -#include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -# Set the build type. Options are: -# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage -# Debug : w/ debug symbols, w/o optimization -# Release : w/o debug symbols, w/ optimization -# RelWithDebInfo : w/ debug symbols, w/ optimization -# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries -#set(ROS_BUILD_TYPE RelWithDebInfo) - -#rosbuild_init() - -#set the default path for built executables to the "bin" directory -#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -#set the default path for built libraries to the "lib" directory -#set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) - -#uncomment if you have defined messages -#rosbuild_genmsg() -#uncomment if you have defined services -#rosbuild_gensrv() - -#common commands for building c++ executables and libraries -#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) -#target_link_libraries(${PROJECT_NAME} another_library) -#rosbuild_add_boost_directories() -#rosbuild_link_boost(${PROJECT_NAME} thread) -#rosbuild_add_executable(example examples/example.cpp) -#target_link_libraries(example ${PROJECT_NAME}) - -#rosbuild_add_executable(odom_visualization src/odom_visualization.cpp) -#target_link_libraries(odom_visualization pose_utils) - -#---------------------------------- -cmake_minimum_required(VERSION 2.8.3) -project(odom_visualization) - -set(CMAKE_BUILD_TYPE "Release") -set(CMAKE_CXX_FLAGS "-O3 -Wall -g") - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - sensor_msgs - nav_msgs - visualization_msgs - quadrotor_msgs - tf - pose_utils -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependencies might have been -## pulled in transitively but can be declared for certainty nonetheless: -## * add a build_depend tag for "message_generation" -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -#add_message_files( -# FILES -# MultiOccupancyGrid.msg -# MultiSparseMap3D.msg -# SparseMap3D.msg -# VerticalOccupancyGridList.msg - #plan_cmd.msg -#) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -#generate_messages( -# DEPENDENCIES -# geometry_msgs -# nav_msgs -#) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES irobot_msgs -# CATKIN_DEPENDS geometry_msgs nav_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### -find_package(Armadillo REQUIRED) -include_directories(${ARMADILLO_INCLUDE_DIRS}) - - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a cpp library -# add_library(irobot_msgs -# src/${PROJECT_NAME}/irobot_msgs.cpp -# ) - -## Declare a cpp executable -add_executable(odom_visualization src/odom_visualization.cpp) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(multi_map_visualization multi_map_server_messages_cpp) - -## Specify libraries to link a library or executable target against -target_link_libraries(odom_visualization - ${catkin_LIBRARIES} - ${ARMADILLO_LIBRARIES} - pose_utils -) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS irobot_msgs irobot_msgs_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_irobot_msgs.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/Makefile b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/Makefile deleted file mode 100644 index b75b928f..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/mainpage.dox b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/mainpage.dox deleted file mode 100644 index 3c09d69e..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/mainpage.dox +++ /dev/null @@ -1,26 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b odom_visualization is ... - - - - -\section codeapi Code API - - - - -*/ diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/meshes/hummingbird.mesh b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/meshes/hummingbird.mesh deleted file mode 100644 index dcde17c5..00000000 Binary files a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/meshes/hummingbird.mesh and /dev/null differ diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/package.xml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/package.xml deleted file mode 100644 index f8756918..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - 0.0.0 - odom_visualization - - - odom_visualization - - - Shaojie Shen - BSD - http://ros.org/wiki/odom_visualization - - catkin - - roscpp - sensor_msgs - nav_msgs - visualization_msgs - tf - pose_utils - - roscpp - sensor_msgs - nav_msgs - visualization_msgs - tf - pose_utils - - - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp deleted file mode 100644 index 67c57fb0..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp +++ /dev/null @@ -1,472 +0,0 @@ -#include -#include -#include "ros/ros.h" -#include "tf/transform_broadcaster.h" -#include "nav_msgs/Odometry.h" -#include "geometry_msgs/PoseWithCovarianceStamped.h" -#include "geometry_msgs/PoseStamped.h" -#include "nav_msgs/Path.h" -#include "sensor_msgs/Range.h" -#include "visualization_msgs/Marker.h" -#include "armadillo" -#include "pose_utils.h" -#include "quadrotor_msgs/PositionCommand.h" - -using namespace arma; -using namespace std; - -static string mesh_resource; -static double color_r, color_g, color_b, color_a, cov_scale, scale; - -bool cross_config = false; -bool tf45 = false; -bool cov_pos = false; -bool cov_vel = false; -bool cov_color = false; -bool origin = false; -bool isOriginSet = false; -colvec poseOrigin(6); -ros::Publisher posePub; -ros::Publisher pathPub; -ros::Publisher velPub; -ros::Publisher covPub; -ros::Publisher covVelPub; -ros::Publisher trajPub; -ros::Publisher sensorPub; -ros::Publisher meshPub; -ros::Publisher heightPub; -tf::TransformBroadcaster* broadcaster; -geometry_msgs::PoseStamped poseROS; -nav_msgs::Path pathROS; -visualization_msgs::Marker velROS; -visualization_msgs::Marker covROS; -visualization_msgs::Marker covVelROS; -visualization_msgs::Marker trajROS; -visualization_msgs::Marker sensorROS; -visualization_msgs::Marker meshROS; -sensor_msgs::Range heightROS; -string _frame_id; - -void odom_callback(const nav_msgs::Odometry::ConstPtr& msg) -{ - if (msg->header.frame_id == string("null")) - return; - colvec pose(6); - pose(0) = msg->pose.pose.position.x; - pose(1) = msg->pose.pose.position.y; - pose(2) = msg->pose.pose.position.z; - colvec q(4); - q(0) = msg->pose.pose.orientation.w; - q(1) = msg->pose.pose.orientation.x; - q(2) = msg->pose.pose.orientation.y; - q(3) = msg->pose.pose.orientation.z; - pose.rows(3,5) = R_to_ypr(quaternion_to_R(q)); - colvec vel(3); - vel(0) = msg->twist.twist.linear.x; - vel(1) = msg->twist.twist.linear.y; - vel(2) = msg->twist.twist.linear.z; - - if (origin && !isOriginSet) - { - isOriginSet = true; - poseOrigin = pose; - } - if (origin) - { - vel = trans(ypr_to_R(pose.rows(3,5))) * vel; - pose = pose_update(pose_inverse(poseOrigin), pose); - vel = ypr_to_R(pose.rows(3,5)) * vel; - } - - // Pose - poseROS.header = msg->header; - poseROS.header.stamp = msg->header.stamp; - poseROS.header.frame_id = string("world"); - poseROS.pose.position.x = pose(0); - poseROS.pose.position.y = pose(1); - poseROS.pose.position.z = pose(2); - q = R_to_quaternion(ypr_to_R(pose.rows(3,5))); - poseROS.pose.orientation.w = q(0); - poseROS.pose.orientation.x = q(1); - poseROS.pose.orientation.y = q(2); - poseROS.pose.orientation.z = q(3); - posePub.publish(poseROS); - - // Velocity - colvec yprVel(3); - yprVel(0) = atan2(vel(1), vel(0)); - yprVel(1) = -atan2(vel(2), norm(vel.rows(0,1),2)); - yprVel(2) = 0; - q = R_to_quaternion(ypr_to_R(yprVel)); - velROS.header.frame_id = string("world"); - velROS.header.stamp = msg->header.stamp; - velROS.ns = string("velocity"); - velROS.id = 0; - velROS.type = visualization_msgs::Marker::ARROW; - velROS.action = visualization_msgs::Marker::ADD; - velROS.pose.position.x = pose(0); - velROS.pose.position.y = pose(1); - velROS.pose.position.z = pose(2); - velROS.pose.orientation.w = q(0); - velROS.pose.orientation.x = q(1); - velROS.pose.orientation.y = q(2); - velROS.pose.orientation.z = q(3); - velROS.scale.x = norm(vel, 2); - velROS.scale.y = 0.05; - velROS.scale.z = 0.05; - velROS.color.a = 1.0; - velROS.color.r = color_r; - velROS.color.g = color_g; - velROS.color.b = color_b; - velPub.publish(velROS); - - // Path - static ros::Time prevt = msg->header.stamp; - if ((msg->header.stamp - prevt).toSec() > 0.1) - { - prevt = msg->header.stamp; - pathROS.header = poseROS.header; - pathROS.poses.push_back(poseROS); - pathPub.publish(pathROS); - } - - // Covariance color - double r = 1; - double g = 1; - double b = 1; - bool G = msg->twist.covariance[33]; - bool V = msg->twist.covariance[34]; - bool L = msg->twist.covariance[35]; - if (cov_color) - { - r = G; - g = V; - b = L; - } - - // Covariance Position - if (cov_pos) - { - mat P(6,6); - for (int j = 0; j < 6; j++) - for (int i = 0; i < 6; i++) - P(i,j) = msg->pose.covariance[i+j*6]; - colvec eigVal; - mat eigVec; - eig_sym(eigVal, eigVec, P.submat(0,0,2,2)); - if (det(eigVec) < 0) - { - for (int k = 0; k < 3; k++) - { - mat eigVecRev = eigVec; - eigVecRev.col(k) *= -1; - if (det(eigVecRev) > 0) - { - eigVec = eigVecRev; - break; - } - } - } - covROS.header.frame_id = string("world"); - covROS.header.stamp = msg->header.stamp; - covROS.ns = string("covariance"); - covROS.id = 0; - covROS.type = visualization_msgs::Marker::SPHERE; - covROS.action = visualization_msgs::Marker::ADD; - covROS.pose.position.x = pose(0); - covROS.pose.position.y = pose(1); - covROS.pose.position.z = pose(2); - q = R_to_quaternion(eigVec); - covROS.pose.orientation.w = q(0); - covROS.pose.orientation.x = q(1); - covROS.pose.orientation.y = q(2); - covROS.pose.orientation.z = q(3); - covROS.scale.x = sqrt(eigVal(0))*cov_scale; - covROS.scale.y = sqrt(eigVal(1))*cov_scale; - covROS.scale.z = sqrt(eigVal(2))*cov_scale; - covROS.color.a = 0.4; - covROS.color.r = r * 0.5; - covROS.color.g = g * 0.5; - covROS.color.b = b * 0.5; - covPub.publish(covROS); - } - - // Covariance Velocity - if (cov_vel) - { - mat P(3,3); - for (int j = 0; j < 3; j++) - for (int i = 0; i < 3; i++) - P(i,j) = msg->twist.covariance[i+j*6]; - mat R = ypr_to_R(pose.rows(3,5)); - P = R * P * trans(R); - colvec eigVal; - mat eigVec; - eig_sym(eigVal, eigVec, P); - if (det(eigVec) < 0) - { - for (int k = 0; k < 3; k++) - { - mat eigVecRev = eigVec; - eigVecRev.col(k) *= -1; - if (det(eigVecRev) > 0) - { - eigVec = eigVecRev; - break; - } - } - } - covVelROS.header.frame_id = string("world"); - covVelROS.header.stamp = msg->header.stamp; - covVelROS.ns = string("covariance_velocity"); - covVelROS.id = 0; - covVelROS.type = visualization_msgs::Marker::SPHERE; - covVelROS.action = visualization_msgs::Marker::ADD; - covVelROS.pose.position.x = pose(0); - covVelROS.pose.position.y = pose(1); - covVelROS.pose.position.z = pose(2); - q = R_to_quaternion(eigVec); - covVelROS.pose.orientation.w = q(0); - covVelROS.pose.orientation.x = q(1); - covVelROS.pose.orientation.y = q(2); - covVelROS.pose.orientation.z = q(3); - covVelROS.scale.x = sqrt(eigVal(0))*cov_scale; - covVelROS.scale.y = sqrt(eigVal(1))*cov_scale; - covVelROS.scale.z = sqrt(eigVal(2))*cov_scale; - covVelROS.color.a = 0.4; - covVelROS.color.r = r; - covVelROS.color.g = g; - covVelROS.color.b = b; - covVelPub.publish(covVelROS); - } - - // Color Coded Trajectory - static colvec ppose = pose; - static ros::Time pt = msg->header.stamp; - ros::Time t = msg->header.stamp; - if ((t - pt).toSec() > 0.5) - { - trajROS.header.frame_id = string("world"); - trajROS.header.stamp = ros::Time::now(); - trajROS.ns = string("trajectory"); - trajROS.type = visualization_msgs::Marker::LINE_LIST; - trajROS.action = visualization_msgs::Marker::ADD; - trajROS.pose.position.x = 0; - trajROS.pose.position.y = 0; - trajROS.pose.position.z = 0; - trajROS.pose.orientation.w = 1; - trajROS.pose.orientation.x = 0; - trajROS.pose.orientation.y = 0; - trajROS.pose.orientation.z = 0; - trajROS.scale.x = 0.1; - trajROS.scale.y = 0; - trajROS.scale.z = 0; - trajROS.color.r = 0.0; - trajROS.color.g = 1.0; - trajROS.color.b = 0.0; - trajROS.color.a = 0.8; - geometry_msgs::Point p; - p.x = ppose(0); - p.y = ppose(1); - p.z = ppose(2); - trajROS.points.push_back(p); - p.x = pose(0); - p.y = pose(1); - p.z = pose(2); - trajROS.points.push_back(p); - std_msgs::ColorRGBA color; - color.r = r; - color.g = g; - color.b = b; - color.a = 1; - trajROS.colors.push_back(color); - trajROS.colors.push_back(color); - ppose = pose; - pt = t; - trajPub.publish(trajROS); - } - - // Sensor availability - sensorROS.header.frame_id = string("world"); - sensorROS.header.stamp = msg->header.stamp; - sensorROS.ns = string("sensor"); - sensorROS.type = visualization_msgs::Marker::TEXT_VIEW_FACING; - sensorROS.action = visualization_msgs::Marker::ADD; - sensorROS.pose.position.x = pose(0); - sensorROS.pose.position.y = pose(1); - sensorROS.pose.position.z = pose(2) + 1.0; - sensorROS.pose.orientation.w = q(0); - sensorROS.pose.orientation.x = q(1); - sensorROS.pose.orientation.y = q(2); - sensorROS.pose.orientation.z = q(3); - string strG = G?string(" GPS "):string(""); - string strV = V?string(" Vision "):string(""); - string strL = L?string(" Laser "):string(""); - sensorROS.text = "| " + strG + strV + strL + " |"; - sensorROS.color.a = 1.0; - sensorROS.color.r = 1.0; - sensorROS.color.g = 1.0; - sensorROS.color.b = 1.0; - sensorROS.scale.z = 0.5; - sensorPub.publish(sensorROS); - - // Laser height measurement - double H = msg->twist.covariance[32]; - heightROS.header.frame_id = string("height"); - heightROS.header.stamp = msg->header.stamp; - heightROS.radiation_type = sensor_msgs::Range::ULTRASOUND; - heightROS.field_of_view = 5.0 * M_PI / 180.0; - heightROS.min_range = -100; - heightROS.max_range = 100; - heightROS.range = H; - heightPub.publish(heightROS); - - // Mesh model - meshROS.header.frame_id = _frame_id; - meshROS.header.stamp = msg->header.stamp; - meshROS.ns = "mesh"; - meshROS.id = 0; - meshROS.type = visualization_msgs::Marker::MESH_RESOURCE; - meshROS.action = visualization_msgs::Marker::ADD; - meshROS.pose.position.x = msg->pose.pose.position.x; - meshROS.pose.position.y = msg->pose.pose.position.y; - meshROS.pose.position.z = msg->pose.pose.position.z; - q(0) = msg->pose.pose.orientation.w; - q(1) = msg->pose.pose.orientation.x; - q(2) = msg->pose.pose.orientation.y; - q(3) = msg->pose.pose.orientation.z; - if (cross_config) - { - colvec ypr = R_to_ypr(quaternion_to_R(q)); - ypr(0) += 45.0*PI/180.0; - q = R_to_quaternion(ypr_to_R(ypr)); - } - meshROS.pose.orientation.w = q(0); - meshROS.pose.orientation.x = q(1); - meshROS.pose.orientation.y = q(2); - meshROS.pose.orientation.z = q(3); - meshROS.scale.x = scale; - meshROS.scale.y = scale; - meshROS.scale.z = scale; - meshROS.color.a = color_a; - meshROS.color.r = color_r; - meshROS.color.g = color_g; - meshROS.color.b = color_b; - meshROS.mesh_resource = mesh_resource; - meshPub.publish(meshROS); - - // TF for raw sensor visualization - if (tf45) - { - tf::Transform transform; - transform.setOrigin(tf::Vector3(pose(0), pose(1), pose(2))); - transform.setRotation(tf::Quaternion(q(1), q(2), q(3), q(0))); - - tf::Transform transform45; - transform45.setOrigin(tf::Vector3(0, 0, 0)); - colvec y45 = zeros(3); - y45(0) = 45.0 * M_PI/180; - colvec q45 = R_to_quaternion(ypr_to_R(y45)); - transform45.setRotation(tf::Quaternion(q45(1), q45(2), q45(3), q45(0))); - - tf::Transform transform90; - transform90.setOrigin(tf::Vector3(0, 0, 0)); - colvec p90 = zeros(3); - p90(1) = 90.0 * M_PI/180; - colvec q90 = R_to_quaternion(ypr_to_R(p90)); - transform90.setRotation(tf::Quaternion(q90(1), q90(2), q90(3), q90(0))); - - broadcaster->sendTransform(tf::StampedTransform(transform, msg->header.stamp, string("world"), string("base"))); - broadcaster->sendTransform(tf::StampedTransform(transform45, msg->header.stamp, string("base"), string("laser"))); - broadcaster->sendTransform(tf::StampedTransform(transform45, msg->header.stamp, string("base"), string("vision"))); - broadcaster->sendTransform(tf::StampedTransform(transform90, msg->header.stamp, string("base"), string("height"))); - } -} - -void cmd_callback(const quadrotor_msgs::PositionCommand cmd) -{ - if (cmd.header.frame_id == string("null")) - return; - - colvec pose(6); - pose(0) = cmd.position.x; - pose(1) = cmd.position.y; - pose(2) = cmd.position.z; - colvec q(4); - q(0) = 1.0; - q(1) = 0.0; - q(2) = 0.0; - q(3) = 0.0; - pose.rows(3,5) = R_to_ypr(quaternion_to_R(q)); - - // Mesh model - meshROS.header.frame_id = _frame_id; - meshROS.header.stamp = cmd.header.stamp; - meshROS.ns = "mesh"; - meshROS.id = 0; - meshROS.type = visualization_msgs::Marker::MESH_RESOURCE; - meshROS.action = visualization_msgs::Marker::ADD; - meshROS.pose.position.x = cmd.position.x; - meshROS.pose.position.y = cmd.position.y; - meshROS.pose.position.z = cmd.position.z; - - if (cross_config) - { - colvec ypr = R_to_ypr(quaternion_to_R(q)); - ypr(0) += 45.0*PI/180.0; - q = R_to_quaternion(ypr_to_R(ypr)); - } - meshROS.pose.orientation.w = q(0); - meshROS.pose.orientation.x = q(1); - meshROS.pose.orientation.y = q(2); - meshROS.pose.orientation.z = q(3); - meshROS.scale.x = 2.0; - meshROS.scale.y = 2.0; - meshROS.scale.z = 2.0; - meshROS.color.a = color_a; - meshROS.color.r = color_r; - meshROS.color.g = color_g; - meshROS.color.b = color_b; - meshROS.mesh_resource = mesh_resource; - meshPub.publish(meshROS); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "odom_visualization"); - ros::NodeHandle n("~"); - - n.param("mesh_resource", mesh_resource, std::string("package://odom_visualization/meshes/hummingbird.mesh")); - n.param("color/r", color_r, 1.0); - n.param("color/g", color_g, 0.0); - n.param("color/b", color_b, 0.0); - n.param("color/a", color_a, 1.0); - n.param("origin", origin, false); - n.param("robot_scale", scale, 2.0); - n.param("frame_id", _frame_id, string("world") ); - - n.param("cross_config", cross_config, false); - n.param("tf45", tf45, false); - n.param("covariance_scale", cov_scale, 100.0); - n.param("covariance_position", cov_pos, false); - n.param("covariance_velocity", cov_vel, false); - n.param("covariance_color", cov_color, false); - - ros::Subscriber sub_odom = n.subscribe("odom", 100, odom_callback); - ros::Subscriber sub_cmd = n.subscribe("cmd", 100, cmd_callback); - posePub = n.advertise("pose", 100, true); - pathPub = n.advertise( "path", 100, true); - velPub = n.advertise("velocity", 100, true); - covPub = n.advertise("covariance", 100, true); - covVelPub = n.advertise("covariance_velocity", 100, true); - trajPub = n.advertise("trajectory", 100, true); - sensorPub = n.advertise("sensor", 100, true); - meshPub = n.advertise("robot", 100, true); - heightPub = n.advertise( "height", 100, true); - tf::TransformBroadcaster b; - broadcaster = &b; - - ros::spin(); - - return 0; -} diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/CMakeLists.txt b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/CMakeLists.txt deleted file mode 100644 index 87beff30..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(pose_utils) - -find_package(catkin REQUIRED COMPONENTS - #armadillo - roscpp -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES pose_utils -# CATKIN_DEPENDS geometry_msgs nav_msgs -# DEPENDS system_lib -) - -find_package(Armadillo REQUIRED) - -include_directories( - ${catkin_INCLUDE_DIRS} - ${ARMADILLO_INCLUDE_DIRS} - include - ) - -add_library(pose_utils - ${ARMADILLO_LIBRARIES} - src/pose_utils.cpp) diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/Makefile b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/Makefile deleted file mode 100644 index b75b928f..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/include/pose_utils.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/include/pose_utils.h deleted file mode 100644 index 326c7c8f..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/include/pose_utils.h +++ /dev/null @@ -1,53 +0,0 @@ -#ifndef POSE_UTILS_H -#define POSE_UTILS_H - -#include -#include "armadillo" - -#define PI 3.14159265359 -#define NUM_INF 999999.9 - -using namespace arma; -using namespace std; - -// Rotation --------------------- -mat ypr_to_R(const colvec& ypr); - -mat yaw_to_R(double yaw); - -colvec R_to_ypr(const mat& R); - -mat quaternion_to_R(const colvec& q); - -colvec R_to_quaternion(const mat& R); - -colvec quaternion_mul(const colvec& q1, const colvec& q2); - -colvec quaternion_inv(const colvec& q); - -// General Pose Update ---------- -colvec pose_update(const colvec& X1, const colvec& X2); - -colvec pose_inverse(const colvec& X); - -colvec pose_update_2d(const colvec& X1, const colvec& X2); - -colvec pose_inverse_2d(const colvec& X); - -// For Pose EKF ----------------- -mat Jplus1(const colvec& X1, const colvec& X2); - -mat Jplus2(const colvec& X1, const colvec& X2); - -// For IMU EKF ------------------ -colvec state_update(const colvec& X, const colvec& U, double dt); - -mat jacobianF(const colvec& X, const colvec& U, double dt); - -mat jacobianU(const colvec& X, const colvec& U, double dt); - -colvec state_measure(const colvec& X); - -mat jacobianH(); - -#endif diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/package.xml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/package.xml deleted file mode 100644 index df593f9b..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/package.xml +++ /dev/null @@ -1,15 +0,0 @@ - - pose_utils - 0.0.0 - pose_utils - Shaojie Shen - BSD - catkin - roscpp - roscpp - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/src/pose_utils.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/src/pose_utils.cpp deleted file mode 100644 index f2b43657..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/src/pose_utils.cpp +++ /dev/null @@ -1,531 +0,0 @@ -#include "pose_utils.h" - -// Rotation --------------------- - -mat ypr_to_R(const colvec& ypr) -{ - double c, s; - mat Rz = zeros(3,3); - double y = ypr(0); - c = cos(y); - s = sin(y); - Rz(0,0) = c; - Rz(1,0) = s; - Rz(0,1) = -s; - Rz(1,1) = c; - Rz(2,2) = 1; - - mat Ry = zeros(3,3); - double p = ypr(1); - c = cos(p); - s = sin(p); - Ry(0,0) = c; - Ry(2,0) = -s; - Ry(0,2) = s; - Ry(2,2) = c; - Ry(1,1) = 1; - - mat Rx = zeros(3,3); - double r = ypr(2); - c = cos(r); - s = sin(r); - Rx(1,1) = c; - Rx(2,1) = s; - Rx(1,2) = -s; - Rx(2,2) = c; - Rx(0,0) = 1; - - mat R = Rz*Ry*Rx; - return R; -} - -mat yaw_to_R(double yaw) -{ - mat R = zeros(2,2); - double c = cos(yaw); - double s = sin(yaw); - R(0,0) = c; - R(1,0) = s; - R(0,1) = -s; - R(1,1) = c; - return R; -} - -colvec R_to_ypr(const mat& R) -{ - colvec n = R.col(0); - colvec o = R.col(1); - colvec a = R.col(2); - - colvec ypr(3); - double y = atan2(n(1), n(0)); - double p = atan2(-n(2), n(0)*cos(y)+n(1)*sin(y)); - double r = atan2(a(0)*sin(y)-a(1)*cos(y), -o(0)*sin(y)+o(1)*cos(y)); - ypr(0) = y; - ypr(1) = p; - ypr(2) = r; - - return ypr; -} - -mat quaternion_to_R(const colvec& q) -{ - double n = norm(q, 2); - colvec nq = q / n; - - double w = nq(0); - double x = nq(1); - double y = nq(2); - double z = nq(3); - double w2 = w*w; - double x2 = x*x; - double y2 = y*y; - double z2 = z*z; - double xy = x*y; - double xz = x*z; - double yz = y*z; - double wx = w*x; - double wy = w*y; - double wz = w*z; - - mat R = zeros(3,3); - R(0,0) = w2+x2-y2-z2; - R(1,0) = 2*(wz + xy); - R(2,0) = 2*(xz - wy); - R(0,1) = 2*(xy - wz); - R(1,1) = w2-x2+y2-z2; - R(2,1) = 2*(wx + yz); - R(0,2) = 2*(wy + xz); - R(1,2) = 2*(yz - wx); - R(2,2) = w2-x2-y2+z2; - return R; -} - -colvec R_to_quaternion(const mat& R) -{ - colvec q(4); - double tr = R(0,0) + R(1,1) + R(2,2); - if (tr > 0) - { - double S = sqrt(tr + 1.0) * 2; - q(0) = 0.25 * S; - q(1) = (R(2,1) - R(1,2)) / S; - q(2) = (R(0,2) - R(2,0)) / S; - q(3) = (R(1,0) - R(0,1)) / S; - } - else if (R(0,0) > R(1,1) && R(0,0) > R(2,2)) - { - double S = sqrt(1.0 + R(0,0) - R(1,1) - R(2,2)) * 2; - q(0) = (R(2,1) - R(1,2)) / S; - q(1) = 0.25 * S; - q(2) = (R(0,1) + R(1,0)) / S; - q(3) = (R(0,2) + R(2,0)) / S; - } - else if (R(1,1) > R(2,2)) - { - double S = sqrt(1.0 + R(1,1) - R(0,0) - R(2,2)) * 2; - q(0) = (R(0,2) - R(2,0)) / S; - q(1) = (R(0,1) + R(1,0)) / S; - q(2) = 0.25 * S; - q(3) = (R(1,2) + R(2,1)) / S; - } - else - { - double S = sqrt(1.0 + R(2,2) - R(0,0) - R(1,1)) * 2; - q(0) = (R(1,0) - R(0,1)) / S; - q(1) = (R(0,2) + R(2,0)) / S; - q(2) = (R(1,2) + R(2,1)) / S; - q(3) = 0.25 * S; - } - return q; -} - -colvec quaternion_mul(const colvec& q1, const colvec& q2) -{ - double a1 = q1(0); - double b1 = q1(1); - double c1 = q1(2); - double d1 = q1(3); - - double a2 = q2(0); - double b2 = q2(1); - double c2 = q2(2); - double d2 = q2(3); - - colvec q3(4); - q3(0) = a1*a2 - b1*b2 - c1*c2 - d1*d2; - q3(1) = a1*b2 + b1*a2 + c1*d2 - d1*c2; - q3(2) = a1*c2 - b1*d2 + c1*a2 + d1*b2; - q3(3) = a1*d2 + b1*c2 - c1*b2 + d1*a2; - return q3; -} - -colvec quaternion_inv(const colvec& q) -{ - colvec q2(4); - q2(0) = q(0); - q2(1) = -q(1); - q2(2) = -q(2); - q2(3) = -q(3); - return q2; -} - -// General Pose Update ---------- - -colvec pose_update(const colvec& X1, const colvec& X2) -{ - mat R1 = ypr_to_R(X1.rows(3,5)); - mat R2 = ypr_to_R(X2.rows(3,5)); - mat R3 = R1 * R2; - - colvec X3xyz = X1.rows(0,2) + R1*X2.rows(0,2); - colvec X3ypr = R_to_ypr(R3); - - colvec X3 = join_cols(X3xyz, X3ypr); - return X3; -} - -colvec pose_inverse(const colvec& X) -{ - mat R = ypr_to_R(X.rows(3,5)); - colvec n = R.col(0); - colvec o = R.col(1); - colvec a = R.col(2); - - colvec XIxyz = -trans(R) * (X.rows(0,2)); - colvec XIypr(3); - double XIy = atan2(o(0), n(0)); - double XIp = atan2(-a(0), n(0)*cos(XIy)+o(0)*sin(XIy)); - double XIr = atan2(n(2)*sin(XIy)-o(2)*cos(XIy), -n(1)*sin(XIy)+o(1)*cos(XIy)); - XIypr(0) = XIy; - XIypr(1) = XIp; - XIypr(2) = XIr; - - colvec XI = join_cols(XIxyz, XIypr); - return XI; -} - -colvec pose_update_2d(const colvec& X1, const colvec& X2) -{ - mat R = yaw_to_R(X1(2)); - colvec X3(3); - X3.rows(0,1) = R * X2.rows(0,1) + X1.rows(0,1); - X3(2) = X1(2) + X2(2); - return X3; -} - -colvec pose_inverse_2d(const colvec& X) -{ - double c = cos(X(2)); - double s = sin(X(2)); - colvec XI(3); - XI(0) = -X(0) * c - X(1) * s; - XI(1) = X(0) * s - X(1) * c; - XI(2) = -X(2); - return XI; -} - - -// For Pose EKF ---------------------- - -mat Jplus1(const colvec& X1, const colvec& X2) -{ - colvec X3 = pose_update(X1,X2); - mat R1 = ypr_to_R(X1.rows(3,5)); - mat R2 = ypr_to_R(X2.rows(3,5)); - mat R3 = ypr_to_R(X3.rows(3,5)); - colvec o1 = R1.col(1); - colvec a1 = R1.col(2); - colvec o2 = R2.col(1); - colvec a2 = R2.col(2); - - mat I = eye(3,3); - mat Z = zeros(3,3); - - double Marr[9] = { -(X3(2-1)-X1(2-1)) , (X3(3-1)-X1(3-1))*cos(X1(4-1)) , a1(1-1)*X2(2-1)-o1(1-1)*X2(3-1) , - X3(1-1)-X1(1-1) , (X3(3-1)-X1(3-1))*sin(X1(4-1)) , a1(2-1)*X2(2-1)-o1(2-1)*X2(3-1) , - 0 , -X2(1-1)*cos(X1(5-1))-X2(2-1)*sin(X1(5-1))*sin(X1(6-1))-X2(3-1)*sin(X1(5-1))*cos(X1(6-1)) , a1(3-1)*X2(2-1)-o1(3-1)*X2(3-1) }; - mat M(3,3); - for (int i =0; i < 9; i++) - M(i) = Marr[i]; - M = trans(M); - - double K1arr[9] = { 1 , sin(X3(5-1))*sin(X3(4-1)-X1(4-1))/cos(X3(5-1)) , ( o2(1-1)*sin(X3(6-1))+a2(1-1)*cos(X3(6-1)) )/cos(X3(5-1)) , - 0 , cos(X3(4-1)-X1(4-1)) , -cos(X1(5-1))*sin(X3(4-1)-X1(4-1)) , - 0 , sin(X3(4-1)-X1(4-1))/cos(X3(5-1)) , cos(X1(5-1))*cos(X3(4-1)-X1(4-1))/cos(X3(5-1)) }; - mat K1(3,3); - for (int i =0; i < 9; i++) - K1(i) = K1arr[i]; - K1 = trans(K1); - - mat J1 = join_cols ( join_rows(I, M) , join_rows(Z,K1) ); - return J1; -} - -mat Jplus2(const colvec& X1, const colvec& X2) -{ - colvec X3 = pose_update(X1,X2); - mat R1 = ypr_to_R(X1.rows(3,5)); - mat R2 = ypr_to_R(X2.rows(3,5)); - mat R3 = ypr_to_R(X3.rows(3,5)); - colvec o1 = R1.col(1); - colvec a1 = R1.col(2); - colvec o2 = R2.col(1); - colvec a2 = R2.col(2); - - mat Z = zeros(3,3); - - double K2arr[9] = { cos(X2(5-1))*cos(X3(6-1)-X2(6-1))/cos(X3(5-1)) , sin(X3(6-1)-X2(6-1))/cos(X3(5-1)) , 0 , - -cos(X2(5-1))*sin(X3(6-1)-X2(6-1)) , cos(X3(6-1)-X2(6-1)) , 0 , - ( a1(1-1)*cos(X3(4-1))+a1(2-1)*sin(X3(4-1)) )/cos(X3(5-1)) , sin(X3(5-1))*sin(X3(6-1)-X2(6-1))/cos(X3(5-1)) , 1 }; - mat K2(3,3); - for (int i =0; i < 9; i++) - K2(i) = K2arr[i]; - K2 = trans(K2); - - mat J2 = join_cols ( join_rows(R1,Z) , join_rows(Z,K2) ); - return J2; -} - - -// For IMU EKF ---------------------- - -colvec state_update(const colvec& X, const colvec& U, double dt) -{ - double ro = X(3); - double pt = X(4); - double ya = X(5); - - colvec ypr(3); - ypr(0) = ya; - ypr(1) = pt; - ypr(2) = ro; - mat R = ypr_to_R(ypr); - - mat M(3,3); - M(0,0) = 1; M(0,1) = 0; M(0,2) = -sin(pt); - M(1,0) = 0; M(1,1) = cos(ro); M(1,2) = cos(pt)*sin(ro); - M(2,0) = 0; M(2,1) = -sin(ro); M(2,2) = cos(pt)*cos(ro); - - colvec Xt(9); - Xt.rows(0,2) = X.rows(0,2) + X.rows(6,8)*dt + R*U.rows(0,2)*dt*dt/2; - Xt.rows(3,5) = X.rows(3,5) + inv(M)*U.rows(3,5)*dt; - Xt.rows(6,8) = X.rows(6,8) + R*U.rows(0,2)*dt; - - return Xt; -} - -mat jacobianF(const colvec& X, const colvec& U, double dt) -{ - double x = X(0); - double y = X(1); - double z = X(2); - double ro = X(3); - double pt = X(4); - double ya = X(5); - double vx = X(6); - double vy = X(7); - double vz = X(8); - double ax = U(0); - double ay = U(1); - double az = U(2); - double wx = U(3); - double wy = U(4); - double wz = U(5); - - mat F(9,9); - - F(0,0) = 1; - F(0,1) = 0; - F(0,2) = 0; - F(0,3) = (dt*dt*(ay*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)) + az*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro))))/2, - F(0,4) = (dt*dt*(az*cos(pt)*cos(ro)*cos(ya) - ax*cos(ya)*sin(pt) + ay*cos(pt)*cos(ya)*sin(ro)))/2; - F(0,5) = -(dt*dt*(ay*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya)) - az*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)) + ax*cos(pt)*sin(ya)))/2; - F(0,6) = dt; - F(0,7) = 0; - F(0,8) = 0; - - F(1,0) = 0; - F(1,1) = 1; - F(1,2) = 0; - F(1,3) = -(dt*dt*(ay*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)) + az*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya))))/2; - F(1,4) = (dt*dt*(az*cos(pt)*cos(ro)*sin(ya) - ax*sin(pt)*sin(ya) + ay*cos(pt)*sin(ro)*sin(ya)))/2; - F(1,5) = (dt*dt*(az*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)) - ay*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro)) + ax*cos(pt)*cos(ya)))/2; - F(1,6) = 0; - F(1,7) = dt; - F(1,8) = 0; - - F(2,0) = 0; - F(2,1) = 0; - F(2,2) = 1; - F(2,3) = (dt*dt*(ay*cos(pt)*cos(ro) - az*cos(pt)*sin(ro)))/2, - F(2,4) = -(dt*dt*(ax*cos(pt) + az*cos(ro)*sin(pt) + ay*sin(pt)*sin(ro)))/2, - F(2,5) = 0; - F(2,6) = 0; - F(2,7) = 0; - F(2,8) = dt; - - F(3,0) = 0; - F(3,1) = 0; - F(3,2) = 0; - F(3,3) = 1 - dt*((wz*(cos(pt - ro)/2 - cos(pt + ro)/2))/cos(pt) - (wy*(sin(pt - ro)/2 + sin(pt + ro)/2))/cos(pt)); - F(3,4) = dt*((wz*(cos(pt - ro)/2 + cos(pt + ro)/2))/cos(pt) - (wy*(sin(pt - ro)/2 - sin(pt + ro)/2))/cos(pt) + - (wy*sin(pt)*(cos(pt - ro)/2 - cos(pt + ro)/2))/(cos(pt)*cos(pt)) + (wz*sin(pt)*(sin(pt - ro)/2 + sin(pt + ro)/2))/(cos(pt)*cos(pt))); - F(3,5) = 0; - F(3,6) = 0; - F(3,7) = 0; - F(3,8) = 0; - - F(4,0) = 0; - F(4,1) = 0; - F(4,2) = 0; - F(4,3) = -dt*(wz*cos(ro) + wy*sin(ro)); - F(4,4) = 1; - F(4,5) = 0; - F(4,6) = 0; - F(4,7) = 0; - F(4,8) = 0; - - F(5,0) = 0; - F(5,1) = 0; - F(5,2) = 0; - F(5,3) = dt*((wy*cos(ro))/cos(pt) - (wz*sin(ro))/cos(pt)); - F(5,4) = dt*((wz*cos(ro)*sin(pt))/(cos(pt)*cos(pt)) + (wy*sin(pt)*sin(ro))/(cos(pt)*cos(pt))); - F(5,5) = 1; - F(5,6) = 0; - F(5,7) = 0; - F(5,8) = 0; - - F(6,0) = 0; - F(6,1) = 0; - F(6,2) = 0; - F(6,3) = dt*(ay*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)) + az*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro))); - F(6,4) = dt*(az*cos(pt)*cos(ro)*cos(ya) - ax*cos(ya)*sin(pt) + ay*cos(pt)*cos(ya)*sin(ro)); - F(6,5) = -dt*(ay*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya)) - az*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)) + ax*cos(pt)*sin(ya)); - F(6,6) = 1; - F(6,7) = 0; - F(6,8) = 0; - - F(7,0) = 0; - F(7,1) = 0; - F(7,2) = 0; - F(7,3) = -dt*(ay*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)) + az*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya))); - F(7,4) = dt*(az*cos(pt)*cos(ro)*sin(ya) - ax*sin(pt)*sin(ya) + ay*cos(pt)*sin(ro)*sin(ya)); - F(7,5) = dt*(az*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)) - ay*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro)) + ax*cos(pt)*cos(ya)); - F(7,6) = 0; - F(7,7) = 1; - F(7,8) = 0; - - F(8,0) = 0; - F(8,1) = 0; - F(8,2) = 0; - F(8,3) = dt*(ay*cos(pt)*cos(ro) - az*cos(pt)*sin(ro)); - F(8,4) = -dt*(ax*cos(pt) + az*cos(ro)*sin(pt) + ay*sin(pt)*sin(ro)); - F(8,5) = 0; - F(8,6) = 0; - F(8,7) = 0; - F(8,8) = 1; - - return F; -} - -mat jacobianU(const colvec& X, const colvec& U, double dt) -{ - double x = X(0); - double y = X(1); - double z = X(2); - double ro = X(3); - double pt = X(4); - double ya = X(5); - double vx = X(6); - double vy = X(7); - double vz = X(8); - double ax = U(0); - double ay = U(1); - double az = U(2); - double wx = U(3); - double wy = U(4); - double wz = U(5); - - mat G(9,6); - - G(0,0) = (dt*dt*cos(pt)*cos(ya))/2; - G(0,1) = -(dt*dt*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro)))/2; - G(0,2) = (dt*dt*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)))/2; - G(0,3) = 0; - G(0,4) = 0; - G(0,5) = 0; - - G(1,0) = (dt*dt*cos(pt)*sin(ya))/2; - G(1,1) = (dt*dt*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya)))/2; - G(1,2) = -(dt*dt*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)))/2; - G(1,3) = 0; - G(1,4) = 0; - G(1,5) = 0; - - G(2,0) = -(dt*dt*sin(pt))/2; - G(2,1) = (dt*dt*cos(pt)*sin(ro))/2; - G(2,2) = (dt*dt*cos(pt)*cos(ro))/2; - G(2,3) = 0; - G(2,4) = 0; - G(2,5) = 0; - - G(3,0) = 0; - G(3,1) = 0; - G(3,2) = 0; - G(3,3) = dt; - G(3,4) = (dt*(cos(pt - ro)/2 - cos(pt + ro)/2))/cos(pt); - G(3,5) = (dt*(sin(pt - ro)/2 + sin(pt + ro)/2))/cos(pt); - - G(4,0) = 0; - G(4,1) = 0; - G(4,2) = 0; - G(4,3) = 0; - G(4,4) = dt*cos(ro); - G(4,5) = -dt*sin(ro); - - G(5,0) = 0; - G(5,1) = 0; - G(5,2) = 0; - G(5,3) = 0; - G(5,4) = (dt*sin(ro))/cos(pt); - G(5,5) = (dt*cos(ro))/cos(pt); - - G(6,0) = dt*cos(pt)*cos(ya); - G(6,1) = -dt*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro)); - G(6,2) = dt*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)); - G(6,3) = 0; - G(6,4) = 0; - G(6,5) = 0; - - G(7,0) = dt*cos(pt)*sin(ya); - G(7,1) = dt*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya)); - G(7,2) = -dt*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)); - G(7,3) = 0; - G(7,4) = 0; - G(7,5) = 0; - - G(8,0) = -dt*sin(pt); - G(8,1) = dt*cos(pt)*sin(ro); - G(8,2) = dt*cos(pt)*cos(ro); - G(8,3) = 0; - G(8,4) = 0; - G(8,5) = 0; - - return G; -} - -colvec state_measure(const colvec& X) -{ - colvec Z = X.rows(0,5); - return Z; -} - -mat jacobianH() -{ - mat H = zeros(6,9); - H.cols(0,5) = eye(6,6); - - return H; -} - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/CMakeLists.txt b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/CMakeLists.txt deleted file mode 100644 index 9838d093..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/CMakeLists.txt +++ /dev/null @@ -1,110 +0,0 @@ -#-cmake_minimum_required(VERSION 2.4.6) -#-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -# Set the build type. Options are: -# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage -# Debug : w/ debug symbols, w/o optimization -# Release : w/o debug symbols, w/ optimization -# RelWithDebInfo : w/ debug symbols, w/ optimization -# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries -#set(ROS_BUILD_TYPE RelWithDebInfo) - -#-rosbuild_init() - -#set the default path for built executables to the "bin" directory -#-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -#set the default path for built libraries to the "lib" directory -#-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) - -#uncomment if you have defined messages -#rosbuild_genmsg() -#uncomment if you have defined services -#rosbuild_gensrv() - -#common commands for building c++ executables and libraries -#-\rosbuild_add_library(${PROJECT_NAME} src/example.cpp) -#target_link_libraries(${PROJECT_NAME} another_library) -#rosbuild_add_boost_directories() -#rosbuild_link_boost(${PROJECT_NAME} thread) -#rosbuild_add_executable(example examples/example.cpp) -#target_link_libraries(example ${PROJECT_NAME}) - -#-rosbuild_add_library(encode_msgs src/encode_msgs.cpp) -#-rosbuild_add_link_flags(encode_msgs -Wl,--as-needed) - -#list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) -#find_package(Eigen3 REQUIRED) - -#include_directories(${EIGEN3_INCLUDE_DIR}) -#-rosbuild_add_library(decode_msgs src/decode_msgs.cpp) -#-rosbuild_add_link_flags(decode_msgs -Wl,--as-needed) - -#------------------------------------------------------------------ -cmake_minimum_required(VERSION 2.8.3) -project(quadrotor_msgs) - -find_package(catkin REQUIRED COMPONENTS - #armadillo - roscpp - nav_msgs - geometry_msgs - message_generation -) - -set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) - -include_directories( - ${catkin_INCLUDE_DIRS} - include - ) - - -add_message_files( - FILES - AuxCommand.msg - Corrections.msg - Gains.msg - OutputData.msg - PositionCommand.msg - PPROutputData.msg - Serial.msg - SO3Command.msg - StatusData.msg - TRPYCommand.msg - Odometry.msg - PolynomialTrajectory.msg - LQRTrajectory.msg - Px4ctrlDebug.msg -) - -generate_messages( - DEPENDENCIES - geometry_msgs - nav_msgs -) - -catkin_package( - #INCLUDE_DIRS include - LIBRARIES encode_msgs decode_msgs - #CATKIN_DEPENDS geometry_msgs nav_msgs - #DEPENDS system_lib - CATKIN_DEPENDS message_runtime -) - -# add_executable(test_exe src/test_exe.cpp) -add_library(decode_msgs src/decode_msgs.cpp) -add_library(encode_msgs src/encode_msgs.cpp) - -# add_dependencies(test_exe quadrotor_msgs_generate_messages_cpp) -add_dependencies(encode_msgs quadrotor_msgs_generate_messages_cpp) -add_dependencies(decode_msgs quadrotor_msgs_generate_messages_cpp) - -find_package(Eigen3 REQUIRED) -include_directories(${EIGEN3_INCLUDE_DIR}) - - -# target_link_libraries(test_exe -# decode_msgs -# encode_msgs -# ) - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/cmake/FindEigen3.cmake b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/cmake/FindEigen3.cmake deleted file mode 100644 index 9c546a05..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/cmake/FindEigen3.cmake +++ /dev/null @@ -1,81 +0,0 @@ -# - Try to find Eigen3 lib -# -# This module supports requiring a minimum version, e.g. you can do -# find_package(Eigen3 3.1.2) -# to require version 3.1.2 or newer of Eigen3. -# -# Once done this will define -# -# EIGEN3_FOUND - system has eigen lib with correct version -# EIGEN3_INCLUDE_DIR - the eigen include directory -# EIGEN3_VERSION - eigen version - -# Copyright (c) 2006, 2007 Montel Laurent, -# Copyright (c) 2008, 2009 Gael Guennebaud, -# Copyright (c) 2009 Benoit Jacob -# Redistribution and use is allowed according to the terms of the 2-clause BSD license. - -if(NOT Eigen3_FIND_VERSION) - if(NOT Eigen3_FIND_VERSION_MAJOR) - set(Eigen3_FIND_VERSION_MAJOR 2) - endif(NOT Eigen3_FIND_VERSION_MAJOR) - if(NOT Eigen3_FIND_VERSION_MINOR) - set(Eigen3_FIND_VERSION_MINOR 91) - endif(NOT Eigen3_FIND_VERSION_MINOR) - if(NOT Eigen3_FIND_VERSION_PATCH) - set(Eigen3_FIND_VERSION_PATCH 0) - endif(NOT Eigen3_FIND_VERSION_PATCH) - - set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") -endif(NOT Eigen3_FIND_VERSION) - -macro(_eigen3_check_version) - file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) - - string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") - set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") - set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") - set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") - - set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) - if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - set(EIGEN3_VERSION_OK FALSE) - else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - set(EIGEN3_VERSION_OK TRUE) - endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - - if(NOT EIGEN3_VERSION_OK) - - message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " - "but at least version ${Eigen3_FIND_VERSION} is required") - endif(NOT EIGEN3_VERSION_OK) -endmacro(_eigen3_check_version) - -if (EIGEN3_INCLUDE_DIR) - - # in cache already - _eigen3_check_version() - set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) - -else (EIGEN3_INCLUDE_DIR) - - find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library - PATHS - ${CMAKE_INSTALL_PREFIX}/include - ${KDE4_INCLUDE_DIR} - PATH_SUFFIXES eigen3 eigen - ) - - if(EIGEN3_INCLUDE_DIR) - _eigen3_check_version() - endif(EIGEN3_INCLUDE_DIR) - - include(FindPackageHandleStandardArgs) - find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) - - mark_as_advanced(EIGEN3_INCLUDE_DIR) - -endif(EIGEN3_INCLUDE_DIR) - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/comm_types.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/comm_types.h deleted file mode 100644 index 42a08aa2..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/comm_types.h +++ /dev/null @@ -1,87 +0,0 @@ -#ifndef __QUADROTOR_MSGS_COMM_TYPES_H__ -#define __QUADROTOR_MSGS_COMM_TYPES_H__ - -#define TYPE_SO3_CMD 's' -struct SO3_CMD_INPUT -{ - // Scaling factors when decoding - int16_t force[3]; // /500 - int8_t des_qx, des_qy, des_qz, des_qw; // /125 - uint8_t kR[3]; // /50 - uint8_t kOm[3]; // /100 - int16_t cur_yaw; // /1e4 - int16_t kf_correction; // /1e11; - uint8_t angle_corrections[2]; // roll,pitch /2500 - uint8_t enable_motors:1; - uint8_t use_external_yaw:1; - uint8_t seq; -}; - -#define TYPE_STATUS_DATA 'c' -struct STATUS_DATA -{ - uint16_t loop_rate; - uint16_t voltage; - uint8_t seq; -}; - -#define TYPE_OUTPUT_DATA 'd' -struct OUTPUT_DATA -{ - uint16_t loop_rate; - uint16_t voltage; - int16_t roll, pitch, yaw; - int16_t ang_vel[3]; - int16_t acc[3]; - int16_t dheight; - int32_t height; - int16_t mag[3]; - uint8_t radio[8]; - //uint8_t rpm[4]; - uint8_t seq; -}; - -#define TYPE_TRPY_CMD 'p' -struct TRPY_CMD -{ - int16_t thrust; - int16_t roll; - int16_t pitch; - int16_t yaw; - int16_t current_yaw; - uint8_t enable_motors:1; - uint8_t use_external_yaw:1; -}; - -#define TYPE_PPR_OUTPUT_DATA 't' -struct PPR_OUTPUT_DATA -{ - uint16_t time; - int16_t des_thrust; - int16_t des_roll; - int16_t des_pitch; - int16_t des_yaw; - int16_t est_roll; - int16_t est_pitch; - int16_t est_yaw; - int16_t est_angvel_x; - int16_t est_angvel_y; - int16_t est_angvel_z; - int16_t est_acc_x; - int16_t est_acc_y; - int16_t est_acc_z; - uint16_t pwm1; - uint16_t pwm2; - uint16_t pwm3; - uint16_t pwm4; -}; - -#define TYPE_PPR_GAINS 'g' -struct PPR_GAINS -{ - int16_t Kp; - int16_t Kd; - int16_t Kp_yaw; - int16_t Kd_yaw; -}; -#endif diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/decode_msgs.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/decode_msgs.h deleted file mode 100644 index a3076ea7..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/decode_msgs.h +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ -#define __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ - -#include -#include -#include -#include -#include - -namespace quadrotor_msgs -{ - -bool decodeOutputData(const std::vector &data, - quadrotor_msgs::OutputData &output); - -bool decodeStatusData(const std::vector &data, - quadrotor_msgs::StatusData &status); - -bool decodePPROutputData(const std::vector &data, - quadrotor_msgs::PPROutputData &output); -} - -#endif diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/encode_msgs.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/encode_msgs.h deleted file mode 100644 index a424b119..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/encode_msgs.h +++ /dev/null @@ -1,22 +0,0 @@ -#ifndef __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ -#define __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ - -#include -#include -#include -#include -#include - -namespace quadrotor_msgs -{ - -void encodeSO3Command(const quadrotor_msgs::SO3Command &so3_command, - std::vector &output); -void encodeTRPYCommand(const quadrotor_msgs::TRPYCommand &trpy_command, - std::vector &output); - -void encodePPRGains(const quadrotor_msgs::Gains &gains, - std::vector &output); -} - -#endif diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/AuxCommand.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/AuxCommand.msg deleted file mode 100644 index f59bf356..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/AuxCommand.msg +++ /dev/null @@ -1,5 +0,0 @@ -float64 current_yaw -float64 kf_correction -float64[2] angle_corrections# Trims for roll, pitch -bool enable_motors -bool use_external_yaw diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Corrections.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Corrections.msg deleted file mode 100644 index e0f4e888..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Corrections.msg +++ /dev/null @@ -1,2 +0,0 @@ -float64 kf_correction -float64[2] angle_corrections diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Gains.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Gains.msg deleted file mode 100644 index f5d10a33..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Gains.msg +++ /dev/null @@ -1,4 +0,0 @@ -float64 Kp -float64 Kd -float64 Kp_yaw -float64 Kd_yaw diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/LQRTrajectory.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/LQRTrajectory.msg deleted file mode 100644 index 0a34e9b6..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/LQRTrajectory.msg +++ /dev/null @@ -1,30 +0,0 @@ -Header header - -# the trajectory id, starts from "1". -uint32 trajectory_id - -# the action command for trajectory server. -uint32 ACTION_ADD = 1 -uint32 ACTION_ABORT = 2 -uint32 ACTION_WARN_START = 3 -uint32 ACTION_WARN_FINAL = 4 -uint32 ACTION_WARN_IMPOSSIBLE = 5 -uint32 action - -# the weight coefficient of the control effort -float64 r - -# the yaw command -float64 start_yaw -float64 final_yaw - -# the initial and final state -float64[6] s0 -float64[3] ut - -float64[6] sf - -# the optimal arrival time -float64 t_f - -string debug_info diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Odometry.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Odometry.msg deleted file mode 100644 index 3272d71a..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Odometry.msg +++ /dev/null @@ -1,8 +0,0 @@ -uint8 STATUS_ODOM_VALID=0 -uint8 STATUS_ODOM_INVALID=1 -uint8 STATUS_ODOM_LOOPCLOSURE=2 - -nav_msgs/Odometry curodom -nav_msgs/Odometry kfodom -uint32 kfid -uint8 status diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/OutputData.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/OutputData.msg deleted file mode 100644 index ac958880..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/OutputData.msg +++ /dev/null @@ -1,12 +0,0 @@ -Header header -uint16 loop_rate -float64 voltage -geometry_msgs/Quaternion orientation -geometry_msgs/Vector3 angular_velocity -geometry_msgs/Vector3 linear_acceleration -float64 pressure_dheight -float64 pressure_height -geometry_msgs/Vector3 magnetic_field -uint8[8] radio_channel -#uint8[4] motor_rpm -uint8 seq diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PPROutputData.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PPROutputData.msg deleted file mode 100644 index 70434a02..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PPROutputData.msg +++ /dev/null @@ -1,16 +0,0 @@ -Header header -uint16 quad_time -float64 des_thrust -float64 des_roll -float64 des_pitch -float64 des_yaw -float64 est_roll -float64 est_pitch -float64 est_yaw -float64 est_angvel_x -float64 est_angvel_y -float64 est_angvel_z -float64 est_acc_x -float64 est_acc_y -float64 est_acc_z -uint16[4] pwm diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg deleted file mode 100644 index 0aab297a..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg +++ /dev/null @@ -1,28 +0,0 @@ -Header header - -# the trajectory id, starts from "1". -uint32 trajectory_id - -# the action command for trajectory server. -uint32 ACTION_ADD = 1 -uint32 ACTION_ABORT = 2 -uint32 ACTION_WARN_START = 3 -uint32 ACTION_WARN_FINAL = 4 -uint32 ACTION_WARN_IMPOSSIBLE = 5 -uint32 action - -# the order of trajectory. -uint32 num_order -uint32 num_segment - -# the polynomial coecfficients of the trajectory. -float64 start_yaw -float64 final_yaw -float64[] coef_x -float64[] coef_y -float64[] coef_z -float64[] time -float64 mag_coeff -uint32[] order - -string debug_info diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg deleted file mode 100644 index 49c6fa1d..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg +++ /dev/null @@ -1,22 +0,0 @@ -Header header -geometry_msgs/Point position -geometry_msgs/Vector3 velocity -geometry_msgs/Vector3 acceleration -geometry_msgs/Vector3 jerk -float64 yaw -float64 yaw_dot -float64[3] kx -float64[3] kv - -uint32 trajectory_id - -uint8 TRAJECTORY_STATUS_EMPTY = 0 -uint8 TRAJECTORY_STATUS_READY = 1 -uint8 TRAJECTORY_STATUS_COMPLETED = 3 -uint8 TRAJECTROY_STATUS_ABORT = 4 -uint8 TRAJECTORY_STATUS_ILLEGAL_START = 5 -uint8 TRAJECTORY_STATUS_ILLEGAL_FINAL = 6 -uint8 TRAJECTORY_STATUS_IMPOSSIBLE = 7 - -# Its ID number will start from 1, allowing you comparing it with 0. -uint8 trajectory_flag diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Px4ctrlDebug.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Px4ctrlDebug.msg deleted file mode 100644 index f28ba2c3..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Px4ctrlDebug.msg +++ /dev/null @@ -1,37 +0,0 @@ -Header header - -float64 des_p_x -float64 des_p_y -float64 des_p_z - -float64 des_v_x -float64 des_v_y -float64 des_v_z - -float64 fb_a_x -float64 fb_a_y -float64 fb_a_z - -float64 des_a_x -float64 des_a_y -float64 des_a_z - -float64 des_q_x -float64 des_q_y -float64 des_q_z -float64 des_q_w - -float64 des_thr -float64 thr2acc -float64 thr_scale_compensate -float64 voltage - -float64 err_axisang_x -float64 err_axisang_y -float64 err_axisang_z -float64 err_axisang_ang - -float64 fb_rate_x -float64 fb_rate_y -float64 fb_rate_z - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/SO3Command.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/SO3Command.msg deleted file mode 100644 index d3868efb..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/SO3Command.msg +++ /dev/null @@ -1,6 +0,0 @@ -Header header -geometry_msgs/Vector3 force -geometry_msgs/Quaternion orientation -float64[3] kR -float64[3] kOm -quadrotor_msgs/AuxCommand aux diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Serial.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Serial.msg deleted file mode 100644 index 5a54cce3..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Serial.msg +++ /dev/null @@ -1,13 +0,0 @@ -# Note: These constants need to be kept in sync with the types -# defined in include/quadrotor_msgs/comm_types.h -uint8 SO3_CMD = 115 # 's' in base 10 -uint8 TRPY_CMD = 112 # 'p' in base 10 -uint8 STATUS_DATA = 99 # 'c' in base 10 -uint8 OUTPUT_DATA = 100 # 'd' in base 10 -uint8 PPR_OUTPUT_DATA = 116 # 't' in base 10 -uint8 PPR_GAINS = 103 # 'g' - -Header header -uint8 channel -uint8 type # One of the types listed above -uint8[] data diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/StatusData.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/StatusData.msg deleted file mode 100644 index d176e4f0..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/StatusData.msg +++ /dev/null @@ -1,4 +0,0 @@ -Header header -uint16 loop_rate -float64 voltage -uint8 seq diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/TRPYCommand.msg b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/TRPYCommand.msg deleted file mode 100644 index 0d471a62..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/TRPYCommand.msg +++ /dev/null @@ -1,6 +0,0 @@ -Header header -float32 thrust -float32 roll -float32 pitch -float32 yaw -quadrotor_msgs/AuxCommand aux diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/package.xml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/package.xml deleted file mode 100644 index 42e9f76e..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - quadrotor_msgs - 0.0.0 - quadrotor_msgs - Kartik Mohta - http://ros.org/wiki/quadrotor_msgs - BSD - catkin - geometry_msgs - nav_msgs - message_generation - geometry_msgs - nav_msgs - message_runtime - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/decode_msgs.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/decode_msgs.cpp deleted file mode 100644 index 609ffb84..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/decode_msgs.cpp +++ /dev/null @@ -1,103 +0,0 @@ -#include "quadrotor_msgs/decode_msgs.h" -#include -#include - -namespace quadrotor_msgs -{ - -bool decodeOutputData(const std::vector &data, - quadrotor_msgs::OutputData &output) -{ - struct OUTPUT_DATA output_data; - if(data.size() != sizeof(output_data)) - return false; - - memcpy(&output_data, &data[0], sizeof(output_data)); - output.loop_rate = output_data.loop_rate; - output.voltage = output_data.voltage/1e3; - - const double roll = output_data.roll/1e2 * M_PI/180; - const double pitch = output_data.pitch/1e2 * M_PI/180; - const double yaw = output_data.yaw/1e2 * M_PI/180; - // Asctec (2012 firmware) uses Z-Y-X convention - Eigen::Quaterniond q = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()); - output.orientation.w = q.w(); - output.orientation.x = q.x(); - output.orientation.y = q.y(); - output.orientation.z = q.z(); - - output.angular_velocity.x = output_data.ang_vel[0]*0.0154*M_PI/180; - output.angular_velocity.y = output_data.ang_vel[1]*0.0154*M_PI/180; - output.angular_velocity.z = output_data.ang_vel[2]*0.0154*M_PI/180; - - output.linear_acceleration.x = output_data.acc[0]/1e3 * 9.81; - output.linear_acceleration.y = output_data.acc[1]/1e3 * 9.81; - output.linear_acceleration.z = output_data.acc[2]/1e3 * 9.81; - - output.pressure_dheight = output_data.dheight/1e3; - output.pressure_height = output_data.height/1e3; - - output.magnetic_field.x = output_data.mag[0]/2500.0; - output.magnetic_field.y = output_data.mag[1]/2500.0; - output.magnetic_field.z = output_data.mag[2]/2500.0; - - for(int i = 0; i < 8; i++) - { - output.radio_channel[i] = output_data.radio[i]; - } - //for(int i = 0; i < 4; i++) - // output.motor_rpm[i] = output_data.rpm[i]; - - output.seq = output_data.seq; - - return true; -} - -bool decodeStatusData(const std::vector &data, - quadrotor_msgs::StatusData &status) -{ - struct STATUS_DATA status_data; - if(data.size() != sizeof(status_data)) - return false; - memcpy(&status_data, &data[0], sizeof(status_data)); - - status.loop_rate = status_data.loop_rate; - status.voltage = status_data.voltage/1e3; - status.seq = status_data.seq; - - return true; -} - -bool decodePPROutputData(const std::vector &data, - quadrotor_msgs::PPROutputData &output) -{ - struct PPR_OUTPUT_DATA output_data; - if(data.size() != sizeof(output_data)) - return false; - memcpy(&output_data, &data[0], sizeof(output_data)); - - output.quad_time = output_data.time; - output.des_thrust = output_data.des_thrust*1e-4; - output.des_roll = output_data.des_roll*1e-4; - output.des_pitch = output_data.des_pitch*1e-4; - output.des_yaw = output_data.des_yaw*1e-4; - output.est_roll = output_data.est_roll*1e-4; - output.est_pitch = output_data.est_pitch*1e-4; - output.est_yaw = output_data.est_yaw*1e-4; - output.est_angvel_x = output_data.est_angvel_x*1e-3; - output.est_angvel_y = output_data.est_angvel_y*1e-3; - output.est_angvel_z = output_data.est_angvel_z*1e-3; - output.est_acc_x = output_data.est_acc_x*1e-4; - output.est_acc_y = output_data.est_acc_y*1e-4; - output.est_acc_z = output_data.est_acc_z*1e-4; - output.pwm[0] = output_data.pwm1; - output.pwm[1] = output_data.pwm2; - output.pwm[2] = output_data.pwm3; - output.pwm[3] = output_data.pwm4; - - return true; -} - -} diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/encode_msgs.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/encode_msgs.cpp deleted file mode 100644 index d0cccf2a..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/encode_msgs.cpp +++ /dev/null @@ -1,73 +0,0 @@ -#include "quadrotor_msgs/encode_msgs.h" -#include - -namespace quadrotor_msgs -{ - -void encodeSO3Command(const quadrotor_msgs::SO3Command &so3_command, - std::vector &output) -{ - struct SO3_CMD_INPUT so3_cmd_input; - - so3_cmd_input.force[0] = so3_command.force.x*500; - so3_cmd_input.force[1] = so3_command.force.y*500; - so3_cmd_input.force[2] = so3_command.force.z*500; - - so3_cmd_input.des_qx = so3_command.orientation.x*125; - so3_cmd_input.des_qy = so3_command.orientation.y*125; - so3_cmd_input.des_qz = so3_command.orientation.z*125; - so3_cmd_input.des_qw = so3_command.orientation.w*125; - - so3_cmd_input.kR[0] = so3_command.kR[0]*50; - so3_cmd_input.kR[1] = so3_command.kR[1]*50; - so3_cmd_input.kR[2] = so3_command.kR[2]*50; - - so3_cmd_input.kOm[0] = so3_command.kOm[0]*100; - so3_cmd_input.kOm[1] = so3_command.kOm[1]*100; - so3_cmd_input.kOm[2] = so3_command.kOm[2]*100; - - so3_cmd_input.cur_yaw = so3_command.aux.current_yaw*1e4; - - so3_cmd_input.kf_correction = so3_command.aux.kf_correction*1e11; - so3_cmd_input.angle_corrections[0] = so3_command.aux.angle_corrections[0]*2500; - so3_cmd_input.angle_corrections[1] = so3_command.aux.angle_corrections[1]*2500; - - so3_cmd_input.enable_motors = so3_command.aux.enable_motors; - so3_cmd_input.use_external_yaw = so3_command.aux.use_external_yaw; - - so3_cmd_input.seq = so3_command.header.seq % 255; - - output.resize(sizeof(so3_cmd_input)); - memcpy(&output[0], &so3_cmd_input, sizeof(so3_cmd_input)); -} - -void encodeTRPYCommand(const quadrotor_msgs::TRPYCommand &trpy_command, - std::vector &output) -{ - struct TRPY_CMD trpy_cmd_input; - trpy_cmd_input.thrust = trpy_command.thrust*1e4; - trpy_cmd_input.roll = trpy_command.roll*1e4; - trpy_cmd_input.pitch = trpy_command.pitch*1e4; - trpy_cmd_input.yaw = trpy_command.yaw*1e4; - trpy_cmd_input.current_yaw = trpy_command.aux.current_yaw*1e4; - trpy_cmd_input.enable_motors = trpy_command.aux.enable_motors; - trpy_cmd_input.use_external_yaw = trpy_command.aux.use_external_yaw; - - output.resize(sizeof(trpy_cmd_input)); - memcpy(&output[0], &trpy_cmd_input, sizeof(trpy_cmd_input)); -} - -void encodePPRGains(const quadrotor_msgs::Gains &gains, - std::vector &output) -{ - struct PPR_GAINS ppr_gains; - ppr_gains.Kp = gains.Kp; - ppr_gains.Kd = gains.Kd; - ppr_gains.Kp_yaw = gains.Kp_yaw; - ppr_gains.Kd_yaw = gains.Kd_yaw; - - output.resize(sizeof(ppr_gains)); - memcpy(&output[0], &ppr_gains, sizeof(ppr_gains)); -} - -} diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/__init__.py b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_AuxCommand.py b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_AuxCommand.py deleted file mode 100644 index 652a2558..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_AuxCommand.py +++ /dev/null @@ -1,143 +0,0 @@ -"""autogenerated by genpy from quadrotor_msgs/AuxCommand.msg. Do not edit.""" -import sys -python3 = True if sys.hexversion > 0x03000000 else False -import genpy -import struct - - -class AuxCommand(genpy.Message): - _md5sum = "94f75840e4b1e03675da764692f2c839" - _type = "quadrotor_msgs/AuxCommand" - _has_header = False #flag to mark the presence of a Header object - _full_text = """float64 current_yaw -float64 kf_correction -float64[2] angle_corrections# Trims for roll, pitch -bool enable_motors -bool use_external_yaw - -""" - __slots__ = ['current_yaw','kf_correction','angle_corrections','enable_motors','use_external_yaw'] - _slot_types = ['float64','float64','float64[2]','bool','bool'] - - def __init__(self, *args, **kwds): - """ - Constructor. Any message fields that are implicitly/explicitly - set to None will be assigned a default value. The recommend - use is keyword arguments as this is more robust to future message - changes. You cannot mix in-order arguments and keyword arguments. - - The available fields are: - current_yaw,kf_correction,angle_corrections,enable_motors,use_external_yaw - - :param args: complete set of field values, in .msg order - :param kwds: use keyword arguments corresponding to message field names - to set specific fields. - """ - if args or kwds: - super(AuxCommand, self).__init__(*args, **kwds) - #message fields cannot be None, assign default values for those that are - if self.current_yaw is None: - self.current_yaw = 0. - if self.kf_correction is None: - self.kf_correction = 0. - if self.angle_corrections is None: - self.angle_corrections = [0.,0.] - if self.enable_motors is None: - self.enable_motors = False - if self.use_external_yaw is None: - self.use_external_yaw = False - else: - self.current_yaw = 0. - self.kf_correction = 0. - self.angle_corrections = [0.,0.] - self.enable_motors = False - self.use_external_yaw = False - - def _get_types(self): - """ - internal API method - """ - return self._slot_types - - def serialize(self, buff): - """ - serialize message into buffer - :param buff: buffer, ``StringIO`` - """ - try: - _x = self - buff.write(_struct_2d.pack(_x.current_yaw, _x.kf_correction)) - buff.write(_struct_2d.pack(*self.angle_corrections)) - _x = self - buff.write(_struct_2B.pack(_x.enable_motors, _x.use_external_yaw)) - except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x)))) - except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x)))) - - def deserialize(self, str): - """ - unpack serialized message in str into this message instance - :param str: byte array of serialized message, ``str`` - """ - try: - end = 0 - _x = self - start = end - end += 16 - (_x.current_yaw, _x.kf_correction,) = _struct_2d.unpack(str[start:end]) - start = end - end += 16 - self.angle_corrections = _struct_2d.unpack(str[start:end]) - _x = self - start = end - end += 2 - (_x.enable_motors, _x.use_external_yaw,) = _struct_2B.unpack(str[start:end]) - self.enable_motors = bool(self.enable_motors) - self.use_external_yaw = bool(self.use_external_yaw) - return self - except struct.error as e: - raise genpy.DeserializationError(e) #most likely buffer underfill - - - def serialize_numpy(self, buff, numpy): - """ - serialize message with numpy array types into buffer - :param buff: buffer, ``StringIO`` - :param numpy: numpy python module - """ - try: - _x = self - buff.write(_struct_2d.pack(_x.current_yaw, _x.kf_correction)) - buff.write(self.angle_corrections.tostring()) - _x = self - buff.write(_struct_2B.pack(_x.enable_motors, _x.use_external_yaw)) - except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x)))) - except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x)))) - - def deserialize_numpy(self, str, numpy): - """ - unpack serialized message in str into this message instance using numpy for array types - :param str: byte array of serialized message, ``str`` - :param numpy: numpy python module - """ - try: - end = 0 - _x = self - start = end - end += 16 - (_x.current_yaw, _x.kf_correction,) = _struct_2d.unpack(str[start:end]) - start = end - end += 16 - self.angle_corrections = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=2) - _x = self - start = end - end += 2 - (_x.enable_motors, _x.use_external_yaw,) = _struct_2B.unpack(str[start:end]) - self.enable_motors = bool(self.enable_motors) - self.use_external_yaw = bool(self.use_external_yaw) - return self - except struct.error as e: - raise genpy.DeserializationError(e) #most likely buffer underfill - -_struct_I = genpy.struct_I -_struct_2d = struct.Struct("<2d") -_struct_2B = struct.Struct("<2B") diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Corrections.py b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Corrections.py deleted file mode 100644 index d2b19c21..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Corrections.py +++ /dev/null @@ -1,111 +0,0 @@ -"""autogenerated by genpy from quadrotor_msgs/Corrections.msg. Do not edit.""" -import sys -python3 = True if sys.hexversion > 0x03000000 else False -import genpy -import struct - - -class Corrections(genpy.Message): - _md5sum = "61e86887a75fe520847d3256306360f5" - _type = "quadrotor_msgs/Corrections" - _has_header = False #flag to mark the presence of a Header object - _full_text = """float64 kf_correction -float64[2] angle_corrections - -""" - __slots__ = ['kf_correction','angle_corrections'] - _slot_types = ['float64','float64[2]'] - - def __init__(self, *args, **kwds): - """ - Constructor. Any message fields that are implicitly/explicitly - set to None will be assigned a default value. The recommend - use is keyword arguments as this is more robust to future message - changes. You cannot mix in-order arguments and keyword arguments. - - The available fields are: - kf_correction,angle_corrections - - :param args: complete set of field values, in .msg order - :param kwds: use keyword arguments corresponding to message field names - to set specific fields. - """ - if args or kwds: - super(Corrections, self).__init__(*args, **kwds) - #message fields cannot be None, assign default values for those that are - if self.kf_correction is None: - self.kf_correction = 0. - if self.angle_corrections is None: - self.angle_corrections = [0.,0.] - else: - self.kf_correction = 0. - self.angle_corrections = [0.,0.] - - def _get_types(self): - """ - internal API method - """ - return self._slot_types - - def serialize(self, buff): - """ - serialize message into buffer - :param buff: buffer, ``StringIO`` - """ - try: - buff.write(_struct_d.pack(self.kf_correction)) - buff.write(_struct_2d.pack(*self.angle_corrections)) - except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x)))) - except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x)))) - - def deserialize(self, str): - """ - unpack serialized message in str into this message instance - :param str: byte array of serialized message, ``str`` - """ - try: - end = 0 - start = end - end += 8 - (self.kf_correction,) = _struct_d.unpack(str[start:end]) - start = end - end += 16 - self.angle_corrections = _struct_2d.unpack(str[start:end]) - return self - except struct.error as e: - raise genpy.DeserializationError(e) #most likely buffer underfill - - - def serialize_numpy(self, buff, numpy): - """ - serialize message with numpy array types into buffer - :param buff: buffer, ``StringIO`` - :param numpy: numpy python module - """ - try: - buff.write(_struct_d.pack(self.kf_correction)) - buff.write(self.angle_corrections.tostring()) - except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x)))) - except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x)))) - - def deserialize_numpy(self, str, numpy): - """ - unpack serialized message in str into this message instance using numpy for array types - :param str: byte array of serialized message, ``str`` - :param numpy: numpy python module - """ - try: - end = 0 - start = end - end += 8 - (self.kf_correction,) = _struct_d.unpack(str[start:end]) - start = end - end += 16 - self.angle_corrections = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=2) - return self - except struct.error as e: - raise genpy.DeserializationError(e) #most likely buffer underfill - -_struct_I = genpy.struct_I -_struct_2d = struct.Struct("<2d") -_struct_d = struct.Struct(" 0x03000000 else False -import genpy -import struct - - -class Gains(genpy.Message): - _md5sum = "b82763b9f24e5595e2a816aa779c9461" - _type = "quadrotor_msgs/Gains" - _has_header = False #flag to mark the presence of a Header object - _full_text = """float64 Kp -float64 Kd -float64 Kp_yaw -float64 Kd_yaw - -""" - __slots__ = ['Kp','Kd','Kp_yaw','Kd_yaw'] - _slot_types = ['float64','float64','float64','float64'] - - def __init__(self, *args, **kwds): - """ - Constructor. Any message fields that are implicitly/explicitly - set to None will be assigned a default value. The recommend - use is keyword arguments as this is more robust to future message - changes. You cannot mix in-order arguments and keyword arguments. - - The available fields are: - Kp,Kd,Kp_yaw,Kd_yaw - - :param args: complete set of field values, in .msg order - :param kwds: use keyword arguments corresponding to message field names - to set specific fields. - """ - if args or kwds: - super(Gains, self).__init__(*args, **kwds) - #message fields cannot be None, assign default values for those that are - if self.Kp is None: - self.Kp = 0. - if self.Kd is None: - self.Kd = 0. - if self.Kp_yaw is None: - self.Kp_yaw = 0. - if self.Kd_yaw is None: - self.Kd_yaw = 0. - else: - self.Kp = 0. - self.Kd = 0. - self.Kp_yaw = 0. - self.Kd_yaw = 0. - - def _get_types(self): - """ - internal API method - """ - return self._slot_types - - def serialize(self, buff): - """ - serialize message into buffer - :param buff: buffer, ``StringIO`` - """ - try: - _x = self - buff.write(_struct_4d.pack(_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw)) - except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x)))) - except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x)))) - - def deserialize(self, str): - """ - unpack serialized message in str into this message instance - :param str: byte array of serialized message, ``str`` - """ - try: - end = 0 - _x = self - start = end - end += 32 - (_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw,) = _struct_4d.unpack(str[start:end]) - return self - except struct.error as e: - raise genpy.DeserializationError(e) #most likely buffer underfill - - - def serialize_numpy(self, buff, numpy): - """ - serialize message with numpy array types into buffer - :param buff: buffer, ``StringIO`` - :param numpy: numpy python module - """ - try: - _x = self - buff.write(_struct_4d.pack(_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw)) - except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x)))) - except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x)))) - - def deserialize_numpy(self, str, numpy): - """ - unpack serialized message in str into this message instance using numpy for array types - :param str: byte array of serialized message, ``str`` - :param numpy: numpy python module - """ - try: - end = 0 - _x = self - start = end - end += 32 - (_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw,) = _struct_4d.unpack(str[start:end]) - return self - except struct.error as e: - raise genpy.DeserializationError(e) #most likely buffer underfill - -_struct_I = genpy.struct_I -_struct_4d = struct.Struct("<4d") diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_OutputData.py b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_OutputData.py deleted file mode 100644 index 357c5f1e..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_OutputData.py +++ /dev/null @@ -1,277 +0,0 @@ -"""autogenerated by genpy from quadrotor_msgs/OutputData.msg. Do not edit.""" -import sys -python3 = True if sys.hexversion > 0x03000000 else False -import genpy -import struct - -import geometry_msgs.msg -import std_msgs.msg - -class OutputData(genpy.Message): - _md5sum = "5759ee97fd5c090dcdccf7fc3e50d024" - _type = "quadrotor_msgs/OutputData" - _has_header = True #flag to mark the presence of a Header object - _full_text = """Header header -uint16 loop_rate -float64 voltage -geometry_msgs/Quaternion orientation -geometry_msgs/Vector3 angular_velocity -geometry_msgs/Vector3 linear_acceleration -float64 pressure_dheight -float64 pressure_height -geometry_msgs/Vector3 magnetic_field -uint8[8] radio_channel -#uint8[4] motor_rpm -uint8 seq - -================================================================================ -MSG: std_msgs/Header -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. -# -# sequence ID: consecutively increasing ID -uint32 seq -#Two-integer timestamp that is expressed as: -# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') -# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') -# time-handling sugar is provided by the client library -time stamp -#Frame this data is associated with -# 0: no frame -# 1: global frame -string frame_id - -================================================================================ -MSG: geometry_msgs/Quaternion -# This represents an orientation in free space in quaternion form. - -float64 x -float64 y -float64 z -float64 w - -================================================================================ -MSG: geometry_msgs/Vector3 -# This represents a vector in free space. - -float64 x -float64 y -float64 z -""" - __slots__ = ['header','loop_rate','voltage','orientation','angular_velocity','linear_acceleration','pressure_dheight','pressure_height','magnetic_field','radio_channel','seq'] - _slot_types = ['std_msgs/Header','uint16','float64','geometry_msgs/Quaternion','geometry_msgs/Vector3','geometry_msgs/Vector3','float64','float64','geometry_msgs/Vector3','uint8[8]','uint8'] - - def __init__(self, *args, **kwds): - """ - Constructor. Any message fields that are implicitly/explicitly - set to None will be assigned a default value. The recommend - use is keyword arguments as this is more robust to future message - changes. You cannot mix in-order arguments and keyword arguments. - - The available fields are: - header,loop_rate,voltage,orientation,angular_velocity,linear_acceleration,pressure_dheight,pressure_height,magnetic_field,radio_channel,seq - - :param args: complete set of field values, in .msg order - :param kwds: use keyword arguments corresponding to message field names - to set specific fields. - """ - if args or kwds: - super(OutputData, self).__init__(*args, **kwds) - #message fields cannot be None, assign default values for those that are - if self.header is None: - self.header = std_msgs.msg.Header() - if self.loop_rate is None: - self.loop_rate = 0 - if self.voltage is None: - self.voltage = 0. - if self.orientation is None: - self.orientation = geometry_msgs.msg.Quaternion() - if self.angular_velocity is None: - self.angular_velocity = geometry_msgs.msg.Vector3() - if self.linear_acceleration is None: - self.linear_acceleration = geometry_msgs.msg.Vector3() - if self.pressure_dheight is None: - self.pressure_dheight = 0. - if self.pressure_height is None: - self.pressure_height = 0. - if self.magnetic_field is None: - self.magnetic_field = geometry_msgs.msg.Vector3() - if self.radio_channel is None: - self.radio_channel = chr(0)*8 - if self.seq is None: - self.seq = 0 - else: - self.header = std_msgs.msg.Header() - self.loop_rate = 0 - self.voltage = 0. - self.orientation = geometry_msgs.msg.Quaternion() - self.angular_velocity = geometry_msgs.msg.Vector3() - self.linear_acceleration = geometry_msgs.msg.Vector3() - self.pressure_dheight = 0. - self.pressure_height = 0. - self.magnetic_field = geometry_msgs.msg.Vector3() - self.radio_channel = chr(0)*8 - self.seq = 0 - - def _get_types(self): - """ - internal API method - """ - return self._slot_types - - def serialize(self, buff): - """ - serialize message into buffer - :param buff: buffer, ``StringIO`` - """ - try: - _x = self - buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) - _x = self.header.frame_id - length = len(_x) - if python3 or type(_x) == unicode: - _x = _x.encode('utf-8') - length = len(_x) - if python3: - buff.write(struct.pack(' 0x03000000 else False -import genpy -import struct - -import std_msgs.msg - -class PPROutputData(genpy.Message): - _md5sum = "732c0e3ca36f241464f8c445e78a0d0a" - _type = "quadrotor_msgs/PPROutputData" - _has_header = True #flag to mark the presence of a Header object - _full_text = """Header header -uint16 quad_time -float64 des_thrust -float64 des_roll -float64 des_pitch -float64 des_yaw -float64 est_roll -float64 est_pitch -float64 est_yaw -float64 est_angvel_x -float64 est_angvel_y -float64 est_angvel_z -float64 est_acc_x -float64 est_acc_y -float64 est_acc_z -uint16[4] pwm - -================================================================================ -MSG: std_msgs/Header -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. -# -# sequence ID: consecutively increasing ID -uint32 seq -#Two-integer timestamp that is expressed as: -# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') -# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') -# time-handling sugar is provided by the client library -time stamp -#Frame this data is associated with -# 0: no frame -# 1: global frame -string frame_id - -""" - __slots__ = ['header','quad_time','des_thrust','des_roll','des_pitch','des_yaw','est_roll','est_pitch','est_yaw','est_angvel_x','est_angvel_y','est_angvel_z','est_acc_x','est_acc_y','est_acc_z','pwm'] - _slot_types = ['std_msgs/Header','uint16','float64','float64','float64','float64','float64','float64','float64','float64','float64','float64','float64','float64','float64','uint16[4]'] - - def __init__(self, *args, **kwds): - """ - Constructor. Any message fields that are implicitly/explicitly - set to None will be assigned a default value. The recommend - use is keyword arguments as this is more robust to future message - changes. You cannot mix in-order arguments and keyword arguments. - - The available fields are: - header,quad_time,des_thrust,des_roll,des_pitch,des_yaw,est_roll,est_pitch,est_yaw,est_angvel_x,est_angvel_y,est_angvel_z,est_acc_x,est_acc_y,est_acc_z,pwm - - :param args: complete set of field values, in .msg order - :param kwds: use keyword arguments corresponding to message field names - to set specific fields. - """ - if args or kwds: - super(PPROutputData, self).__init__(*args, **kwds) - #message fields cannot be None, assign default values for those that are - if self.header is None: - self.header = std_msgs.msg.Header() - if self.quad_time is None: - self.quad_time = 0 - if self.des_thrust is None: - self.des_thrust = 0. - if self.des_roll is None: - self.des_roll = 0. - if self.des_pitch is None: - self.des_pitch = 0. - if self.des_yaw is None: - self.des_yaw = 0. - if self.est_roll is None: - self.est_roll = 0. - if self.est_pitch is None: - self.est_pitch = 0. - if self.est_yaw is None: - self.est_yaw = 0. - if self.est_angvel_x is None: - self.est_angvel_x = 0. - if self.est_angvel_y is None: - self.est_angvel_y = 0. - if self.est_angvel_z is None: - self.est_angvel_z = 0. - if self.est_acc_x is None: - self.est_acc_x = 0. - if self.est_acc_y is None: - self.est_acc_y = 0. - if self.est_acc_z is None: - self.est_acc_z = 0. - if self.pwm is None: - self.pwm = [0,0,0,0] - else: - self.header = std_msgs.msg.Header() - self.quad_time = 0 - self.des_thrust = 0. - self.des_roll = 0. - self.des_pitch = 0. - self.des_yaw = 0. - self.est_roll = 0. - self.est_pitch = 0. - self.est_yaw = 0. - self.est_angvel_x = 0. - self.est_angvel_y = 0. - self.est_angvel_z = 0. - self.est_acc_x = 0. - self.est_acc_y = 0. - self.est_acc_z = 0. - self.pwm = [0,0,0,0] - - def _get_types(self): - """ - internal API method - """ - return self._slot_types - - def serialize(self, buff): - """ - serialize message into buffer - :param buff: buffer, ``StringIO`` - """ - try: - _x = self - buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) - _x = self.header.frame_id - length = len(_x) - if python3 or type(_x) == unicode: - _x = _x.encode('utf-8') - length = len(_x) - if python3: - buff.write(struct.pack(' 0x03000000 else False -import genpy -import struct - -import geometry_msgs.msg -import std_msgs.msg - -class PositionCommand(genpy.Message): - _md5sum = "835935bcd6f18632d9e26a3093237902" - _type = "quadrotor_msgs/PositionCommand" - _has_header = True #flag to mark the presence of a Header object - _full_text = """Header header -geometry_msgs/Point position -geometry_msgs/Vector3 velocity -geometry_msgs/Vector3 acceleration -float64 yaw -float64 yaw_dot -float64[3] kx -float64[3] kv - -================================================================================ -MSG: std_msgs/Header -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. -# -# sequence ID: consecutively increasing ID -uint32 seq -#Two-integer timestamp that is expressed as: -# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') -# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') -# time-handling sugar is provided by the client library -time stamp -#Frame this data is associated with -# 0: no frame -# 1: global frame -string frame_id - -================================================================================ -MSG: geometry_msgs/Point -# This contains the position of a point in free space -float64 x -float64 y -float64 z - -================================================================================ -MSG: geometry_msgs/Vector3 -# This represents a vector in free space. - -float64 x -float64 y -float64 z -""" - __slots__ = ['header','position','velocity','acceleration','yaw','yaw_dot','kx','kv'] - _slot_types = ['std_msgs/Header','geometry_msgs/Point','geometry_msgs/Vector3','geometry_msgs/Vector3','float64','float64','float64[3]','float64[3]'] - - def __init__(self, *args, **kwds): - """ - Constructor. Any message fields that are implicitly/explicitly - set to None will be assigned a default value. The recommend - use is keyword arguments as this is more robust to future message - changes. You cannot mix in-order arguments and keyword arguments. - - The available fields are: - header,position,velocity,acceleration,yaw,yaw_dot,kx,kv - - :param args: complete set of field values, in .msg order - :param kwds: use keyword arguments corresponding to message field names - to set specific fields. - """ - if args or kwds: - super(PositionCommand, self).__init__(*args, **kwds) - #message fields cannot be None, assign default values for those that are - if self.header is None: - self.header = std_msgs.msg.Header() - if self.position is None: - self.position = geometry_msgs.msg.Point() - if self.velocity is None: - self.velocity = geometry_msgs.msg.Vector3() - if self.acceleration is None: - self.acceleration = geometry_msgs.msg.Vector3() - if self.yaw is None: - self.yaw = 0. - if self.yaw_dot is None: - self.yaw_dot = 0. - if self.kx is None: - self.kx = [0.,0.,0.] - if self.kv is None: - self.kv = [0.,0.,0.] - else: - self.header = std_msgs.msg.Header() - self.position = geometry_msgs.msg.Point() - self.velocity = geometry_msgs.msg.Vector3() - self.acceleration = geometry_msgs.msg.Vector3() - self.yaw = 0. - self.yaw_dot = 0. - self.kx = [0.,0.,0.] - self.kv = [0.,0.,0.] - - def _get_types(self): - """ - internal API method - """ - return self._slot_types - - def serialize(self, buff): - """ - serialize message into buffer - :param buff: buffer, ``StringIO`` - """ - try: - _x = self - buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) - _x = self.header.frame_id - length = len(_x) - if python3 or type(_x) == unicode: - _x = _x.encode('utf-8') - length = len(_x) - if python3: - buff.write(struct.pack(' 0x03000000 else False -import genpy -import struct - -import geometry_msgs.msg -import quadrotor_msgs.msg -import std_msgs.msg - -class SO3Command(genpy.Message): - _md5sum = "a466650b2633e768513aa3bf62383c86" - _type = "quadrotor_msgs/SO3Command" - _has_header = True #flag to mark the presence of a Header object - _full_text = """Header header -geometry_msgs/Vector3 force -geometry_msgs/Quaternion orientation -float64[3] kR -float64[3] kOm -quadrotor_msgs/AuxCommand aux - -================================================================================ -MSG: std_msgs/Header -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. -# -# sequence ID: consecutively increasing ID -uint32 seq -#Two-integer timestamp that is expressed as: -# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') -# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') -# time-handling sugar is provided by the client library -time stamp -#Frame this data is associated with -# 0: no frame -# 1: global frame -string frame_id - -================================================================================ -MSG: geometry_msgs/Vector3 -# This represents a vector in free space. - -float64 x -float64 y -float64 z -================================================================================ -MSG: geometry_msgs/Quaternion -# This represents an orientation in free space in quaternion form. - -float64 x -float64 y -float64 z -float64 w - -================================================================================ -MSG: quadrotor_msgs/AuxCommand -float64 current_yaw -float64 kf_correction -float64[2] angle_corrections# Trims for roll, pitch -bool enable_motors -bool use_external_yaw - -""" - __slots__ = ['header','force','orientation','kR','kOm','aux'] - _slot_types = ['std_msgs/Header','geometry_msgs/Vector3','geometry_msgs/Quaternion','float64[3]','float64[3]','quadrotor_msgs/AuxCommand'] - - def __init__(self, *args, **kwds): - """ - Constructor. Any message fields that are implicitly/explicitly - set to None will be assigned a default value. The recommend - use is keyword arguments as this is more robust to future message - changes. You cannot mix in-order arguments and keyword arguments. - - The available fields are: - header,force,orientation,kR,kOm,aux - - :param args: complete set of field values, in .msg order - :param kwds: use keyword arguments corresponding to message field names - to set specific fields. - """ - if args or kwds: - super(SO3Command, self).__init__(*args, **kwds) - #message fields cannot be None, assign default values for those that are - if self.header is None: - self.header = std_msgs.msg.Header() - if self.force is None: - self.force = geometry_msgs.msg.Vector3() - if self.orientation is None: - self.orientation = geometry_msgs.msg.Quaternion() - if self.kR is None: - self.kR = [0.,0.,0.] - if self.kOm is None: - self.kOm = [0.,0.,0.] - if self.aux is None: - self.aux = quadrotor_msgs.msg.AuxCommand() - else: - self.header = std_msgs.msg.Header() - self.force = geometry_msgs.msg.Vector3() - self.orientation = geometry_msgs.msg.Quaternion() - self.kR = [0.,0.,0.] - self.kOm = [0.,0.,0.] - self.aux = quadrotor_msgs.msg.AuxCommand() - - def _get_types(self): - """ - internal API method - """ - return self._slot_types - - def serialize(self, buff): - """ - serialize message into buffer - :param buff: buffer, ``StringIO`` - """ - try: - _x = self - buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) - _x = self.header.frame_id - length = len(_x) - if python3 or type(_x) == unicode: - _x = _x.encode('utf-8') - length = len(_x) - if python3: - buff.write(struct.pack(' 0x03000000 else False -import genpy -import struct - -import std_msgs.msg - -class Serial(genpy.Message): - _md5sum = "e448fb7595af9a8adfcab5ec241c7d4f" - _type = "quadrotor_msgs/Serial" - _has_header = True #flag to mark the presence of a Header object - _full_text = """# Note: These constants need to be kept in sync with the types -# defined in include/quadrotor_msgs/comm_types.h -uint8 SO3_CMD = 115 # 's' in base 10 -uint8 TRPY_CMD = 112 # 'p' in base 10 -uint8 STATUS_DATA = 99 # 'c' in base 10 -uint8 OUTPUT_DATA = 100 # 'd' in base 10 -uint8 PPR_OUTPUT_DATA = 116 # 't' in base 10 -uint8 PPR_GAINS = 103 # 'g' - -Header header -uint8 channel -uint8 type # One of the types listed above -uint8[] data - -================================================================================ -MSG: std_msgs/Header -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. -# -# sequence ID: consecutively increasing ID -uint32 seq -#Two-integer timestamp that is expressed as: -# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') -# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') -# time-handling sugar is provided by the client library -time stamp -#Frame this data is associated with -# 0: no frame -# 1: global frame -string frame_id - -""" - # Pseudo-constants - SO3_CMD = 115 - TRPY_CMD = 112 - STATUS_DATA = 99 - OUTPUT_DATA = 100 - PPR_OUTPUT_DATA = 116 - PPR_GAINS = 103 - - __slots__ = ['header','channel','type','data'] - _slot_types = ['std_msgs/Header','uint8','uint8','uint8[]'] - - def __init__(self, *args, **kwds): - """ - Constructor. Any message fields that are implicitly/explicitly - set to None will be assigned a default value. The recommend - use is keyword arguments as this is more robust to future message - changes. You cannot mix in-order arguments and keyword arguments. - - The available fields are: - header,channel,type,data - - :param args: complete set of field values, in .msg order - :param kwds: use keyword arguments corresponding to message field names - to set specific fields. - """ - if args or kwds: - super(Serial, self).__init__(*args, **kwds) - #message fields cannot be None, assign default values for those that are - if self.header is None: - self.header = std_msgs.msg.Header() - if self.channel is None: - self.channel = 0 - if self.type is None: - self.type = 0 - if self.data is None: - self.data = '' - else: - self.header = std_msgs.msg.Header() - self.channel = 0 - self.type = 0 - self.data = '' - - def _get_types(self): - """ - internal API method - """ - return self._slot_types - - def serialize(self, buff): - """ - serialize message into buffer - :param buff: buffer, ``StringIO`` - """ - try: - _x = self - buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) - _x = self.header.frame_id - length = len(_x) - if python3 or type(_x) == unicode: - _x = _x.encode('utf-8') - length = len(_x) - if python3: - buff.write(struct.pack(' 0x03000000 else False -import genpy -import struct - -import std_msgs.msg - -class StatusData(genpy.Message): - _md5sum = "c70a4ec4725273263ae176ad30f89553" - _type = "quadrotor_msgs/StatusData" - _has_header = True #flag to mark the presence of a Header object - _full_text = """Header header -uint16 loop_rate -float64 voltage -uint8 seq - -================================================================================ -MSG: std_msgs/Header -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. -# -# sequence ID: consecutively increasing ID -uint32 seq -#Two-integer timestamp that is expressed as: -# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') -# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') -# time-handling sugar is provided by the client library -time stamp -#Frame this data is associated with -# 0: no frame -# 1: global frame -string frame_id - -""" - __slots__ = ['header','loop_rate','voltage','seq'] - _slot_types = ['std_msgs/Header','uint16','float64','uint8'] - - def __init__(self, *args, **kwds): - """ - Constructor. Any message fields that are implicitly/explicitly - set to None will be assigned a default value. The recommend - use is keyword arguments as this is more robust to future message - changes. You cannot mix in-order arguments and keyword arguments. - - The available fields are: - header,loop_rate,voltage,seq - - :param args: complete set of field values, in .msg order - :param kwds: use keyword arguments corresponding to message field names - to set specific fields. - """ - if args or kwds: - super(StatusData, self).__init__(*args, **kwds) - #message fields cannot be None, assign default values for those that are - if self.header is None: - self.header = std_msgs.msg.Header() - if self.loop_rate is None: - self.loop_rate = 0 - if self.voltage is None: - self.voltage = 0. - if self.seq is None: - self.seq = 0 - else: - self.header = std_msgs.msg.Header() - self.loop_rate = 0 - self.voltage = 0. - self.seq = 0 - - def _get_types(self): - """ - internal API method - """ - return self._slot_types - - def serialize(self, buff): - """ - serialize message into buffer - :param buff: buffer, ``StringIO`` - """ - try: - _x = self - buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) - _x = self.header.frame_id - length = len(_x) - if python3 or type(_x) == unicode: - _x = _x.encode('utf-8') - length = len(_x) - if python3: - buff.write(struct.pack(' 0x03000000 else False -import genpy -import struct - -import quadrotor_msgs.msg -import std_msgs.msg - -class TRPYCommand(genpy.Message): - _md5sum = "6779ee8a86cc757aeea21725df32d00c" - _type = "quadrotor_msgs/TRPYCommand" - _has_header = True #flag to mark the presence of a Header object - _full_text = """Header header -float32 thrust -float32 roll -float32 pitch -float32 yaw -quadrotor_msgs/AuxCommand aux - -================================================================================ -MSG: std_msgs/Header -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. -# -# sequence ID: consecutively increasing ID -uint32 seq -#Two-integer timestamp that is expressed as: -# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') -# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') -# time-handling sugar is provided by the client library -time stamp -#Frame this data is associated with -# 0: no frame -# 1: global frame -string frame_id - -================================================================================ -MSG: quadrotor_msgs/AuxCommand -float64 current_yaw -float64 kf_correction -float64[2] angle_corrections# Trims for roll, pitch -bool enable_motors -bool use_external_yaw - -""" - __slots__ = ['header','thrust','roll','pitch','yaw','aux'] - _slot_types = ['std_msgs/Header','float32','float32','float32','float32','quadrotor_msgs/AuxCommand'] - - def __init__(self, *args, **kwds): - """ - Constructor. Any message fields that are implicitly/explicitly - set to None will be assigned a default value. The recommend - use is keyword arguments as this is more robust to future message - changes. You cannot mix in-order arguments and keyword arguments. - - The available fields are: - header,thrust,roll,pitch,yaw,aux - - :param args: complete set of field values, in .msg order - :param kwds: use keyword arguments corresponding to message field names - to set specific fields. - """ - if args or kwds: - super(TRPYCommand, self).__init__(*args, **kwds) - #message fields cannot be None, assign default values for those that are - if self.header is None: - self.header = std_msgs.msg.Header() - if self.thrust is None: - self.thrust = 0. - if self.roll is None: - self.roll = 0. - if self.pitch is None: - self.pitch = 0. - if self.yaw is None: - self.yaw = 0. - if self.aux is None: - self.aux = quadrotor_msgs.msg.AuxCommand() - else: - self.header = std_msgs.msg.Header() - self.thrust = 0. - self.roll = 0. - self.pitch = 0. - self.yaw = 0. - self.aux = quadrotor_msgs.msg.AuxCommand() - - def _get_types(self): - """ - internal API method - """ - return self._slot_types - - def serialize(self, buff): - """ - serialize message into buffer - :param buff: buffer, ``StringIO`` - """ - try: - _x = self - buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) - _x = self.header.frame_id - length = len(_x) - if python3 or type(_x) == unicode: - _x = _x.encode('utf-8') - length = len(_x) - if python3: - buff.write(struct.pack(' - Value: true - - Class: rviz/Axes - Enabled: true - Length: 10 - Name: Axes - Radius: 0.01 - Reference Frame: - Value: true - Enabled: true - Name: Basic - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_ukf/robot - Name: Robot UKF - Namespaces: - mesh: true - Queue Size: 100 - Value: true - - Alpha: 1 - Axes Length: 0.5 - Axes Radius: 0.05 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.3 - Head Radius: 0.1 - Name: Pose UKF - Shaft Length: 1 - Shaft Radius: 0.05 - Shape: Axes - Topic: /odom_visualization_ukf/pose - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_ukf/velocity - Name: Velocity UKF - Namespaces: - velocity: true - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 0; 0 - Enabled: true - Line Style: Lines - Line Width: 0.03 - Name: Path UKF - Offset: - X: 0 - Y: 0 - Z: 0 - Topic: /odom_visualization_ukf/path - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /odom_visualization_ukf/covariance - Name: Cov UKF - Namespaces: - {} - Queue Size: 100 - Value: false - Enabled: true - Name: UKF Odom - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_slam/robot - Name: Robot SLAM - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Axes Length: 0.3 - Axes Radius: 0.12 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.3 - Head Radius: 0.1 - Name: Pose SLAM - Shaft Length: 1 - Shaft Radius: 0.05 - Shape: Axes - Topic: /odom_visualization_slam/pose - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_slam/velocity - Name: Velocity SLAM - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 255; 0 - Enabled: true - Line Style: Lines - Line Width: 0.03 - Name: Path SLAM - Offset: - X: 0 - Y: 0 - Z: 0 - Topic: /odom_visualization_slam/path - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /odom_visualization_slam/covariance - Name: Cov SLAM - Namespaces: - {} - Queue Size: 100 - Value: false - Enabled: false - Name: SLAM Odom - - Class: rviz/Group - Displays: - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 0; 0 - Enabled: true - Line Style: Lines - Line Width: 0.03 - Name: Path World - Offset: - X: 0 - Y: 0 - Z: 0 - Topic: /waypoint_generator/waypoints_vis - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 255; 0 - Enabled: true - Line Style: Lines - Line Width: 0.03 - Name: Path Desired - Offset: - X: 0 - Y: 0 - Z: 0 - Topic: /trajectory_generator/path - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 0; 255 - Enabled: true - Line Style: Lines - Line Width: 0.03 - Name: Path Trajectory - Offset: - X: 0 - Y: 0 - Z: 0 - Topic: /trajectory_generator/traj - Value: true - Enabled: true - Name: Trajectory Generator - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud - Color: 255; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Features - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.2 - Style: Spheres - Topic: /visual_pose_estimator/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud - Color: 255; 255; 255 - Color Transformer: RGBF32 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Loc Err Points - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.1 - Style: Spheres - Topic: /visual_pose_estimator/loc_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /visual_pose_estimator/loc_connect - Name: Loc Err Lines - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud - Color: 255; 255; 255 - Color Transformer: RGBF32 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Stereo Dense - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.1 - Style: Boxes - Topic: /visual_slam/cloud_stereo - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud - Color: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Stereo Sparse - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.1 - Style: Spheres - Topic: /visual_slam/points_stereo - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.2 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.8 - Min Value: -0.8 - Value: true - Axis: Z - Channel Name: z - Class: rviz/PointCloud - Color: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 2 - Min Color: 0; 0; 0 - Min Intensity: -0.8 - Name: Map 3D Vision - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.2 - Style: Boxes - Topic: /visual_slam/map - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /visual_slam/pose_graph_odom - Name: SLAM Pose Graph - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /visual_slam/pose_graph_loop - Name: SLAM Loop CLosure - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /visual_slam/pose_graph_loop_tf - Name: SLAM Loop TF - Namespaces: - {} - Queue Size: 100 - Value: false - Enabled: true - Name: Hummingbird - - Class: rviz/Group - Displays: - - Alpha: 0.7 - Class: rviz_plugins/ProbMapDisplay - Draw Behind: false - Enabled: true - Name: Map 2D Laser - Topic: /map3d_visualization/map2d - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud - Color: 255; 255; 255 - Color Transformer: RGBF32 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Map 3D Laser - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.1 - Style: Spheres - Topic: /map3d_visualization/map3d - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 0.60212 - Min Value: -0.161681 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/LaserScan - Color: 255; 0; 4 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 0; 4 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Laser Raw - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.01 - Style: Points - Topic: /laser/scan - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/LaserScan - Color: 255; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 0 - Max Intensity: 4096 - Min Color: 255; 255; 0 - Min Intensity: 0 - Name: Laser - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.01 - Style: Points - Topic: /laser_odom_pelican/lscan - Use Fixed Frame: true - Use rainbow: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud - Color: 255; 170; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Kinect - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.01 - Style: Points - Topic: /kinect/depth/points - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 255; 0 - Enabled: true - Line Style: Lines - Line Width: 0.03 - Name: RRT* Path - Offset: - X: 0 - Y: 0 - Z: 0 - Topic: /rrts/path - Value: true - - Arrow Length: 0.3 - Class: rviz/PoseArray - Color: 255; 255; 0 - Enabled: true - Name: RRT* Pose - Topic: /rrts/path_pose - Value: true - Enabled: true - Name: Pelican - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /monocular_velocity_estimator/plane - Name: Plane - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /monocular_velocity_estimator/normal - Name: Normal - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /monocular_velocity_estimator/velocity - Name: Velocity - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: OpticalFlow - - Class: rviz/Group - Displays: - - Alpha: 0.7 - Class: rviz_plugins/AerialMapDisplay - Draw Behind: true - Enabled: true - Name: AerialMapDisplay - Topic: /aerial_map_publisher/aerial_map - Value: true - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_ukf_laser/robot - Name: Robot Laser - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Axes Length: 0.5 - Axes Radius: 0.05 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.3 - Head Radius: 0.1 - Name: Pose Laser - Shaft Length: 1 - Shaft Radius: 0.05 - Shape: Axes - Topic: /odom_visualization_ukf_laser/pose - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_ukf_laser/velocity - Name: Velocity Laser - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 0; 0 - Enabled: true - Line Style: Lines - Line Width: 0.03 - Name: Path Laser - Offset: - X: 0 - Y: 0 - Z: 0 - Topic: /odom_visualization_ukf_laser/path - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /odom_visualization_ukf_laser/covariance - Name: Cov Laser - Namespaces: - {} - Queue Size: 100 - Value: false - Enabled: false - Name: Laser - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_ukf_vision/robot - Name: Robot Vision - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Axes Length: 0.5 - Axes Radius: 0.05 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.3 - Head Radius: 0.1 - Name: Pose Vision - Shaft Length: 1 - Shaft Radius: 0.05 - Shape: Axes - Topic: /odom_visualization_ukf_vision/pose - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /odom_visualization_ukf_vision/velocity - Name: Velocity Vision - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 255; 0 - Enabled: true - Line Style: Lines - Line Width: 0.03 - Name: Path Vision - Offset: - X: 0 - Y: 0 - Z: 0 - Topic: /odom_visualization_ukf_vision/path - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /odom_visualization_ukf_vision/covariance - Name: Cov Vision - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud - Color: 255; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Stereo Features - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 10 - Size (m): 0.2 - Style: Points - Topic: /stereo_pose_estimator/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Vision - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_ukf_gps/robot - Name: Robot GPS - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Axes Length: 0.5 - Axes Radius: 0.05 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.3 - Head Radius: 0.1 - Name: Pose GPS - Shaft Length: 1 - Shaft Radius: 0.05 - Shape: Axes - Topic: /odom_visualization_ukf_gps/pose - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_ukf_gps/velocity - Name: Velocity GPS - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 255; 0 - Enabled: true - Line Style: Lines - Line Width: 0.03 - Name: Path GPS - Offset: - X: 0 - Y: 0 - Z: 0 - Topic: /odom_visualization_ukf_gps/path - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_ukf_gps/covariance - Name: Cov GPS - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: false - Name: GPS - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_ukf_multi/robot - Name: Robot Multi - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Axes Length: 0.5 - Axes Radius: 0.05 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.3 - Head Radius: 0.1 - Name: Pose Multi - Shaft Length: 1 - Shaft Radius: 0.05 - Shape: Axes - Topic: /odom_visualization_ukf_multi/pose - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_ukf_multi/velocity - Name: Velocity Multi - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 0; 0 - Enabled: true - Line Style: Lines - Line Width: 0.03 - Name: Path Multi - Offset: - X: 0 - Y: 0 - Z: 0 - Topic: /odom_visualization_ukf_multi/path - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /odom_visualization_ukf_multi/trajectory - Name: Traj Multi - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_ukf_multi/covariance - Name: Cov Multi - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /odom_visualization_ukf_multi/covariance_velocity - Name: Cov Vel Multi - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /multi_sensor_slam/graph - Name: SLAM Graph - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz_plugins/MultiProbMapDisplay - Draw Behind: true - Enabled: true - Name: MultiProbMapDisplay - Topic: /multi_map_visualization/maps2d - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.15 - Min Value: -0.35 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Multi3DMap - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.1 - Style: Spheres - Topic: /multi_map_visualization/map3d - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 0.5 - Buffer Length: 1 - Class: rviz/Range - Color: 255; 0; 255 - Enabled: true - Name: Laser Height - Queue Size: 100 - Topic: /odom_visualization_ukf_multi/height - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_ukf_multi/sensor - Name: Sensor Availability - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: Multi - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_vins/robot - Name: Robot VINS - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Axes Length: 0.5 - Axes Radius: 0.05 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.3 - Head Radius: 0.1 - Name: Pose VINS - Shaft Length: 1 - Shaft Radius: 0.05 - Shape: Axes - Topic: /odom_visualization_vins/pose - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_vins/velocity - Name: Velocity VINS - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 0; 0 - Enabled: false - Line Style: Lines - Line Width: 0.03 - Name: Path VINS - Offset: - X: 0 - Y: 0 - Z: 0 - Topic: /odom_visualization_vins/path - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud - Color: 255; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 0; 255; 0 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Cloud VINS - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.1 - Style: Spheres - Topic: /monocular_vins_estimator/cloud - Use Fixed Frame: true - Use rainbow: true - Value: true - - Arrow Length: 0.3 - Class: rviz/PoseArray - Color: 255; 0; 0 - Enabled: true - Name: Poses VINS - Topic: /monocular_vins_estimator/poses - Value: true - Enabled: true - Name: VINS - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_nonlinear/robot - Name: Robot Nonlinear - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Axes Length: 0.5 - Axes Radius: 0.05 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.3 - Head Radius: 0.1 - Name: Pose Nonlinear - Shaft Length: 1 - Shaft Radius: 0.05 - Shape: Axes - Topic: /odom_visualization_nonlinear/pose - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_nonlinear/velocity - Name: Velocity Nonlinear - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 255; 0 - Enabled: true - Line Style: Lines - Line Width: 0.03 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Topic: /odom_visualization_nonlinear/path - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud - Color: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Cloud Nonlinear - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.1 - Style: Spheres - Topic: /monocular_vins_estimator/cloud_nonlinear - Use Fixed Frame: true - Use rainbow: true - Value: true - - Arrow Length: 0.3 - Class: rviz/PoseArray - Color: 0; 255; 0 - Enabled: true - Name: Poses Nonlinear - Topic: /monocular_vins_estimator/poses_nonlinear - Value: true - Enabled: true - Name: Nonlinear - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_imu/robot - Name: Robot IMU - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Axes Length: 0.5 - Axes Radius: 0.05 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: false - Head Length: 0.3 - Head Radius: 0.1 - Name: Pose IMU - Shaft Length: 1 - Shaft Radius: 0.05 - Shape: Axes - Topic: /odom_visualization_imu/pose - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_imu/velocity - Name: Velocity IMU - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 255; 0 - Enabled: false - Line Style: Lines - Line Width: 0.03 - Name: Path IMU - Offset: - X: 0 - Y: 0 - Z: 0 - Topic: /odom_visualization_imu/path - Value: false - Enabled: false - Name: IMU - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_control/robot - Name: Robot Control - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Axes Length: 0.75 - Axes Radius: 0.075 - Class: rviz/Pose - Color: 0; 0; 255 - Enabled: false - Head Length: 0.3 - Head Radius: 0.1 - Name: Pose Control - Shaft Length: 0.5 - Shaft Radius: 0.05 - Shape: Axes - Topic: /odom_visualization_control/pose - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /odom_visualization_control/velocity - Name: Velocity Control - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 0; 255 - Enabled: false - Line Style: Lines - Line Width: 0.03 - Name: Path Control - Offset: - X: 0 - Y: 0 - Z: 0 - Topic: /odom_visualization_control/path - Value: false - Enabled: false - Name: Control - Enabled: true - Name: VINS - Enabled: true - Name: NUC - - Class: rviz/Marker - Enabled: true - Marker Topic: /irobot/irobot_marker - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 61; 61; 61 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/MoveCamera - - Class: rviz_plugins/Goal3DTool - Topic: goal - - Class: rviz/SetInitialPose - Topic: trigger - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 0.0262036 - Enable Stereo Rendering: - Stereo Eye Separation: 0.06 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 4.92202 - Y: -11.2494 - Z: 11.5579 - Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.674797 - Target Frame: - Value: Orbit (rviz) - Yaw: 4.72857 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 744 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000010000000000000190000002a2fc020000000ffb0000000a0049006d0061006700650000000041000001760000000000000000fb000000100044006900730070006c0061007900730100000028000002a2000000dd00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000001c5000000810000006400fffffffb0000000a0049006d00610067006501000001fc0000011d0000000000000000fb0000000a0049006d0061006700650100000154000000e90000000000000000fb0000000a0049006d0061006700650100000211000001080000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000041000004f40000006400fffffffb000000100044006900730070006c0061007900730100000041000002d80000000000000000fb0000000a0049006d00610067006501000001db0000013e0000000000000000fb0000000a0049006d0061006700650100000186000001930000000000000000fb0000000a00560069006500770073000000017c0000019d000000b000fffffffb0000000a0049006d00610067006501000001da0000013f0000000000000000fb0000000a0049006d006100670065010000027d0000009c0000000000000000fb0000000a0049006d00610067006501000001d2000001470000000000000000fb0000000a0049006d00610067006501000001af0000016a000000000000000000000383000002a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1305 - X: 61 - Y: 24 diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/package.xml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/package.xml deleted file mode 100644 index e025d755..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - 0.0.0 - rviz_plugins - - - Additional plugins for RViz - - - Shaojie Shen - BSD - http://ros.org/wiki/rviz_plugins - - catkin - - rviz - roscpp - - qtbase5-dev - - rviz - - roscpp - libqt5-core - libqt5-gui - libqt5-widgets - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/plugin_description.xml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/plugin_description.xml deleted file mode 100644 index e7cd4dc7..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/plugin_description.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - Tool for setting 3D goal - - - - - Display of -inf +inf probabilistic occupancy grid map - - - - - Display of aerial map - - - - - Display of multiple -inf +inf probabilistic occupancy grid map - - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.cpp deleted file mode 100644 index 4d5ab223..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.cpp +++ /dev/null @@ -1,538 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -#include -#include -#include -#include -#include - -#include - -#include - -#include "rviz/frame_manager.h" -#include "rviz/ogre_helpers/grid.h" -#include "rviz/properties/float_property.h" -#include "rviz/properties/int_property.h" -#include "rviz/properties/property.h" -#include "rviz/properties/quaternion_property.h" -#include "rviz/properties/ros_topic_property.h" -#include "rviz/properties/vector_property.h" -#include "rviz/validate_floats.h" -#include "rviz/display_context.h" - -#include "aerialmap_display.h" - -namespace rviz -{ - -AerialMapDisplay::AerialMapDisplay() - : Display() - , manual_object_( NULL ) - //, material_( 0 ) - , loaded_( false ) - , resolution_( 0.0f ) - , width_( 0 ) - , height_( 0 ) - , position_(Ogre::Vector3::ZERO) - , orientation_(Ogre::Quaternion::IDENTITY) - , new_map_(false) -{ - topic_property_ = new RosTopicProperty( "Topic", "", - QString::fromStdString( ros::message_traits::datatype() ), - "nav_msgs::OccupancyGrid topic to subscribe to.", - this, SLOT( updateTopic() )); - - alpha_property_ = new FloatProperty( "Alpha", 0.7, - "Amount of transparency to apply to the map.", - this, SLOT( updateAlpha() )); - alpha_property_->setMin( 0 ); - alpha_property_->setMax( 1 ); - - draw_under_property_ = new Property( "Draw Behind", false, - "Rendering option, controls whether or not the map is always" - " drawn behind everything else.", - this, SLOT( updateDrawUnder() )); - - resolution_property_ = new FloatProperty( "Resolution", 0, - "Resolution of the map. (not editable)", this ); - resolution_property_->setReadOnly( true ); - - width_property_ = new IntProperty( "Width", 0, - "Width of the map, in meters. (not editable)", this ); - width_property_->setReadOnly( true ); - - height_property_ = new IntProperty( "Height", 0, - "Height of the map, in meters. (not editable)", this ); - height_property_->setReadOnly( true ); - - position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO, - "Position of the bottom left corner of the map, in meters. (not editable)", - this ); - position_property_->setReadOnly( true ); - - orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY, - "Orientation of the map. (not editable)", - this ); - orientation_property_->setReadOnly( true ); -} - -AerialMapDisplay::~AerialMapDisplay() -{ - unsubscribe(); - clear(); -} - -void AerialMapDisplay::onInitialize() -{ - static int count = 0; - std::stringstream ss; - ss << "AerialMapObjectMaterial" << count++; - material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME ); - material_->setReceiveShadows(false); - material_->getTechnique(0)->setLightingEnabled(false); - material_->setDepthBias( -16.0f, 0.0f ); - material_->setCullingMode( Ogre::CULL_NONE ); - material_->setDepthWriteEnabled(false); - - updateAlpha(); -} - -void AerialMapDisplay::onEnable() -{ - subscribe(); -} - -void AerialMapDisplay::onDisable() -{ - unsubscribe(); - clear(); -} - -void AerialMapDisplay::subscribe() -{ - if ( !isEnabled() ) - { - return; - } - - if( !topic_property_->getTopic().isEmpty() ) - { - try - { - map_sub_ = update_nh_.subscribe( topic_property_->getTopicStd(), 1, &AerialMapDisplay::incomingAerialMap, this ); - setStatus( StatusProperty::Ok, "Topic", "OK" ); - } - catch( ros::Exception& e ) - { - setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() ); - } - } -} - -void AerialMapDisplay::unsubscribe() -{ - map_sub_.shutdown(); -} - -void AerialMapDisplay::updateAlpha() -{ - float alpha = alpha_property_->getFloat(); - - Ogre::Pass* pass = material_->getTechnique( 0 )->getPass( 0 ); - Ogre::TextureUnitState* tex_unit = NULL; - if( pass->getNumTextureUnitStates() > 0 ) - { - tex_unit = pass->getTextureUnitState( 0 ); - } - else - { - tex_unit = pass->createTextureUnitState(); - } - - tex_unit->setAlphaOperation( Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, alpha ); - - if( alpha < 0.9998 ) - { - material_->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA ); - material_->setDepthWriteEnabled( false ); - } - else - { - material_->setSceneBlending( Ogre::SBT_REPLACE ); - material_->setDepthWriteEnabled( !draw_under_property_->getValue().toBool() ); - } -} - -void AerialMapDisplay::updateDrawUnder() -{ - bool draw_under = draw_under_property_->getValue().toBool(); - - if( alpha_property_->getFloat() >= 0.9998 ) - { - material_->setDepthWriteEnabled( !draw_under ); - } - - if( manual_object_ ) - { - if( draw_under ) - { - manual_object_->setRenderQueueGroup( Ogre::RENDER_QUEUE_4 ); - } - else - { - manual_object_->setRenderQueueGroup( Ogre::RENDER_QUEUE_MAIN ); - } - } -} - -void AerialMapDisplay::updateTopic() -{ - unsubscribe(); - subscribe(); - clear(); -} - -void AerialMapDisplay::clear() -{ - setStatus( StatusProperty::Warn, "Message", "No map received" ); - - if( !loaded_ ) - { - return; - } - - scene_manager_->destroyManualObject( manual_object_ ); - manual_object_ = NULL; - - std::string tex_name = texture_->getName(); - texture_.setNull(); - Ogre::TextureManager::getSingleton().unload( tex_name ); - - loaded_ = false; -} -/* -bool validateFloats(const nav_msgs::OccupancyGrid& msg) -{ - bool valid = true; - valid = valid && validateFloats( msg.info.resolution ); - valid = valid && validateFloats( msg.info.origin ); - return valid; -} -*/ -void AerialMapDisplay::update( float wall_dt, float ros_dt ) -{ - { - boost::mutex::scoped_lock lock(mutex_); - - current_map_ = updated_map_; - } - - if (!current_map_ || !new_map_) - { - return; - } - - if (current_map_->data.empty()) - { - return; - } - - new_map_ = false; -/* - if( !validateFloats( *current_map_ )) - { - setStatus( StatusProperty::Error, "Map", "Message contained invalid floating point values (nans or infs)" ); - return; - } -*/ - if( current_map_->info.width * current_map_->info.height == 0 ) - { - std::stringstream ss; - ss << "AerialMap is zero-sized (" << current_map_->info.width << "x" << current_map_->info.height << ")"; - setStatus( StatusProperty::Error, "AerialMap", QString::fromStdString( ss.str() )); - return; - } - - clear(); - - setStatus( StatusProperty::Ok, "Message", "AerialMap received" ); - - ROS_DEBUG( "Received a %d X %d map @ %.3f m/pix\n", - current_map_->info.width, - current_map_->info.height, - current_map_->info.resolution ); - - float resolution = current_map_->info.resolution; - - int width = current_map_->info.width; - int height = current_map_->info.height; - - - Ogre::Vector3 position( current_map_->info.origin.position.x, - current_map_->info.origin.position.y, - current_map_->info.origin.position.z ); - Ogre::Quaternion orientation( current_map_->info.origin.orientation.w, - current_map_->info.origin.orientation.x, - current_map_->info.origin.orientation.y, - current_map_->info.origin.orientation.z ); - frame_ = current_map_->header.frame_id; - if (frame_.empty()) - { - frame_ = "/map"; - } - - // Expand it to be RGB data - unsigned int pixels_size = width * height * 3; - unsigned char* pixels = new unsigned char[pixels_size]; - memset(pixels, 255, pixels_size); - - bool map_status_set = false; - unsigned int num_pixels_to_copy = pixels_size; - if( pixels_size != current_map_->data.size() ) - { - std::stringstream ss; - ss << "Data size doesn't match width*height: width = " << width - << ", height = " << height << ", data size = " << current_map_->data.size(); - setStatus( StatusProperty::Error, "AerialMap", QString::fromStdString( ss.str() )); - map_status_set = true; - - // Keep going, but don't read past the end of the data. - if( current_map_->data.size() < pixels_size ) - { - num_pixels_to_copy = current_map_->data.size(); - } - } - - // TODO: a fragment shader could do this on the video card, and - // would allow a non-grayscale color to mark the out-of-range - // values. - for( unsigned int pixel_index = 0; pixel_index < num_pixels_to_copy; pixel_index++ ) - { -/* - unsigned char val; - int8_t data = current_map_->data[ pixel_index ]; - if(data > 0) - val = 0; - else if(data < 0) - val = 255; - else - val = 127; - pixels[ pixel_index ] = val; -*/ - pixels[ pixel_index ] = current_map_->data[ pixel_index ]; - } - - Ogre::DataStreamPtr pixel_stream; - pixel_stream.bind( new Ogre::MemoryDataStream( pixels, pixels_size )); - static int tex_count = 0; - std::stringstream ss; - ss << "AerialMapTexture" << tex_count++; - try - { - texture_ = Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, - pixel_stream, width, height, Ogre::PF_R8G8B8, Ogre::TEX_TYPE_2D, - 0); - - if( !map_status_set ) - { - setStatus( StatusProperty::Ok, "AerialMap", "AerialMap OK" ); - } - } - catch(Ogre::RenderingAPIException&) - { - Ogre::Image image; - pixel_stream->seek(0); - float fwidth = width; - float fheight = height; - if( width > height ) - { - float aspect = fheight / fwidth; - fwidth = 2048; - fheight = fwidth * aspect; - } - else - { - float aspect = fwidth / fheight; - fheight = 2048; - fwidth = fheight * aspect; - } - - { - std::stringstream ss; - ss << "AerialMap is larger than your graphics card supports. Downsampled from [" << width << "x" << height << "] to [" << fwidth << "x" << fheight << "]"; - setStatus(StatusProperty::Ok, "AerialMap", QString::fromStdString( ss.str() )); - } - - ROS_WARN("Failed to create full-size map texture, likely because your graphics card does not support textures of size > 2048. Downsampling to [%d x %d]...", (int)fwidth, (int)fheight); - //ROS_INFO("Stream size [%d], width [%f], height [%f], w * h [%f]", pixel_stream->size(), width, height, width * height); - image.loadRawData(pixel_stream, width, height, Ogre::PF_R8G8B8); - image.resize(fwidth, fheight, Ogre::Image::FILTER_NEAREST); - ss << "Downsampled"; - texture_ = Ogre::TextureManager::getSingleton().loadImage(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image); - } - - delete [] pixels; - - Ogre::Pass* pass = material_->getTechnique(0)->getPass(0); - Ogre::TextureUnitState* tex_unit = NULL; - if (pass->getNumTextureUnitStates() > 0) - { - tex_unit = pass->getTextureUnitState(0); - } - else - { - tex_unit = pass->createTextureUnitState(); - } - - tex_unit->setTextureName(texture_->getName()); - tex_unit->setTextureFiltering( Ogre::TFO_NONE ); - - static int map_count = 0; - std::stringstream ss2; - ss2 << "AerialMapObject" << map_count++; - manual_object_ = scene_manager_->createManualObject( ss2.str() ); - scene_node_->attachObject( manual_object_ ); - - manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST); - { - // First triangle - { - // Bottom left - manual_object_->position( 0.0f, 0.0f, 0.0f ); - manual_object_->textureCoord(0.0f, 0.0f); - manual_object_->normal( 0.0f, 0.0f, 1.0f ); - - // Top right - manual_object_->position( resolution*width, resolution*height, 0.0f ); - manual_object_->textureCoord(1.0f, 1.0f); - manual_object_->normal( 0.0f, 0.0f, 1.0f ); - - // Top left - manual_object_->position( 0.0f, resolution*height, 0.0f ); - manual_object_->textureCoord(0.0f, 1.0f); - manual_object_->normal( 0.0f, 0.0f, 1.0f ); - } - - // Second triangle - { - // Bottom left - manual_object_->position( 0.0f, 0.0f, 0.0f ); - manual_object_->textureCoord(0.0f, 0.0f); - manual_object_->normal( 0.0f, 0.0f, 1.0f ); - - // Bottom right - manual_object_->position( resolution*width, 0.0f, 0.0f ); - manual_object_->textureCoord(1.0f, 0.0f); - manual_object_->normal( 0.0f, 0.0f, 1.0f ); - - // Top right - manual_object_->position( resolution*width, resolution*height, 0.0f ); - manual_object_->textureCoord(1.0f, 1.0f); - manual_object_->normal( 0.0f, 0.0f, 1.0f ); - } - } - manual_object_->end(); - - if( draw_under_property_->getValue().toBool() ) - { - manual_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_4); - } - - resolution_property_->setValue( resolution ); - width_property_->setValue( width ); - height_property_->setValue( height ); - position_property_->setVector( position ); - orientation_property_->setQuaternion( orientation ); - - transformAerialMap(); - - loaded_ = true; - - context_->queueRender(); -} - -void AerialMapDisplay::incomingAerialMap(const nav_msgs::OccupancyGrid::ConstPtr& msg) -{ - - updated_map_ = msg; - boost::mutex::scoped_lock lock(mutex_); - new_map_ = true; -} - - - -void AerialMapDisplay::transformAerialMap() -{ - if (!current_map_) - { - return; - } - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if (!context_->getFrameManager()->transform(frame_, ros::Time(), current_map_->info.origin, position, orientation)) - { - ROS_DEBUG( "Error transforming map '%s' from frame '%s' to frame '%s'", - qPrintable( getName() ), frame_.c_str(), qPrintable( fixed_frame_ )); - - setStatus( StatusProperty::Error, "Transform", - "No transform from [" + QString::fromStdString( frame_ ) + "] to [" + fixed_frame_ + "]" ); - } - else - { - setStatus(StatusProperty::Ok, "Transform", "Transform OK"); - } - - scene_node_->setPosition( position ); - scene_node_->setOrientation( orientation ); -} - -void AerialMapDisplay::fixedFrameChanged() -{ - transformAerialMap(); -} - -void AerialMapDisplay::reset() -{ - Display::reset(); - - clear(); - // Force resubscription so that the map will be re-sent - updateTopic(); -} - -} // namespace rviz - -#include -PLUGINLIB_EXPORT_CLASS( rviz::AerialMapDisplay, rviz::Display ) diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.h deleted file mode 100644 index 72504f12..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.h +++ /dev/null @@ -1,134 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef AERIAL_MAP_DISPLAY_H -#define AERIAL_MAP_DISPLAY_H - -#include -#include -#include - -#include -#include - -#include - -#include "rviz/display.h" - -namespace Ogre -{ -class ManualObject; -} - -namespace rviz -{ - -class FloatProperty; -class IntProperty; -class Property; -class QuaternionProperty; -class RosTopicProperty; -class VectorProperty; - -/** - * \class AerialMapDisplay - * \brief Displays a map along the XY plane. - */ -class AerialMapDisplay: public Display -{ -Q_OBJECT -public: - AerialMapDisplay(); - virtual ~AerialMapDisplay(); - - // Overrides from Display - virtual void onInitialize(); - virtual void fixedFrameChanged(); - virtual void reset(); - virtual void update( float wall_dt, float ros_dt ); - - float getResolution() { return resolution_; } - int getWidth() { return width_; } - int getHeight() { return height_; } - Ogre::Vector3 getPosition() { return position_; } - Ogre::Quaternion getOrientation() { return orientation_; } - -protected Q_SLOTS: - void updateAlpha(); - void updateTopic(); - void updateDrawUnder(); - - -protected: - // overrides from Display - virtual void onEnable(); - virtual void onDisable(); - - virtual void subscribe(); - virtual void unsubscribe(); - - void incomingAerialMap(const nav_msgs::OccupancyGrid::ConstPtr& msg); - - void clear(); - - void transformAerialMap(); - - Ogre::ManualObject* manual_object_; - Ogre::TexturePtr texture_; - Ogre::MaterialPtr material_; - bool loaded_; - - std::string topic_; - float resolution_; - int width_; - int height_; - Ogre::Vector3 position_; - Ogre::Quaternion orientation_; - std::string frame_; - - ros::Subscriber map_sub_; - - RosTopicProperty* topic_property_; - FloatProperty* resolution_property_; - IntProperty* width_property_; - IntProperty* height_property_; - VectorProperty* position_property_; - QuaternionProperty* orientation_property_; - FloatProperty* alpha_property_; - Property* draw_under_property_; - - nav_msgs::OccupancyGrid::ConstPtr updated_map_; - nav_msgs::OccupancyGrid::ConstPtr current_map_; - boost::mutex mutex_; - bool new_map_; -}; - -} // namespace rviz - - #endif diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/goal_tool.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/goal_tool.cpp deleted file mode 100644 index a5676585..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/goal_tool.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -#include - -#include "rviz/display_context.h" -#include "rviz/properties/string_property.h" - -#include "goal_tool.h" - -namespace rviz -{ - -Goal3DTool::Goal3DTool() -{ - shortcut_key_ = 'g'; - - topic_property_ = new StringProperty( "Topic", "goal", - "The topic on which to publish navigation goals.", - getPropertyContainer(), SLOT( updateTopic() ), this ); -} - -void Goal3DTool::onInitialize() -{ - Pose3DTool::onInitialize(); - setName( "3D Nav Goal" ); - updateTopic(); -} - -void Goal3DTool::updateTopic() -{ - pub_ = nh_.advertise( topic_property_->getStdString(), 1 ); -} - -void Goal3DTool::onPoseSet(double x, double y, double z, double theta) -{ - ROS_WARN("3D Goal Set"); - std::string fixed_frame = context_->getFixedFrame().toStdString(); - tf::Quaternion quat; - quat.setRPY(0.0, 0.0, theta); - tf::Stamped p = tf::Stamped(tf::Pose(quat, tf::Point(x, y, z)), ros::Time::now(), fixed_frame); - geometry_msgs::PoseStamped goal; - tf::poseStampedTFToMsg(p, goal); - ROS_INFO("Setting goal: Frame:%s, Position(%.3f, %.3f, %.3f), Orientation(%.3f, %.3f, %.3f, %.3f) = Angle: %.3f\n", fixed_frame.c_str(), - goal.pose.position.x, goal.pose.position.y, goal.pose.position.z, - goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w, theta); - pub_.publish(goal); -} - -} // end namespace rviz - -#include -PLUGINLIB_EXPORT_CLASS( rviz::Goal3DTool, rviz::Tool ) diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/goal_tool.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/goal_tool.h deleted file mode 100644 index dd69e07f..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/goal_tool.h +++ /dev/null @@ -1,72 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef RVIZ_GOAL_TOOL_H -#define RVIZ_GOAL_TOOL_H - -#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 -# include - -# include - -# include "pose_tool.h" -#endif - -namespace rviz -{ -class Arrow; -class DisplayContext; -class StringProperty; - -class Goal3DTool: public Pose3DTool -{ -Q_OBJECT -public: - Goal3DTool(); - virtual ~Goal3DTool() {} - virtual void onInitialize(); - -protected: - virtual void onPoseSet(double x, double y, double z, double theta); - -private Q_SLOTS: - void updateTopic(); - -private: - ros::NodeHandle nh_; - ros::Publisher pub_; - - StringProperty* topic_property_; -}; - -} - -#endif - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.cpp deleted file mode 100644 index 131b1183..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.cpp +++ /dev/null @@ -1,365 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -#include -#include -#include -#include -#include - -#include - -#include - -#include "rviz/frame_manager.h" -#include "rviz/ogre_helpers/grid.h" -#include "rviz/properties/float_property.h" -#include "rviz/properties/int_property.h" -#include "rviz/properties/property.h" -#include "rviz/properties/quaternion_property.h" -#include "rviz/properties/ros_topic_property.h" -#include "rviz/properties/vector_property.h" -#include "rviz/validate_floats.h" -#include "rviz/display_context.h" - -#include "multi_probmap_display.h" - -namespace rviz -{ - -MultiProbMapDisplay::MultiProbMapDisplay() - : Display() - , loaded_( false ) - , new_map_(false) -{ - topic_property_ = new RosTopicProperty( "Topic", "", - QString::fromStdString( ros::message_traits::datatype() ), - "multi_map_server::MultiOccupancyGrid topic to subscribe to.", - this, SLOT( updateTopic() )); - - draw_under_property_ = new Property( "Draw Behind", false, - "Rendering option, controls whether or not the map is always" - " drawn behind everything else.", - this, SLOT( updateDrawUnder() )); -} - -MultiProbMapDisplay::~MultiProbMapDisplay() -{ - unsubscribe(); - clear(); -} - -void MultiProbMapDisplay::onInitialize() -{ -} - -void MultiProbMapDisplay::onEnable() -{ - subscribe(); -} - -void MultiProbMapDisplay::onDisable() -{ - unsubscribe(); - clear(); -} - -void MultiProbMapDisplay::subscribe() -{ - if ( !isEnabled() ) - { - return; - } - - if( !topic_property_->getTopic().isEmpty() ) - { - try - { - map_sub_ = update_nh_.subscribe( topic_property_->getTopicStd(), 1, &MultiProbMapDisplay::incomingMap, this ); - setStatus( StatusProperty::Ok, "Topic", "OK" ); - } - catch( ros::Exception& e ) - { - setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() ); - } - } -} - -void MultiProbMapDisplay::unsubscribe() -{ - map_sub_.shutdown(); -} - -void MultiProbMapDisplay::updateDrawUnder() -{ - bool draw_under = draw_under_property_->getValue().toBool(); - - for (unsigned int k = 0; k < material_.size(); k++) - material_[k]->setDepthWriteEnabled( !draw_under ); - - for (unsigned int k = 0; k < manual_object_.size(); k++) - { - if( draw_under ) - manual_object_[k]->setRenderQueueGroup( Ogre::RENDER_QUEUE_4 ); - else - manual_object_[k]->setRenderQueueGroup( Ogre::RENDER_QUEUE_MAIN ); - } -} - -void MultiProbMapDisplay::updateTopic() -{ - unsubscribe(); - subscribe(); - clear(); -} - -void MultiProbMapDisplay::clear() -{ - setStatus( StatusProperty::Warn, "Message", "No map received" ); - - if( !loaded_ ) - { - return; - } - - for (unsigned k = 0; k < manual_object_.size(); k++) - { - scene_manager_->destroyManualObject( manual_object_[k] ); - std::string tex_name = texture_[k]->getName(); - texture_[k].setNull(); - Ogre::TextureManager::getSingleton().unload( tex_name ); - } - manual_object_.clear(); - texture_.clear(); - material_.clear(); - - loaded_ = false; -} - -// *********************************************************************************************************************************** - -void MultiProbMapDisplay::update( float wall_dt, float ros_dt ) -{ - { - boost::mutex::scoped_lock lock(mutex_); - current_map_ = updated_map_; - } - - if (!new_map_) - return; - new_map_ = false; - - clear(); - - //ros::Time t[5]; - //double dt[4] = {0,0,0,0}; - for (unsigned int k = 0; k < current_map_->maps.size(); k++) - { - if (current_map_->maps[k].data.empty()) - continue; - setStatus( StatusProperty::Ok, "Message", "Map received" ); - - // Map info - float resolution = current_map_->maps[k].info.resolution; - int width = current_map_->maps[k].info.width; - int height = current_map_->maps[k].info.height; - - // Load pixel - //t[0] = ros::Time::now(); - unsigned int pixels_size = width * height; - unsigned char* pixels = new unsigned char[pixels_size]; - memset(pixels, 255, pixels_size); - unsigned int num_pixels_to_copy = pixels_size; - if( pixels_size != current_map_->maps[k].data.size() ) - if( current_map_->maps[k].data.size() < pixels_size ) - num_pixels_to_copy = current_map_->maps[k].data.size(); - for( unsigned int pixel_index = 0; pixel_index < num_pixels_to_copy; pixel_index++ ) - { - unsigned char val; - int8_t data = current_map_->maps[k].data[ pixel_index ]; - if(data > 0) - val = 255; - else if(data < 0) - val = 180; - else - val = 0; - pixels[ pixel_index ] = val; - } -/* - int pixels_size = current_map_->maps[k].data.size(); - unsigned char* pixels = new unsigned char[pixels_size]; - memcpy(pixels, ¤t_map_->maps[k].data[0], pixels_size); -*/ - // Set texture - //t[1] = ros::Time::now(); - Ogre::DataStreamPtr pixel_stream; - pixel_stream.bind( new Ogre::MemoryDataStream( pixels, pixels_size )); - static int tex_count = 0; - std::stringstream ss1; - ss1 << "MultiMapTexture" << tex_count++; - Ogre::TexturePtr _texture_; - //t[2] = ros::Time::now(); - _texture_ = Ogre::TextureManager::getSingleton().loadRawData( ss1.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, - pixel_stream, width, height, Ogre::PF_L8, Ogre::TEX_TYPE_2D, 0); - //t[3] = ros::Time::now(); - texture_.push_back(_texture_); - delete [] pixels; - setStatus( StatusProperty::Ok, "Map", "Map OK" ); - //t[4] = ros::Time::now(); - - // Set material - static int material_count = 0; - std::stringstream ss0; - ss0 << "MultiMapObjectMaterial" << material_count++; - Ogre::MaterialPtr _material_; - _material_ = Ogre::MaterialManager::getSingleton().create( ss0.str(), - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME ); - _material_->setReceiveShadows(false); - _material_->getTechnique(0)->setLightingEnabled(false); - _material_->setDepthBias( -16.0f, 0.0f ); - _material_->setCullingMode( Ogre::CULL_NONE ); - _material_->setDepthWriteEnabled(false); - material_.push_back(_material_); - material_.back()->setSceneBlending( Ogre::SBT_TRANSPARENT_COLOUR ); - material_.back()->setDepthWriteEnabled( false ); - Ogre::Pass* pass = material_.back()->getTechnique(0)->getPass(0); - Ogre::TextureUnitState* tex_unit = NULL; - if (pass->getNumTextureUnitStates() > 0) - tex_unit = pass->getTextureUnitState(0); - else - tex_unit = pass->createTextureUnitState(); - tex_unit->setTextureName(texture_.back()->getName()); - tex_unit->setTextureFiltering( Ogre::TFO_NONE ); - - // Set manual object - static int map_count = 0; - std::stringstream ss2; - ss2 << "MultiMapObject" << map_count++; - Ogre::ManualObject* _manual_object_ = scene_manager_->createManualObject( ss2.str() ); - manual_object_.push_back(_manual_object_); - scene_node_->attachObject( manual_object_.back() ); - float yo = tf::getYaw(current_map_->origins[k].orientation); - float co = cos(yo); - float so = sin(yo); - float dxo = current_map_->origins[k].position.x; - float dyo = current_map_->origins[k].position.y; - float ym = tf::getYaw(current_map_->maps[k].info.origin.orientation); - float dxm = current_map_->maps[k].info.origin.position.x; - float dym = current_map_->maps[k].info.origin.position.y; - float yaw = yo + ym; - float c = cos(yaw); - float s = sin(yaw); - float dx = co * dxm - so * dym + dxo; - float dy = so * dxm + co * dym + dyo; - float x = 0.0; - float y = 0.0; - manual_object_.back()->begin(material_.back()->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST); - { - // First triangle - { - // Bottom left - x = c * 0.0 - s * 0.0 + dx; - y = s * 0.0 + c * 0.0 + dy; - manual_object_.back()->position( x, y, 0.0f ); - manual_object_.back()->textureCoord(0.0f, 0.0f); - manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); - - // Top right - x = c * resolution*width - s * resolution*height + dx; - y = s * resolution*width + c * resolution*height + dy; - manual_object_.back()->position( x, y, 0.0f ); - manual_object_.back()->textureCoord(1.0f, 1.0f); - manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); - - // Top left - x = c * 0.0 - s * resolution*height + dx; - y = s * 0.0 + c * resolution*height + dy; - manual_object_.back()->position( x, y, 0.0f ); - manual_object_.back()->textureCoord(0.0f, 1.0f); - manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); - } - - // Second triangle - { - // Bottom left - x = c * 0.0 - s * 0.0 + dx; - y = s * 0.0 + c * 0.0 + dy; - manual_object_.back()->position( x, y, 0.0f ); - manual_object_.back()->textureCoord(0.0f, 0.0f); - manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); - - // Bottom right - x = c * resolution*width - s * 0.0 + dx; - y = s * resolution*width + c * 0.0 + dy; - manual_object_.back()->position( x, y, 0.0f ); - manual_object_.back()->textureCoord(1.0f, 0.0f); - manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); - - // Top right - x = c * resolution*width - s * resolution*height + dx; - y = s * resolution*width + c * resolution*height + dy; - manual_object_.back()->position( x, y, 0.0f ); - manual_object_.back()->textureCoord(1.0f, 1.0f); - manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); - } - } - manual_object_.back()->end(); - if( draw_under_property_->getValue().toBool() ) - manual_object_.back()->setRenderQueueGroup(Ogre::RENDER_QUEUE_4); - - //for (int i = 0; i < 4; i++) - // dt[i] += (t[i+1] - t[i]).toSec(); - } - loaded_ = true; - context_->queueRender(); - //ROS_ERROR("RVIZ MAP: %f %f %f %f", dt[0],dt[1],dt[2],dt[3]); -} - -// *********************************************************************************************************************************** - -void MultiProbMapDisplay::incomingMap(const multi_map_server::MultiOccupancyGrid::ConstPtr& msg) -{ - updated_map_ = msg; - boost::mutex::scoped_lock lock(mutex_); - new_map_ = true; -} - -void MultiProbMapDisplay::reset() -{ - Display::reset(); - clear(); - updateTopic(); -} - -} // namespace rviz - -#include -PLUGINLIB_EXPORT_CLASS( rviz::MultiProbMapDisplay, rviz::Display ) diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.h deleted file mode 100644 index ee64fc5c..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.h +++ /dev/null @@ -1,114 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef MULTI_PROB_MAP_DISPLAY_H -#define MULTI_PROB_MAP_DISPLAY_H - -#include -#include -#include - -#include -#include - -#include -#include - -#include "rviz/display.h" - -namespace Ogre -{ -class ManualObject; -} - -namespace rviz -{ - -class FloatProperty; -class IntProperty; -class Property; -class QuaternionProperty; -class RosTopicProperty; -class VectorProperty; - -/** - * \class MultiProbMapDisplay - * \brief Displays a map along the XY plane. - */ -class MultiProbMapDisplay: public Display -{ -Q_OBJECT -public: - MultiProbMapDisplay(); - virtual ~MultiProbMapDisplay(); - - // Overrides from Display - virtual void onInitialize(); - virtual void reset(); - virtual void update( float wall_dt, float ros_dt ); - -protected Q_SLOTS: - void updateTopic(); - void updateDrawUnder(); - - -protected: - // overrides from Display - virtual void onEnable(); - virtual void onDisable(); - - virtual void subscribe(); - virtual void unsubscribe(); - - void incomingMap(const multi_map_server::MultiOccupancyGrid::ConstPtr& msg); - - void clear(); - - std::vector manual_object_; - std::vector texture_; - std::vector material_; - - bool loaded_; - - std::string topic_; - - ros::Subscriber map_sub_; - - RosTopicProperty* topic_property_; - Property* draw_under_property_; - - multi_map_server::MultiOccupancyGrid::ConstPtr updated_map_; - multi_map_server::MultiOccupancyGrid::ConstPtr current_map_; - boost::mutex mutex_; - bool new_map_; -}; - -} // namespace rviz - - #endif diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/pose_tool.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/pose_tool.cpp deleted file mode 100644 index 8249d1e1..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/pose_tool.cpp +++ /dev/null @@ -1,167 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include - -#include "rviz/geometry.h" -#include "rviz/ogre_helpers/arrow.h" -#include "rviz/viewport_mouse_event.h" -#include "rviz/load_resource.h" -#include "rviz/render_panel.h" - -#include "pose_tool.h" - -namespace rviz -{ - -Pose3DTool::Pose3DTool() - : Tool() - , arrow_( NULL ) -{ -} - -Pose3DTool::~Pose3DTool() -{ - delete arrow_; -} - -void Pose3DTool::onInitialize() -{ - arrow_ = new Arrow( scene_manager_, NULL, 2.0f, 0.2f, 0.5f, 0.35f ); - arrow_->setColor( 0.0f, 1.0f, 0.0f, 1.0f ); - arrow_->getSceneNode()->setVisible( false ); -} - -void Pose3DTool::activate() -{ - setStatus( "Click and drag mouse to set position/orientation." ); - state_ = Position; -} - -void Pose3DTool::deactivate() -{ - arrow_->getSceneNode()->setVisible( false ); -} - -int Pose3DTool::processMouseEvent( ViewportMouseEvent& event ) -{ - int flags = 0; - static Ogre::Vector3 ang_pos; - static double initz; - static double prevz; - static double prevangle; - const double z_scale = 50; - const double z_interval = 0.5; - Ogre::Quaternion orient_x = Ogre::Quaternion( Ogre::Radian(-Ogre::Math::HALF_PI), Ogre::Vector3::UNIT_Y ); - - if( event.leftDown() ) - { - ROS_ASSERT( state_ == Position ); - Ogre::Vector3 intersection; - Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f ); - if( getPointOnPlaneFromWindowXY( event.viewport, - ground_plane, - event.x, event.y, intersection )) - { - pos_ = intersection; - arrow_->setPosition( pos_ ); - state_ = Orientation; - flags |= Render; - } - } - else if( event.type == QEvent::MouseMove && event.left() ) - { - if( state_ == Orientation ) - { - //compute angle in x-y plane - Ogre::Vector3 cur_pos; - Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f ); - if( getPointOnPlaneFromWindowXY( event.viewport, - ground_plane, - event.x, event.y, cur_pos )) - { - double angle = atan2( cur_pos.y - pos_.y, cur_pos.x - pos_.x ); - arrow_->getSceneNode()->setVisible( true ); - arrow_->setOrientation( Ogre::Quaternion( Ogre::Radian(angle), Ogre::Vector3::UNIT_Z ) * orient_x ); - if ( event.right() ) - state_ = Height; - initz = pos_.z; - prevz = event.y; - prevangle = angle; - flags |= Render; - } - } - if ( state_ == Height ) - { - double z = event.y; - double dz = z - prevz; - prevz = z; - pos_.z -= dz / z_scale; - arrow_->setPosition( pos_ ); - // Create a list of arrows - for (int k = 0; k < arrow_array.size(); k++) - delete arrow_array[k]; - arrow_array.clear(); - int cnt = ceil( fabs(initz - pos_.z) / z_interval ); - for (int k = 0; k < cnt; k++) - { - Arrow* arrow__; - arrow__ = new Arrow( scene_manager_, NULL, 0.5f, 0.1f, 0.0f, 0.1f ); - arrow__->setColor( 0.0f, 1.0f, 0.0f, 1.0f ); - arrow__->getSceneNode()->setVisible( true ); - Ogre::Vector3 arr_pos = pos_; - arr_pos.z = initz - ( (initz - pos_.z > 0)?1:-1 ) * k * z_interval; - arrow__->setPosition( arr_pos ); - arrow__->setOrientation( Ogre::Quaternion( Ogre::Radian(prevangle), Ogre::Vector3::UNIT_Z ) * orient_x ); - arrow_array.push_back(arrow__); - } - flags |= Render; - } - } - else if( event.leftUp() ) - { - if( state_ == Orientation || state_ == Height) - { - // Create a list of arrows - for (int k = 0; k < arrow_array.size(); k++) - delete arrow_array[k]; - arrow_array.clear(); - onPoseSet(pos_.x, pos_.y, pos_.z, prevangle); - flags |= (Finished|Render); - } - } - - return flags; -} - -} - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/pose_tool.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/pose_tool.h deleted file mode 100644 index f2aa36cd..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/pose_tool.h +++ /dev/null @@ -1,80 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef RVIZ_POSE_TOOL_H -#define RVIZ_POSE_TOOL_H - -#include - -#include - -#include - -#include "rviz/tool.h" - -namespace rviz -{ -class Arrow; -class DisplayContext; - -class Pose3DTool: public Tool -{ -public: - Pose3DTool(); - virtual ~Pose3DTool(); - - virtual void onInitialize(); - - virtual void activate(); - virtual void deactivate(); - - virtual int processMouseEvent( ViewportMouseEvent& event ); - -protected: - virtual void onPoseSet(double x, double y, double z, double theta) = 0; - - Arrow* arrow_; - std::vector arrow_array; - - enum State - { - Position, - Orientation, - Height - }; - State state_; - - Ogre::Vector3 pos_; -}; - -} - -#endif - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/probmap_display.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/probmap_display.cpp deleted file mode 100644 index 608e2e24..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/probmap_display.cpp +++ /dev/null @@ -1,535 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -#include -#include -#include -#include -#include - -#include - -#include - -#include "rviz/frame_manager.h" -#include "rviz/ogre_helpers/grid.h" -#include "rviz/properties/float_property.h" -#include "rviz/properties/int_property.h" -#include "rviz/properties/property.h" -#include "rviz/properties/quaternion_property.h" -#include "rviz/properties/ros_topic_property.h" -#include "rviz/properties/vector_property.h" -#include "rviz/validate_floats.h" -#include "rviz/display_context.h" - -#include "probmap_display.h" - -namespace rviz -{ - -ProbMapDisplay::ProbMapDisplay() - : Display() - , manual_object_( NULL ) - // , material_( 0 ) - , loaded_( false ) - , resolution_( 0.0f ) - , width_( 0 ) - , height_( 0 ) - , position_(Ogre::Vector3::ZERO) - , orientation_(Ogre::Quaternion::IDENTITY) - , new_map_(false) -{ - topic_property_ = new RosTopicProperty( "Topic", "", - QString::fromStdString( ros::message_traits::datatype() ), - "nav_msgs::OccupancyGrid topic to subscribe to.", - this, SLOT( updateTopic() )); - - alpha_property_ = new FloatProperty( "Alpha", 0.7, - "Amount of transparency to apply to the map.", - this, SLOT( updateAlpha() )); - alpha_property_->setMin( 0 ); - alpha_property_->setMax( 1 ); - - draw_under_property_ = new Property( "Draw Behind", false, - "Rendering option, controls whether or not the map is always" - " drawn behind everything else.", - this, SLOT( updateDrawUnder() )); - - resolution_property_ = new FloatProperty( "Resolution", 0, - "Resolution of the map. (not editable)", this ); - resolution_property_->setReadOnly( true ); - - width_property_ = new IntProperty( "Width", 0, - "Width of the map, in meters. (not editable)", this ); - width_property_->setReadOnly( true ); - - height_property_ = new IntProperty( "Height", 0, - "Height of the map, in meters. (not editable)", this ); - height_property_->setReadOnly( true ); - - position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO, - "Position of the bottom left corner of the map, in meters. (not editable)", - this ); - position_property_->setReadOnly( true ); - - orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY, - "Orientation of the map. (not editable)", - this ); - orientation_property_->setReadOnly( true ); -} - -ProbMapDisplay::~ProbMapDisplay() -{ - unsubscribe(); - clear(); -} - -void ProbMapDisplay::onInitialize() -{ - static int count = 0; - std::stringstream ss; - ss << "MapObjectMaterial" << count++; - material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME ); - material_->setReceiveShadows(false); - material_->getTechnique(0)->setLightingEnabled(false); - material_->setDepthBias( -16.0f, 0.0f ); - material_->setCullingMode( Ogre::CULL_NONE ); - material_->setDepthWriteEnabled(false); - - updateAlpha(); -} - -void ProbMapDisplay::onEnable() -{ - subscribe(); -} - -void ProbMapDisplay::onDisable() -{ - unsubscribe(); - clear(); -} - -void ProbMapDisplay::subscribe() -{ - if ( !isEnabled() ) - { - return; - } - - if( !topic_property_->getTopic().isEmpty() ) - { - try - { - map_sub_ = update_nh_.subscribe( topic_property_->getTopicStd(), 1, &ProbMapDisplay::incomingMap, this ); - setStatus( StatusProperty::Ok, "Topic", "OK" ); - } - catch( ros::Exception& e ) - { - setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() ); - } - } -} - -void ProbMapDisplay::unsubscribe() -{ - map_sub_.shutdown(); -} - -void ProbMapDisplay::updateAlpha() -{ - float alpha = alpha_property_->getFloat(); - - Ogre::Pass* pass = material_->getTechnique( 0 )->getPass( 0 ); - Ogre::TextureUnitState* tex_unit = NULL; - if( pass->getNumTextureUnitStates() > 0 ) - { - tex_unit = pass->getTextureUnitState( 0 ); - } - else - { - tex_unit = pass->createTextureUnitState(); - } - - tex_unit->setAlphaOperation( Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, alpha ); - - if( alpha < 0.9998 ) - { - material_->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA ); - material_->setDepthWriteEnabled( false ); - } - else - { - material_->setSceneBlending( Ogre::SBT_REPLACE ); - material_->setDepthWriteEnabled( !draw_under_property_->getValue().toBool() ); - } -} - -void ProbMapDisplay::updateDrawUnder() -{ - bool draw_under = draw_under_property_->getValue().toBool(); - - if( alpha_property_->getFloat() >= 0.9998 ) - { - material_->setDepthWriteEnabled( !draw_under ); - } - - if( manual_object_ ) - { - if( draw_under ) - { - manual_object_->setRenderQueueGroup( Ogre::RENDER_QUEUE_4 ); - } - else - { - manual_object_->setRenderQueueGroup( Ogre::RENDER_QUEUE_MAIN ); - } - } -} - -void ProbMapDisplay::updateTopic() -{ - unsubscribe(); - subscribe(); - clear(); -} - -void ProbMapDisplay::clear() -{ - setStatus( StatusProperty::Warn, "Message", "No map received" ); - - if( !loaded_ ) - { - return; - } - - scene_manager_->destroyManualObject( manual_object_ ); - manual_object_ = NULL; - - std::string tex_name = texture_->getName(); - texture_.setNull(); - Ogre::TextureManager::getSingleton().unload( tex_name ); - - loaded_ = false; -} - -bool validateFloats(const nav_msgs::OccupancyGrid& msg) -{ - bool valid = true; - valid = valid && validateFloats( msg.info.resolution ); - valid = valid && validateFloats( msg.info.origin ); - return valid; -} - -void ProbMapDisplay::update( float wall_dt, float ros_dt ) -{ - { - boost::mutex::scoped_lock lock(mutex_); - - current_map_ = updated_map_; - } - - if (!current_map_ || !new_map_) - { - return; - } - - if (current_map_->data.empty()) - { - return; - } - - new_map_ = false; - - if( !validateFloats( *current_map_ )) - { - setStatus( StatusProperty::Error, "Map", "Message contained invalid floating point values (nans or infs)" ); - return; - } - - if( current_map_->info.width * current_map_->info.height == 0 ) - { - std::stringstream ss; - ss << "Map is zero-sized (" << current_map_->info.width << "x" << current_map_->info.height << ")"; - setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() )); - return; - } - - clear(); - - setStatus( StatusProperty::Ok, "Message", "Map received" ); - - ROS_DEBUG( "Received a %d X %d map @ %.3f m/pix\n", - current_map_->info.width, - current_map_->info.height, - current_map_->info.resolution ); - - float resolution = current_map_->info.resolution; - - int width = current_map_->info.width; - int height = current_map_->info.height; - - - Ogre::Vector3 position( current_map_->info.origin.position.x, - current_map_->info.origin.position.y, - current_map_->info.origin.position.z ); - Ogre::Quaternion orientation( current_map_->info.origin.orientation.w, - current_map_->info.origin.orientation.x, - current_map_->info.origin.orientation.y, - current_map_->info.origin.orientation.z ); - frame_ = current_map_->header.frame_id; - if (frame_.empty()) - { - frame_ = "/map"; - } - - // Expand it to be RGB data - unsigned int pixels_size = width * height; - unsigned char* pixels = new unsigned char[pixels_size]; - memset(pixels, 255, pixels_size); - - bool map_status_set = false; - unsigned int num_pixels_to_copy = pixels_size; - if( pixels_size != current_map_->data.size() ) - { - std::stringstream ss; - ss << "Data size doesn't match width*height: width = " << width - << ", height = " << height << ", data size = " << current_map_->data.size(); - setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() )); - map_status_set = true; - - // Keep going, but don't read past the end of the data. - if( current_map_->data.size() < pixels_size ) - { - num_pixels_to_copy = current_map_->data.size(); - } - } - - // TODO: a fragment shader could do this on the video card, and - // would allow a non-grayscale color to mark the out-of-range - // values. - for( unsigned int pixel_index = 0; pixel_index < num_pixels_to_copy; pixel_index++ ) - { - unsigned char val; - int8_t data = current_map_->data[ pixel_index ]; - if(data > 0) - val = 0; - else if(data < 0) - val = 255; - else - val = 127; - pixels[ pixel_index ] = val; - } - - Ogre::DataStreamPtr pixel_stream; - pixel_stream.bind( new Ogre::MemoryDataStream( pixels, pixels_size )); - static int tex_count = 0; - std::stringstream ss; - ss << "MapTexture" << tex_count++; - try - { - texture_ = Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, - pixel_stream, width, height, Ogre::PF_L8, Ogre::TEX_TYPE_2D, - 0); - - if( !map_status_set ) - { - setStatus( StatusProperty::Ok, "Map", "Map OK" ); - } - } - catch(Ogre::RenderingAPIException&) - { - Ogre::Image image; - pixel_stream->seek(0); - float fwidth = width; - float fheight = height; - if( width > height ) - { - float aspect = fheight / fwidth; - fwidth = 2048; - fheight = fwidth * aspect; - } - else - { - float aspect = fwidth / fheight; - fheight = 2048; - fwidth = fheight * aspect; - } - - { - std::stringstream ss; - ss << "Map is larger than your graphics card supports. Downsampled from [" << width << "x" << height << "] to [" << fwidth << "x" << fheight << "]"; - setStatus(StatusProperty::Ok, "Map", QString::fromStdString( ss.str() )); - } - - ROS_WARN("Failed to create full-size map texture, likely because your graphics card does not support textures of size > 2048. Downsampling to [%d x %d]...", (int)fwidth, (int)fheight); - //ROS_INFO("Stream size [%d], width [%f], height [%f], w * h [%f]", pixel_stream->size(), width, height, width * height); - image.loadRawData(pixel_stream, width, height, Ogre::PF_L8); - image.resize(fwidth, fheight, Ogre::Image::FILTER_NEAREST); - ss << "Downsampled"; - texture_ = Ogre::TextureManager::getSingleton().loadImage(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image); - } - - delete [] pixels; - - Ogre::Pass* pass = material_->getTechnique(0)->getPass(0); - Ogre::TextureUnitState* tex_unit = NULL; - if (pass->getNumTextureUnitStates() > 0) - { - tex_unit = pass->getTextureUnitState(0); - } - else - { - tex_unit = pass->createTextureUnitState(); - } - - tex_unit->setTextureName(texture_->getName()); - tex_unit->setTextureFiltering( Ogre::TFO_NONE ); - - static int map_count = 0; - std::stringstream ss2; - ss2 << "MapObject" << map_count++; - manual_object_ = scene_manager_->createManualObject( ss2.str() ); - scene_node_->attachObject( manual_object_ ); - - manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST); - { - // First triangle - { - // Bottom left - manual_object_->position( 0.0f, 0.0f, 0.0f ); - manual_object_->textureCoord(0.0f, 0.0f); - manual_object_->normal( 0.0f, 0.0f, 1.0f ); - - // Top right - manual_object_->position( resolution*width, resolution*height, 0.0f ); - manual_object_->textureCoord(1.0f, 1.0f); - manual_object_->normal( 0.0f, 0.0f, 1.0f ); - - // Top left - manual_object_->position( 0.0f, resolution*height, 0.0f ); - manual_object_->textureCoord(0.0f, 1.0f); - manual_object_->normal( 0.0f, 0.0f, 1.0f ); - } - - // Second triangle - { - // Bottom left - manual_object_->position( 0.0f, 0.0f, 0.0f ); - manual_object_->textureCoord(0.0f, 0.0f); - manual_object_->normal( 0.0f, 0.0f, 1.0f ); - - // Bottom right - manual_object_->position( resolution*width, 0.0f, 0.0f ); - manual_object_->textureCoord(1.0f, 0.0f); - manual_object_->normal( 0.0f, 0.0f, 1.0f ); - - // Top right - manual_object_->position( resolution*width, resolution*height, 0.0f ); - manual_object_->textureCoord(1.0f, 1.0f); - manual_object_->normal( 0.0f, 0.0f, 1.0f ); - } - } - manual_object_->end(); - - if( draw_under_property_->getValue().toBool() ) - { - manual_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_4); - } - - resolution_property_->setValue( resolution ); - width_property_->setValue( width ); - height_property_->setValue( height ); - position_property_->setVector( position ); - orientation_property_->setQuaternion( orientation ); - - transformMap(); - - loaded_ = true; - - context_->queueRender(); -} - -void ProbMapDisplay::incomingMap(const nav_msgs::OccupancyGrid::ConstPtr& msg) -{ - - updated_map_ = msg; - boost::mutex::scoped_lock lock(mutex_); - new_map_ = true; -} - - - -void ProbMapDisplay::transformMap() -{ - if (!current_map_) - { - return; - } - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if (!context_->getFrameManager()->transform(frame_, ros::Time(), current_map_->info.origin, position, orientation)) - { - ROS_DEBUG( "Error transforming map '%s' from frame '%s' to frame '%s'", - qPrintable( getName() ), frame_.c_str(), qPrintable( fixed_frame_ )); - - setStatus( StatusProperty::Error, "Transform", - "No transform from [" + QString::fromStdString( frame_ ) + "] to [" + fixed_frame_ + "]" ); - } - else - { - setStatus(StatusProperty::Ok, "Transform", "Transform OK"); - } - - scene_node_->setPosition( position ); - scene_node_->setOrientation( orientation ); -} - -void ProbMapDisplay::fixedFrameChanged() -{ - transformMap(); -} - -void ProbMapDisplay::reset() -{ - Display::reset(); - - clear(); - // Force resubscription so that the map will be re-sent - updateTopic(); -} - -} // namespace rviz - -#include -PLUGINLIB_EXPORT_CLASS( rviz::ProbMapDisplay, rviz::Display ) diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/probmap_display.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/probmap_display.h deleted file mode 100644 index 332def21..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/probmap_display.h +++ /dev/null @@ -1,134 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef PROB_MAP_DISPLAY_H -#define PROB_MAP_DISPLAY_H - -#include -#include -#include - -#include -#include - -#include - -#include "rviz/display.h" - -namespace Ogre -{ -class ManualObject; -} - -namespace rviz -{ - -class FloatProperty; -class IntProperty; -class Property; -class QuaternionProperty; -class RosTopicProperty; -class VectorProperty; - -/** - * \class ProbMapDisplay - * \brief Displays a map along the XY plane. - */ -class ProbMapDisplay: public Display -{ -Q_OBJECT -public: - ProbMapDisplay(); - virtual ~ProbMapDisplay(); - - // Overrides from Display - virtual void onInitialize(); - virtual void fixedFrameChanged(); - virtual void reset(); - virtual void update( float wall_dt, float ros_dt ); - - float getResolution() { return resolution_; } - int getWidth() { return width_; } - int getHeight() { return height_; } - Ogre::Vector3 getPosition() { return position_; } - Ogre::Quaternion getOrientation() { return orientation_; } - -protected Q_SLOTS: - void updateAlpha(); - void updateTopic(); - void updateDrawUnder(); - - -protected: - // overrides from Display - virtual void onEnable(); - virtual void onDisable(); - - virtual void subscribe(); - virtual void unsubscribe(); - - void incomingMap(const nav_msgs::OccupancyGrid::ConstPtr& msg); - - void clear(); - - void transformMap(); - - Ogre::ManualObject* manual_object_; - Ogre::TexturePtr texture_; - Ogre::MaterialPtr material_; - bool loaded_; - - std::string topic_; - float resolution_; - int width_; - int height_; - Ogre::Vector3 position_; - Ogre::Quaternion orientation_; - std::string frame_; - - ros::Subscriber map_sub_; - - RosTopicProperty* topic_property_; - FloatProperty* resolution_property_; - IntProperty* width_property_; - IntProperty* height_property_; - VectorProperty* position_property_; - QuaternionProperty* orientation_property_; - FloatProperty* alpha_property_; - Property* draw_under_property_; - - nav_msgs::OccupancyGrid::ConstPtr updated_map_; - nav_msgs::OccupancyGrid::ConstPtr current_map_; - boost::mutex mutex_; - bool new_map_; -}; - -} // namespace rviz - - #endif diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/CMakeLists.txt b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/CMakeLists.txt deleted file mode 100644 index 6a905522..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/CMakeLists.txt +++ /dev/null @@ -1,100 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(uav_utils) - -## Turn on verbose output -set(CMAKE_VERBOSE_MAKEFILE "false") - -## Enable C++11 -include(CheckCXXCompilerFlag) -CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) -CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) -if(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -elseif(COMPILER_SUPPORTS_CXX0X) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") -else() - message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") -endif() - -## Add some compile flags -set(ADDITIONAL_CXX_FLAG "-Wall -O3 -g") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_CXX_FLAG}") - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - cmake_utils -) - - -find_package(Eigen REQUIRED) # try to find manually installed eigen (Usually in /usr/local with provided FindEigen3.cmake) - -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS include - # LIBRARIES uav_utils - CATKIN_DEPENDS roscpp rospy - # DEPENDS system_lib -) - -########### -## Build ## -########### - - -include_directories( - include - ${EIGEN_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS geometry_utils geometry_utils_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE -) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -catkin_add_gtest(${PROJECT_NAME}-test src/${PROJECT_NAME}_test.cpp) -if(TARGET ${PROJECT_NAME}-test) - target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/converters.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/converters.h deleted file mode 100644 index 9932731d..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/converters.h +++ /dev/null @@ -1,95 +0,0 @@ -#ifndef __UAVUTILS_CONVERTERS_H -#define __UAVUTILS_CONVERTERS_H - -#include -#include -#include -#include - -#include -#include -#include - -namespace uav_utils { - -inline void extract_odometry(nav_msgs::OdometryConstPtr msg, Eigen::Vector3d& p, - Eigen::Vector3d& v, Eigen::Quaterniond& q) -{ - p(0) = msg->pose.pose.position.x; - p(1) = msg->pose.pose.position.y; - p(2) = msg->pose.pose.position.z; - - v(0) = msg->twist.twist.linear.x; - v(1) = msg->twist.twist.linear.y; - v(2) = msg->twist.twist.linear.z; - - q.w() = msg->pose.pose.orientation.w; - q.x() = msg->pose.pose.orientation.x; - q.y() = msg->pose.pose.orientation.y; - q.z() = msg->pose.pose.orientation.z; -} - -inline void extract_odometry(nav_msgs::OdometryConstPtr msg, Eigen::Vector3d& p, - Eigen::Vector3d& v, Eigen::Quaterniond& q, Eigen::Vector3d& w) -{ - extract_odometry(msg, p, v, q); - - w(0) = msg->twist.twist.angular.x; - w(1) = msg->twist.twist.angular.y; - w(2) = msg->twist.twist.angular.z; -} - - -template -Eigen::Matrix from_vector3_msg(const geometry_msgs::Vector3& msg) { - return Eigen::Matrix(msg.x, msg.y, msg.z); -} - -template -geometry_msgs::Vector3 to_vector3_msg(const Eigen::DenseBase& v) { - EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); - EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - - geometry_msgs::Vector3 msg; - msg.x = v.x(); - msg.y = v.y(); - msg.z = v.z(); - return msg; -} - -template -Eigen::Matrix from_point_msg(const geometry_msgs::Point& msg) { - return Eigen::Matrix(msg.x, msg.y, msg.z); -} - -template -geometry_msgs::Point to_point_msg(const Eigen::DenseBase& v) { - EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); - EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - - geometry_msgs::Point msg; - msg.x = v.x(); - msg.y = v.y(); - msg.z = v.z(); - return msg; -} - -template -Eigen::Quaternion from_quaternion_msg(const geometry_msgs::Quaternion& msg) { - return Eigen::Quaternion(msg.w, msg.x, msg.y, msg.z); -} - -template -geometry_msgs::Quaternion to_quaternion_msg(const Eigen::Quaternion& q) { - geometry_msgs::Quaternion msg; - msg.x = q.x(); - msg.y = q.y(); - msg.z = q.z(); - msg.w = q.w(); - return msg; -} -} - -#endif diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/geometry_utils.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/geometry_utils.h deleted file mode 100644 index c3babb1c..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/geometry_utils.h +++ /dev/null @@ -1,248 +0,0 @@ -#ifndef __GEOMETRY_UTILS_H -#define __GEOMETRY_UTILS_H - -#include - -/* clang-format off */ -namespace uav_utils { - -template -Scalar_t toRad(const Scalar_t& x) { - return x / 180.0 * M_PI; -} - -template -Scalar_t toDeg(const Scalar_t& x) { - return x * 180.0 / M_PI; -} - -template -Eigen::Matrix rotx(Scalar_t t) { - Eigen::Matrix R; - R(0, 0) = 1.0; - R(0, 1) = 0.0; - R(0, 2) = 0.0; - R(1, 0) = 0.0; - R(1, 1) = std::cos(t); - R(1, 2) = -std::sin(t); - R(2, 0) = 0.0; - R(2, 1) = std::sin(t); - R(2, 2) = std::cos(t); - - return R; -} - -template -Eigen::Matrix roty(Scalar_t t) { - Eigen::Matrix R; - R(0, 0) = std::cos(t); - R(0, 1) = 0.0; - R(0, 2) = std::sin(t); - R(1, 0) = 0.0; - R(1, 1) = 1.0; - R(1, 2) = 0; - R(2, 0) = -std::sin(t); - R(2, 1) = 0.0; - R(2, 2) = std::cos(t); - - return R; -} - -template -Eigen::Matrix rotz(Scalar_t t) { - Eigen::Matrix R; - R(0, 0) = std::cos(t); - R(0, 1) = -std::sin(t); - R(0, 2) = 0.0; - R(1, 0) = std::sin(t); - R(1, 1) = std::cos(t); - R(1, 2) = 0.0; - R(2, 0) = 0.0; - R(2, 1) = 0.0; - R(2, 2) = 1.0; - - return R; -} - -template -Eigen::Matrix ypr_to_R(const Eigen::DenseBase& ypr) { - EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); - EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - - typename Derived::Scalar c, s; - - Eigen::Matrix Rz = Eigen::Matrix::Zero(); - typename Derived::Scalar y = ypr(0); - c = cos(y); - s = sin(y); - Rz(0, 0) = c; - Rz(1, 0) = s; - Rz(0, 1) = -s; - Rz(1, 1) = c; - Rz(2, 2) = 1; - - Eigen::Matrix Ry = Eigen::Matrix::Zero(); - typename Derived::Scalar p = ypr(1); - c = cos(p); - s = sin(p); - Ry(0, 0) = c; - Ry(2, 0) = -s; - Ry(0, 2) = s; - Ry(2, 2) = c; - Ry(1, 1) = 1; - - Eigen::Matrix Rx = Eigen::Matrix::Zero(); - typename Derived::Scalar r = ypr(2); - c = cos(r); - s = sin(r); - Rx(1, 1) = c; - Rx(2, 1) = s; - Rx(1, 2) = -s; - Rx(2, 2) = c; - Rx(0, 0) = 1; - - Eigen::Matrix R = Rz * Ry * Rx; - return R; -} - -template -Eigen::Matrix R_to_ypr(const Eigen::DenseBase& R) { - EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); - EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - - Eigen::Matrix n = R.col(0); - Eigen::Matrix o = R.col(1); - Eigen::Matrix a = R.col(2); - - Eigen::Matrix ypr(3); - typename Derived::Scalar y = atan2(n(1), n(0)); - typename Derived::Scalar p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); - typename Derived::Scalar r = - atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); - ypr(0) = y; - ypr(1) = p; - ypr(2) = r; - - return ypr; -} - -template -Eigen::Quaternion ypr_to_quaternion(const Eigen::DenseBase& ypr) { - EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); - EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - - const typename Derived::Scalar cy = cos(ypr(0) / 2.0); - const typename Derived::Scalar sy = sin(ypr(0) / 2.0); - const typename Derived::Scalar cp = cos(ypr(1) / 2.0); - const typename Derived::Scalar sp = sin(ypr(1) / 2.0); - const typename Derived::Scalar cr = cos(ypr(2) / 2.0); - const typename Derived::Scalar sr = sin(ypr(2) / 2.0); - - Eigen::Quaternion q; - - q.w() = cr * cp * cy + sr * sp * sy; - q.x() = sr * cp * cy - cr * sp * sy; - q.y() = cr * sp * cy + sr * cp * sy; - q.z() = cr * cp * sy - sr * sp * cy; - - return q; -} - -template -Eigen::Matrix quaternion_to_ypr(const Eigen::Quaternion& q_) { - Eigen::Quaternion q = q_.normalized(); - - Eigen::Matrix ypr; - ypr(2) = atan2(2 * (q.w() * q.x() + q.y() * q.z()), 1 - 2 * (q.x() * q.x() + q.y() * q.y())); - ypr(1) = asin(2 * (q.w() * q.y() - q.z() * q.x())); - ypr(0) = atan2(2 * (q.w() * q.z() + q.x() * q.y()), 1 - 2 * (q.y() * q.y() + q.z() * q.z())); - - return ypr; -} - -template -Scalar_t get_yaw_from_quaternion(const Eigen::Quaternion& q) { - return quaternion_to_ypr(q)(0); -} - -template -Eigen::Quaternion yaw_to_quaternion(Scalar_t yaw) { - return Eigen::Quaternion(rotz(yaw)); -} - -template -Scalar_t normalize_angle(Scalar_t a) { - int cnt = 0; - while (true) { - cnt++; - - if (a < -M_PI) { - a += M_PI * 2.0; - } else if (a > M_PI) { - a -= M_PI * 2.0; - } - - if (-M_PI <= a && a <= M_PI) { - break; - }; - - assert(cnt < 10 && "[uav_utils/geometry_msgs] INVALID INPUT ANGLE"); - } - - return a; -} - -template -Scalar_t angle_add(Scalar_t a, Scalar_t b) { - Scalar_t c = a + b; - c = normalize_angle(c); - assert(-M_PI <= c && c <= M_PI); - return c; -} - -template -Scalar_t yaw_add(Scalar_t a, Scalar_t b) { - return angle_add(a, b); -} - -template -Eigen::Matrix get_skew_symmetric(const Eigen::DenseBase& v) { - EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); - EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - - Eigen::Matrix M; - M.setZero(); - M(0, 1) = -v(2); - M(0, 2) = v(1); - M(1, 0) = v(2); - M(1, 2) = -v(0); - M(2, 0) = -v(1); - M(2, 1) = v(0); - return M; -} - -template -Eigen::Matrix from_skew_symmetric(const Eigen::DenseBase& M) { - EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); - EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - - Eigen::Matrix v; - v(0) = M(2, 1); - v(1) = -M(2, 0); - v(2) = M(1, 0); - - assert(v.isApprox(Eigen::Matrix(-M(1, 2), M(0, 2), -M(0, 1)))); - - return v; -} - - -} // end of namespace uav_utils -/* clang-format on */ - -#endif diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/utils.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/utils.h deleted file mode 100644 index 0ce4c281..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/utils.h +++ /dev/null @@ -1,59 +0,0 @@ -#ifndef __UAV_UTILS_H -#define __UAV_UTILS_H - -#include - -#include -#include - -namespace uav_utils -{ - -/* judge if value belongs to [low,high] */ -template -bool -in_range(T value, const T2& low, const T2& high) -{ - ROS_ASSERT_MSG(low < high, "%f < %f?", low, high); - return (low <= value) && (value <= high); -} - -/* judge if value belongs to [-limit, limit] */ -template -bool -in_range(T value, const T2& limit) -{ - ROS_ASSERT_MSG(limit > 0, "%f > 0?", limit); - return in_range(value, -limit, limit); -} - -template -void -limit_range(T& value, const T2& low, const T2& high) -{ - ROS_ASSERT_MSG(low < high, "%f < %f?", low, high); - if (value < low) - { - value = low; - } - - if (value > high) - { - value = high; - } - - return; -} - -template -void -limit_range(T& value, const T2& limit) -{ - ROS_ASSERT_MSG(limit > 0, "%f > 0?", limit); - limit_range(value, -limit, limit); -} - -typedef std::stringstream DebugSS_t; -} // end of namespace uav_utils - -#endif diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/package.xml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/package.xml deleted file mode 100644 index db864b75..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/package.xml +++ /dev/null @@ -1,58 +0,0 @@ - - - uav_utils - 0.0.0 - The uav_utils package - - - - - LIU Tianbo - - - - - - LGPLv3 - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - cmake_modules - cmake_utils - roscpp - rospy - cmake_modules - cmake_utils - - - - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/odom_to_euler.py b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/odom_to_euler.py deleted file mode 100644 index 92ac8799..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/odom_to_euler.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python - -import rospy -import numpy as np -import tf -from tf import transformations as tfs -from nav_msgs.msg import Odometry -from sensor_msgs.msg import Imu -from geometry_msgs.msg import Vector3Stamped -from sensor_msgs.msg import Joy - -pub = None -pub1 = None - -def callback(odom_msg): - q = np.array([odom_msg.pose.pose.orientation.x, - odom_msg.pose.pose.orientation.y, - odom_msg.pose.pose.orientation.z, - odom_msg.pose.pose.orientation.w]) - - e = tfs.euler_from_quaternion(q, 'rzyx') - - euler_msg = Vector3Stamped() - euler_msg.header = odom_msg.header - euler_msg.vector.z = e[0]*180.0/3.14159 - euler_msg.vector.y = e[1]*180.0/3.14159 - euler_msg.vector.x = e[2]*180.0/3.14159 - - pub.publish(euler_msg) - -def imu_callback(imu_msg): - q = np.array([imu_msg.orientation.x, - imu_msg.orientation.y, - imu_msg.orientation.z, - imu_msg.orientation.w]) - - e = tfs.euler_from_quaternion(q, 'rzyx') - - euler_msg = Vector3Stamped() - euler_msg.header = imu_msg.header - euler_msg.vector.z = e[0]*180.0/3.14159 - euler_msg.vector.y = e[1]*180.0/3.14159 - euler_msg.vector.x = e[2]*180.0/3.14159 - - pub1.publish(euler_msg) - -def joy_callback(joy_msg): - out_msg = Vector3Stamped() - out_msg.header = joy_msg.header - out_msg.vector.z = -joy_msg.axes[3] - out_msg.vector.y = joy_msg.axes[1] - out_msg.vector.x = joy_msg.axes[0] - - pub2.publish(out_msg) - - -if __name__ == "__main__": - rospy.init_node("odom_to_euler") - - pub = rospy.Publisher("~euler", Vector3Stamped, queue_size=10) - sub = rospy.Subscriber("~odom", Odometry, callback) - - pub1 = rospy.Publisher("~imueuler", Vector3Stamped, queue_size=10) - sub1 = rospy.Subscriber("~imu", Imu, imu_callback) - - pub2 = rospy.Publisher("~ctrlout", Vector3Stamped, queue_size=10) - sub2 = rospy.Subscriber("~ctrlin", Joy, joy_callback) - - rospy.spin() diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/send_odom.py b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/send_odom.py deleted file mode 100644 index a08b586c..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/send_odom.py +++ /dev/null @@ -1,42 +0,0 @@ -#!/usr/bin/env python - -import rospy -import numpy as np -import tf -from tf import transformations as tfs -from nav_msgs.msg import Odometry - -if __name__ == "__main__": - rospy.init_node("odom_sender") - - msg = Odometry() - - msg.header.stamp = rospy.Time.now()-rospy.Duration(0.2) - msg.header.frame_id = "world" - - q = tfs.quaternion_from_euler(0,0,0,"rzyx") - - msg.pose.pose.position.x = 0 - msg.pose.pose.position.y = 0 - msg.pose.pose.position.z = 0 - msg.twist.twist.linear.x = 0 - msg.twist.twist.linear.y = 0 - msg.twist.twist.linear.z = 0 - msg.pose.pose.orientation.x = q[0] - msg.pose.pose.orientation.y = q[1] - msg.pose.pose.orientation.z = q[2] - msg.pose.pose.orientation.w = q[3] - - print(msg) - - pub = rospy.Publisher("odom", Odometry, queue_size=10) - - counter = 0 - r = rospy.Rate(1) - - while not rospy.is_shutdown(): - counter += 1 - msg.header.stamp = rospy.Time.now()-rospy.Duration(0.2) - pub.publish(msg) - rospy.loginfo("Send %3d msg(s)."%counter) - r.sleep() diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/tf_assist.py b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/tf_assist.py deleted file mode 100644 index 259e8a18..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/tf_assist.py +++ /dev/null @@ -1,162 +0,0 @@ -#!/usr/bin/env python - -import rospy -import numpy as np -import tf -from tf import transformations as tfs -from math import pi -from nav_msgs.msg import Odometry -from nav_msgs.msg import Path -from geometry_msgs.msg import PoseStamped -from sensor_msgs.msg import Imu -from sensor_msgs.msg import Joy - - -imu_pub = None -odom_pub = None -br = None - - -class OdometryConverter(object): - - def __init__(self, frame_id_in_, frame_id_out_, broadcast_tf_, body_frame_id_, intermediate_frame_id_, world_frame_id_): - self.frame_id_in = frame_id_in_ - self.frame_id_out = frame_id_out_ - self.broadcast_tf = broadcast_tf_ - self.body_frame_id = body_frame_id_ - self.intermediate_frame_id = intermediate_frame_id_ - self.world_frame_id = world_frame_id_ - self.in_odom_sub = None - self.out_odom_pub = None - self.out_path_pub = None - self.path_pub_timer = None - self.tf_pub_flag = True - if self.broadcast_tf: - rospy.loginfo('ROSTopic: [%s]->[%s] TF: [%s]-[%s]-[%s]', - self.frame_id_in, self.frame_id_out, self.body_frame_id, self.intermediate_frame_id, self.world_frame_id) - else: - rospy.loginfo('ROSTopic: [%s]->[%s] No TF', - self.frame_id_in, self.frame_id_out) - - self.path = [] - - def in_odom_callback(self, in_odom_msg): - q = np.array([in_odom_msg.pose.pose.orientation.x, - in_odom_msg.pose.pose.orientation.y, - in_odom_msg.pose.pose.orientation.z, - in_odom_msg.pose.pose.orientation.w]) - p = np.array([in_odom_msg.pose.pose.position.x, - in_odom_msg.pose.pose.position.y, - in_odom_msg.pose.pose.position.z]) - - e = tfs.euler_from_quaternion(q, 'rzyx') - wqb = tfs.quaternion_from_euler(e[0], e[1], e[2], 'rzyx') - wqc = tfs.quaternion_from_euler(e[0], 0.0, 0.0, 'rzyx') - - #### odom #### - odom_msg = in_odom_msg - assert(in_odom_msg.header.frame_id == self.frame_id_in) - odom_msg.header.frame_id = self.frame_id_out - odom_msg.child_frame_id = "" - self.out_odom_pub.publish(odom_msg) - - #### tf #### - if self.broadcast_tf and self.tf_pub_flag: - self.tf_pub_flag = False - if not self.frame_id_in == self.frame_id_out: - br.sendTransform((0.0, 0.0, 0.0), - tfs.quaternion_from_euler(0.0, 0.0, 0.0, 'rzyx'), - odom_msg.header.stamp, - self.frame_id_in, - self.frame_id_out) - - if not self.world_frame_id == self.frame_id_out: - br.sendTransform((0.0, 0.0, 0.0), - tfs.quaternion_from_euler(0.0, 0.0, 0.0, 'rzyx'), - odom_msg.header.stamp, - self.world_frame_id, - self.frame_id_out) - - br.sendTransform((p[0], p[1], p[2]), - wqb, - odom_msg.header.stamp, - self.body_frame_id, - self.world_frame_id) - - br.sendTransform(((p[0], p[1], p[2])), - wqc, - odom_msg.header.stamp, - self.intermediate_frame_id, - self.world_frame_id) - #### path #### - pose = PoseStamped() - pose.header = odom_msg.header - pose.pose.position.x = p[0] - pose.pose.position.y = p[1] - pose.pose.position.z = p[2] - pose.pose.orientation.x = q[0] - pose.pose.orientation.y = q[1] - pose.pose.orientation.z = q[2] - pose.pose.orientation.w = q[3] - - self.path.append(pose) - - def path_pub_callback(self, event): - if self.path: - path = Path() - path.header = self.path[-1].header - path.poses = self.path[-30000::1] - self.out_path_pub.publish(path) - - def tf_pub_callback(self, event): - self.tf_pub_flag = True - - -if __name__ == "__main__": - rospy.init_node('tf_assist') - - converters = [] - index = 0 - while True: - prefix = "~converter%d/" % index - try: - frame_id_in = rospy.get_param('%sframe_id_in' % prefix) - frame_id_out = rospy.get_param('%sframe_id_out' % prefix) - broadcast_tf = rospy.get_param('%sbroadcast_tf' % prefix, False) - body_frame_id = rospy.get_param('%sbody_frame_id' % prefix, 'body') - intermediate_frame_id = rospy.get_param( - '%sintermediate_frame_id' % prefix, 'intermediate') - world_frame_id = rospy.get_param( - '%sworld_frame_id' % prefix, 'world') - - converter = OdometryConverter( - frame_id_in, frame_id_out, broadcast_tf, body_frame_id, intermediate_frame_id, world_frame_id) - converter.in_odom_sub = rospy.Subscriber( - '%sin_odom' % prefix, Odometry, converter.in_odom_callback, tcp_nodelay=True) - converter.out_odom_pub = rospy.Publisher( - '%sout_odom' % prefix, Odometry, queue_size=10, tcp_nodelay=True) - converter.out_path_pub = rospy.Publisher( - '%sout_path' % prefix, Path, queue_size=10) - - converter.tf_pub_timer = rospy.Timer( - rospy.Duration(0.1), converter.tf_pub_callback) - - converter.path_pub_timer = rospy.Timer( - rospy.Duration(0.5), converter.path_pub_callback) - - index += 1 - except KeyError, e: - if index == 0: - raise(KeyError(e)) - else: - if index == 1: - rospy.loginfo( - 'prefix:"%s" not found. Generate %d converter.' % (prefix, index)) - else: - rospy.loginfo( - 'prefix:"%s" not found. Generate %d converters' % (prefix, index)) - break - - br = tf.TransformBroadcaster() - - rospy.spin() diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/topic_statistics.py b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/topic_statistics.py deleted file mode 100644 index c69ffb64..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/topic_statistics.py +++ /dev/null @@ -1,15 +0,0 @@ -#!/usr/bin/env python - -import argparse as ap -import argcomplete - -def main(**args): - pass - -if __name__ == '__main__': - parser = ap.ArgumentParser() - parser.add_argument('positional', choices=['spam', 'eggs']) - parser.add_argument('--optional', choices=['foo1', 'foo2', 'bar']) - argcomplete.autocomplete(parser) - args = parser.parse_args() - main(**vars(args)) \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/src/uav_utils_test.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/src/uav_utils_test.cpp deleted file mode 100644 index 60da5afb..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/src/uav_utils_test.cpp +++ /dev/null @@ -1,122 +0,0 @@ -#include -#include - -using namespace uav_utils; -using namespace Eigen; - -#define DOUBLE_EPS 1.0e-15 -#define FLOAT_EPS 1.0e-6 - -TEST(GeometryUtilsDouble, Rotation) { - Vector3d v(0.1, 0.2, 0.3); - ASSERT_TRUE(v.isApprox(R_to_ypr(ypr_to_R(v)),DOUBLE_EPS)); - ASSERT_TRUE(v.isApprox(quaternion_to_ypr(ypr_to_quaternion(v)),DOUBLE_EPS)); - ASSERT_TRUE(v.isApprox(R_to_ypr(rotz(v(0)) * roty(v(1)) * rotx(v(2))),DOUBLE_EPS)); - - double yaw = 30.0; - ASSERT_DOUBLE_EQ(30.0, toDeg(get_yaw_from_quaternion(yaw_to_quaternion(toRad(yaw))))); -} - -TEST(GeometryUtilsFloat, Rotation) { - Vector3f v(0.1, 0.2, 0.3); - ASSERT_TRUE(v.isApprox(R_to_ypr(ypr_to_R(v)),FLOAT_EPS)); - ASSERT_TRUE(v.isApprox(quaternion_to_ypr(ypr_to_quaternion(v)),FLOAT_EPS)); - ASSERT_TRUE(v.isApprox(R_to_ypr(rotz(v(0)) * roty(v(1)) * rotx(v(2))),FLOAT_EPS)); - - float yaw = 30.0; - ASSERT_FLOAT_EQ(30.0, toDeg(get_yaw_from_quaternion(yaw_to_quaternion(toRad(yaw))))); -} - -TEST(GeometryUtilsDouble, Skew) { - double v1 = 0.1; - double v2 = 0.2; - double v3 = 0.3; - Vector3d v(v1, v2, v3); - Matrix3d M; - M << .0, -v3, v2, - v3, .0, -v1, - -v2, v1, .0; - - ASSERT_TRUE(M.isApprox(get_skew_symmetric(v), DOUBLE_EPS)); - ASSERT_TRUE(v.isApprox(from_skew_symmetric(M), DOUBLE_EPS)); -} - -TEST(GeometryUtilsFloat, Skew) { - float v1 = 0.1; - float v2 = 0.2; - float v3 = 0.3; - Vector3f v(v1, v2, v3); - Matrix3f M; - M << .0, -v3, v2, - v3, .0, -v1, - -v2, v1, .0; - - ASSERT_TRUE(M.isApprox(get_skew_symmetric(v), FLOAT_EPS)); - ASSERT_TRUE(v.isApprox(from_skew_symmetric(M), FLOAT_EPS)); -} - -TEST(GeometryUtilsDouble, Angle) { - double a = toRad(179.0); - double b = toRad(2.0); - ASSERT_DOUBLE_EQ(-179.0, toDeg(angle_add(a, b))); - ASSERT_DOUBLE_EQ(-179.0, toDeg(yaw_add(a, b))); - ASSERT_DOUBLE_EQ(179.0, toDeg(angle_add(-a, -b))); - ASSERT_DOUBLE_EQ(179.0, toDeg(yaw_add(-a, -b))); - ASSERT_DOUBLE_EQ(177.0, toDeg(angle_add(a, -b))); - ASSERT_DOUBLE_EQ(177.0, toDeg(yaw_add(a, -b))); - ASSERT_NEAR(toRad(-2.0), normalize_angle(toRad(358.0)), DOUBLE_EPS); -} - -TEST(GeometryUtilsFloat, Angle) { - float a = toRad(179.0); - float b = toRad(2.0); - ASSERT_FLOAT_EQ(-179.0, toDeg(angle_add(a, b))); - ASSERT_FLOAT_EQ(-179.0, toDeg(yaw_add(a, b))); - ASSERT_FLOAT_EQ(179.0, toDeg(angle_add(-a, -b))); - ASSERT_FLOAT_EQ(179.0, toDeg(yaw_add(-a, -b))); - ASSERT_FLOAT_EQ(177.0, toDeg(angle_add(a, -b))); - ASSERT_FLOAT_EQ(177.0, toDeg(yaw_add(a, -b))); - ASSERT_NEAR(-2.0, toDeg(normalize_angle(toRad(358.0))),FLOAT_EPS); -} - -TEST(ConverterDouble, Equality) { - nav_msgs::OdometryPtr pOdom(new nav_msgs::Odometry()); - - pOdom->pose.pose.position.x = 1.0; - pOdom->pose.pose.position.y = 2.0; - pOdom->pose.pose.position.z = 3.0; - - pOdom->pose.pose.orientation.w = 0.5; - pOdom->pose.pose.orientation.x = -0.5; - pOdom->pose.pose.orientation.y = 0.5; - pOdom->pose.pose.orientation.z = -0.5; - - pOdom->twist.twist.linear.x = -1.0; - pOdom->twist.twist.linear.y = -2.0; - pOdom->twist.twist.linear.z = -3.0; - - pOdom->twist.twist.angular.x = -0.1; - pOdom->twist.twist.angular.y = -0.2; - pOdom->twist.twist.angular.z = -0.3; - - Eigen::Vector3d p, v, w; - Eigen::Quaterniond q; - - nav_msgs::Odometry odom_ = *pOdom; - - extract_odometry(pOdom, p, v, q, w); - - ASSERT_TRUE(v.isApprox(from_vector3_msg(pOdom->twist.twist.linear))); - ASSERT_TRUE(w.isApprox(from_vector3_msg(pOdom->twist.twist.angular))); - ASSERT_TRUE(p.isApprox(from_point_msg(pOdom->pose.pose.position))); - ASSERT_TRUE(q.isApprox(from_quaternion_msg(pOdom->pose.pose.orientation))); - - ASSERT_TRUE(v.isApprox(from_vector3_msg(to_vector3_msg(v)))); - ASSERT_TRUE(p.isApprox(from_point_msg(to_point_msg(p)))); - ASSERT_TRUE(q.isApprox(from_quaternion_msg(to_quaternion_msg(q)))); -} - -int main(int argc, char* argv[]) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/CMakeLists.txt b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/CMakeLists.txt deleted file mode 100644 index eb4e7524..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/CMakeLists.txt +++ /dev/null @@ -1,35 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(waypoint_generator) - -set(CMAKE_VERBOSE_MAKEFILE "false") -include(CheckCXXCompilerFlag) -CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) -CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) -if(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -elseif(COMPILER_SUPPORTS_CXX0X) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") -else() - message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") -endif() - -set(ADDITIONAL_CXX_FLAG "-Wall -O3 -march=native") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_CXX_FLAG}") - -find_package(catkin REQUIRED COMPONENTS - roscpp - tf - nav_msgs -) -catkin_package() - -include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -add_executable(waypoint_generator src/waypoint_generator.cpp) - -target_link_libraries(waypoint_generator - ${catkin_LIBRARIES} -) diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/package.xml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/package.xml deleted file mode 100644 index 2c6078d6..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - 0.0.0 - waypoint_generator - - - waypoint_generator - - - Shaojie Shen - BSD - http://ros.org/wiki/waypoint_generator - - catkin - - roscpp - tf - nav_msgs - - roscpp - tf - nav_msgs - - - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/src/sample_waypoints.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/src/sample_waypoints.h deleted file mode 100644 index 8daad3c9..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/src/sample_waypoints.h +++ /dev/null @@ -1,213 +0,0 @@ -#ifndef SAMPLE_WAYPOINTS_H -#define SAMPLE_WAYPOINTS_H - -#include -#include -#include - -nav_msgs::Path point() -{ - // Circle parameters - nav_msgs::Path waypoints; - geometry_msgs::PoseStamped pt; - pt.pose.orientation = tf::createQuaternionMsgFromYaw(0.0); - - double h = 1.0; - double scale = 7.0; - - pt.pose.position.y = scale * 0.0; - pt.pose.position.x = scale * 2.0; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - - pt.pose.position.y = scale * 0.0; - pt.pose.position.x = scale * 4.0; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - - pt.pose.position.y = scale * 0.25; - pt.pose.position.x = scale * 5.0; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - - pt.pose.position.y = scale * 0.5; - pt.pose.position.x = scale * 5.3; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - - pt.pose.position.y = scale * 0.75; - pt.pose.position.x = scale * 5.0; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - - pt.pose.position.y = scale * 1.0; - pt.pose.position.x = scale * 4.0; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - - pt.pose.position.y = scale * 1.0; - pt.pose.position.x = scale * 2.0; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - - pt.pose.position.y = scale * 1.0; - pt.pose.position.x = scale * 0.0; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - // Return - return waypoints; -} - -// Circle trajectory -nav_msgs::Path circle() -{ - double h = 1.0; - double scale = 5.0; - nav_msgs::Path waypoints; - geometry_msgs::PoseStamped pt; - pt.pose.orientation = tf::createQuaternionMsgFromYaw(0.0); - - pt.pose.position.y = -1.2 * scale; - pt.pose.position.x = 2.5 * scale; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - - pt.pose.position.y = -2.4 * scale; - pt.pose.position.x = 5.0 * scale; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - pt.pose.position.y = 0.0 * scale; - pt.pose.position.x = 5.0 * scale; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - - pt.pose.position.y = -1.2 * scale; - pt.pose.position.x = 2.5 * scale; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - - pt.pose.position.y = -2.4 * scale; - pt.pose.position.x = 0. * scale; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - pt.pose.position.y = 0.0 * scale; - pt.pose.position.x = 0.0 * scale; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - - pt.pose.position.y = -1.2 * scale; - pt.pose.position.x = 2.5 * scale; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - - pt.pose.position.y = -2.4 * scale; - pt.pose.position.x = 5.0 * scale; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - pt.pose.position.y = 0.0 * scale; - pt.pose.position.x = 5.0 * scale; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - - pt.pose.position.y = -1.2 * scale; - pt.pose.position.x = 2.5 * scale; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - - pt.pose.position.y = -2.4 * scale; - pt.pose.position.x = 0. * scale; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - pt.pose.position.y = 0.0 * scale; - pt.pose.position.x = 0.0 * scale; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - - // Return - return waypoints; -} - -// Figure 8 trajectory -nav_msgs::Path eight() -{ - // Circle parameters - double offset_x = 0.0; - double offset_y = 0.0; - double r = 10.0; - double h = 2.0; - nav_msgs::Path waypoints; - geometry_msgs::PoseStamped pt; - pt.pose.orientation = tf::createQuaternionMsgFromYaw(0.0); - - for(int i=0; i< 1; ++i) - { - // First loop - pt.pose.position.x = r + offset_x; - pt.pose.position.y = -r + offset_y; - pt.pose.position.z = h/2; - waypoints.poses.push_back(pt); - pt.pose.position.x = r*2 + offset_x * 2; - pt.pose.position.y = 0 ; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - pt.pose.position.x = r*3 + offset_x * 3; - pt.pose.position.y = r ; - pt.pose.position.z = h/2; - waypoints.poses.push_back(pt); - pt.pose.position.x = r*4 + offset_x * 4; - pt.pose.position.y = 0 ; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - pt.pose.position.x = r*3 + offset_x * 3; - pt.pose.position.y = -r ; - pt.pose.position.z = h/2; - waypoints.poses.push_back(pt); - pt.pose.position.x = r*2 + offset_x * 2; - pt.pose.position.y = 0 ; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - pt.pose.position.x = r + offset_x * 2; - pt.pose.position.y = r ; - pt.pose.position.z = h/2; - waypoints.poses.push_back(pt); - pt.pose.position.x = 0 + offset_x; - pt.pose.position.y = 0; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - // Second loop - pt.pose.position.x = r + offset_x; - pt.pose.position.y = -r; - pt.pose.position.z = h / 2 * 3; - waypoints.poses.push_back(pt); - pt.pose.position.x = r*2 + offset_x * 2; - pt.pose.position.y = 0; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - pt.pose.position.x = r*3 + offset_x * 3; - pt.pose.position.y = r; - pt.pose.position.z = h / 2 * 3; - waypoints.poses.push_back(pt); - pt.pose.position.x = r*4 + offset_x * 4; - pt.pose.position.y = 0; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - pt.pose.position.x = r*3 + offset_x * 3; - pt.pose.position.y = -r; - pt.pose.position.z = h / 2 * 3; - waypoints.poses.push_back(pt); - pt.pose.position.x = r*2 + offset_x * 2; - pt.pose.position.y = 0; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - pt.pose.position.x = r + offset_x; - pt.pose.position.y = r + offset_y; - pt.pose.position.z = h / 2 * 3; - waypoints.poses.push_back(pt); - pt.pose.position.x = 0; - pt.pose.position.y = 0; - pt.pose.position.z = h; - waypoints.poses.push_back(pt); - } - return waypoints; -} -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/src/waypoint_generator.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/src/waypoint_generator.cpp deleted file mode 100644 index 1eb2d043..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/src/waypoint_generator.cpp +++ /dev/null @@ -1,258 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include "sample_waypoints.h" -#include -#include -#include -#include - -using namespace std; -using bfmt = boost::format; - -ros::Publisher pub1; -ros::Publisher pub2; -ros::Publisher pub3; -string waypoint_type = string("manual"); -bool is_odom_ready; -nav_msgs::Odometry odom; -nav_msgs::Path waypoints; - -// series waypoint needed -std::deque waypointSegments; -ros::Time trigged_time; - -void load_seg(ros::NodeHandle& nh, int segid, const ros::Time& time_base) { - std::string seg_str = boost::str(bfmt("seg%d/") % segid); - double yaw; - double time_of_start = 0.0; - ROS_INFO("Getting segment %d", segid); - ROS_ASSERT(nh.getParam(seg_str + "yaw", yaw)); - ROS_ASSERT_MSG((yaw > -3.1499999) && (yaw < 3.14999999), "yaw=%.3f", yaw); - ROS_ASSERT(nh.getParam(seg_str + "time_of_start", time_of_start)); - ROS_ASSERT(time_of_start >= 0.0); - - std::vector ptx; - std::vector pty; - std::vector ptz; - - ROS_ASSERT(nh.getParam(seg_str + "x", ptx)); - ROS_ASSERT(nh.getParam(seg_str + "y", pty)); - ROS_ASSERT(nh.getParam(seg_str + "z", ptz)); - - ROS_ASSERT(ptx.size()); - ROS_ASSERT(ptx.size() == pty.size() && ptx.size() == ptz.size()); - - nav_msgs::Path path_msg; - - path_msg.header.stamp = time_base + ros::Duration(time_of_start); - - double baseyaw = tf::getYaw(odom.pose.pose.orientation); - - for (size_t k = 0; k < ptx.size(); ++k) { - geometry_msgs::PoseStamped pt; - pt.pose.orientation = tf::createQuaternionMsgFromYaw(baseyaw + yaw); - Eigen::Vector2d dp(ptx.at(k), pty.at(k)); - Eigen::Vector2d rdp; - rdp.x() = std::cos(-baseyaw-yaw)*dp.x() + std::sin(-baseyaw-yaw)*dp.y(); - rdp.y() =-std::sin(-baseyaw-yaw)*dp.x() + std::cos(-baseyaw-yaw)*dp.y(); - pt.pose.position.x = rdp.x() + odom.pose.pose.position.x; - pt.pose.position.y = rdp.y() + odom.pose.pose.position.y; - pt.pose.position.z = ptz.at(k) + odom.pose.pose.position.z; - path_msg.poses.push_back(pt); - } - - waypointSegments.push_back(path_msg); -} - -void load_waypoints(ros::NodeHandle& nh, const ros::Time& time_base) { - int seg_cnt = 0; - waypointSegments.clear(); - ROS_ASSERT(nh.getParam("segment_cnt", seg_cnt)); - for (int i = 0; i < seg_cnt; ++i) { - load_seg(nh, i, time_base); - if (i > 0) { - ROS_ASSERT(waypointSegments[i - 1].header.stamp < waypointSegments[i].header.stamp); - } - } - ROS_INFO("Overall load %zu segments", waypointSegments.size()); -} - -void publish_waypoints() { - waypoints.header.frame_id = std::string("world"); - waypoints.header.stamp = ros::Time::now(); - pub1.publish(waypoints); - geometry_msgs::PoseStamped init_pose; - init_pose.header = odom.header; - init_pose.pose = odom.pose.pose; - waypoints.poses.insert(waypoints.poses.begin(), init_pose); - // pub2.publish(waypoints); - waypoints.poses.clear(); -} - -void publish_waypoints_vis() { - nav_msgs::Path wp_vis = waypoints; - geometry_msgs::PoseArray poseArray; - poseArray.header.frame_id = std::string("world"); - poseArray.header.stamp = ros::Time::now(); - - { - geometry_msgs::Pose init_pose; - init_pose = odom.pose.pose; - poseArray.poses.push_back(init_pose); - } - - for (auto it = waypoints.poses.begin(); it != waypoints.poses.end(); ++it) { - geometry_msgs::Pose p; - p = it->pose; - poseArray.poses.push_back(p); - } - pub2.publish(poseArray); -} - -void odom_callback(const nav_msgs::Odometry::ConstPtr& msg) { - is_odom_ready = true; - odom = *msg; - - if (waypointSegments.size()) { - ros::Time expected_time = waypointSegments.front().header.stamp; - if (odom.header.stamp >= expected_time) { - waypoints = waypointSegments.front(); - - std::stringstream ss; - ss << bfmt("Series send %.3f from start:\n") % trigged_time.toSec(); - for (auto& pose_stamped : waypoints.poses) { - ss << bfmt("P[%.2f, %.2f, %.2f] q(%.2f,%.2f,%.2f,%.2f)") % - pose_stamped.pose.position.x % pose_stamped.pose.position.y % - pose_stamped.pose.position.z % pose_stamped.pose.orientation.w % - pose_stamped.pose.orientation.x % pose_stamped.pose.orientation.y % - pose_stamped.pose.orientation.z << std::endl; - } - ROS_INFO_STREAM(ss.str()); - - publish_waypoints_vis(); - publish_waypoints(); - - waypointSegments.pop_front(); - } - } -} - -void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) { -/* if (!is_odom_ready) { - ROS_ERROR("[waypoint_generator] No odom!"); - return; - }*/ - - trigged_time = ros::Time::now(); //odom.header.stamp; - //ROS_ASSERT(trigged_time > ros::Time(0)); - - ros::NodeHandle n("~"); - n.param("waypoint_type", waypoint_type, string("manual")); - - if (waypoint_type == string("circle")) { - waypoints = circle(); - publish_waypoints_vis(); - publish_waypoints(); - } else if (waypoint_type == string("eight")) { - waypoints = eight(); - publish_waypoints_vis(); - publish_waypoints(); - } else if (waypoint_type == string("points")) { - waypoints = point(); - publish_waypoints_vis(); - publish_waypoints(); - } else if (waypoint_type == string("series")) { - load_waypoints(n, trigged_time); - } else if (waypoint_type == string("manual-lonely-waypoint")) { - if (msg->pose.position.z > -0.1) { - // if height > 0, it's a valid goal; - geometry_msgs::PoseStamped pt = *msg; - waypoints.poses.clear(); - waypoints.poses.push_back(pt); - publish_waypoints_vis(); - publish_waypoints(); - } else { - ROS_WARN("[waypoint_generator] invalid goal in manual-lonely-waypoint mode."); - } - } else { - if (msg->pose.position.z > 0) { - // if height > 0, it's a normal goal; - geometry_msgs::PoseStamped pt = *msg; - if (waypoint_type == string("noyaw")) { - double yaw = tf::getYaw(odom.pose.pose.orientation); - pt.pose.orientation = tf::createQuaternionMsgFromYaw(yaw); - } - waypoints.poses.push_back(pt); - publish_waypoints_vis(); - } else if (msg->pose.position.z > -1.0) { - // if 0 > height > -1.0, remove last goal; - if (waypoints.poses.size() >= 1) { - waypoints.poses.erase(std::prev(waypoints.poses.end())); - } - publish_waypoints_vis(); - } else { - // if -1.0 > height, end of input - if (waypoints.poses.size() >= 1) { - publish_waypoints_vis(); - publish_waypoints(); - } - } - } -} - -void traj_start_trigger_callback(const geometry_msgs::PoseStamped& msg) { - if (!is_odom_ready) { - ROS_ERROR("[waypoint_generator] No odom!"); - return; - } - - ROS_WARN("[waypoint_generator] Trigger!"); - trigged_time = odom.header.stamp; - ROS_ASSERT(trigged_time > ros::Time(0)); - - ros::NodeHandle n("~"); - n.param("waypoint_type", waypoint_type, string("manual")); - - ROS_ERROR_STREAM("Pattern " << waypoint_type << " generated!"); - if (waypoint_type == string("free")) { - waypoints = point(); - publish_waypoints_vis(); - publish_waypoints(); - } else if (waypoint_type == string("circle")) { - waypoints = circle(); - publish_waypoints_vis(); - publish_waypoints(); - } else if (waypoint_type == string("eight")) { - waypoints = eight(); - publish_waypoints_vis(); - publish_waypoints(); - } else if (waypoint_type == string("point")) { - waypoints = point(); - publish_waypoints_vis(); - publish_waypoints(); - } else if (waypoint_type == string("series")) { - load_waypoints(n, trigged_time); - } -} - -int main(int argc, char** argv) { - ros::init(argc, argv, "waypoint_generator"); - ros::NodeHandle n("~"); - n.param("waypoint_type", waypoint_type, string("manual")); - ros::Subscriber sub1 = n.subscribe("odom", 10, odom_callback); - ros::Subscriber sub2 = n.subscribe("goal", 10, goal_callback); - ros::Subscriber sub3 = n.subscribe("traj_start_trigger", 10, traj_start_trigger_callback); - pub1 = n.advertise("waypoints", 50); - pub2 = n.advertise("waypoints_vis", 10); - - trigged_time = ros::Time(0); - - ros::spin(); - return 0; -} diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/CMakeLists.txt b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/CMakeLists.txt deleted file mode 100644 index fcc912cd..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/CMakeLists.txt +++ /dev/null @@ -1,228 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(my_visualization) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages - -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - quadrotor_msgs -) - -find_package(Eigen3 REQUIRED) - -# find_package(Boost REQUIRED COMPONENTS system) -## System dependencies are found with CMake's conventions - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - LIBRARIES my_visualization -# CATKIN_DEPENDS roscpp rospy std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations - -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Eigen3_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/my_visualization.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/my_visualization_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -add_library( plan_visual - include/my_visualization/plan_visual.h - src/plan_visual.cpp -) -target_link_libraries(plan_visual ${catkin_LIBRARIES}) - -add_executable(obvp_visual src/obvp_visual.cpp) -target_link_libraries(obvp_visual plan_visual) - -add_executable(min_snap_visual src/min_snap_visual.cpp) -target_link_libraries(min_snap_visual plan_visual) - -# add_executable(position_cmd_test src/position_cmd_test.cpp) -# target_link_libraries(position_cmd_test ${catkin_LIBRARIES}) - - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_my_visualization.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/include/my_visualization/plan_visual.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/include/my_visualization/plan_visual.h deleted file mode 100644 index ede173a8..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/include/my_visualization/plan_visual.h +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef _PLAN_VISUAL_H_ -#define _PLAN_VISUAL_H_ - -// rviz\DisplayTypes\Marker: http://wiki.ros.org/rviz/DisplayTypes/Marker#Arrow_.28ARROW.3D0.29 - -#include -#include -#include -#include -#include -#include - -namespace my_planner -{ - class PlanVisual - { - private: - ros::NodeHandle nh; - - ros::Publisher goal_point_pub; - ros::Publisher acc_pub; - ros::Publisher vel_pub; - ros::Publisher jerk_pub; - ros::Publisher traj_pub; - - public: - enum pub_type{VEL, ACC, JERK}; - PlanVisual(){}; - ~PlanVisual(){}; - PlanVisual(ros::NodeHandle &node); - typedef std::shared_ptr Ptr; - void visualInit(ros::NodeHandle &node); - void displayMakerList(ros::Publisher pub, const std::vector &list, Eigen::Vector4d color, const double scale, int id); - void displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id); - void displayArrow(PlanVisual::pub_type type_id, Eigen::Vector3d start, Eigen::Vector3d end, Eigen::Vector4d color, int id); - void displayTraj(std::vector &list, int id); - void deleteAllMarker(); - }; -} - -#endif \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/package.xml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/package.xml deleted file mode 100644 index 141b5394..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/package.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - my_visualization - 0.0.0 - The my_visualization package - - - - - mywang - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - - - - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/min_snap_visual.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/min_snap_visual.cpp deleted file mode 100644 index de217a9b..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/min_snap_visual.cpp +++ /dev/null @@ -1,92 +0,0 @@ -#include -#include -#include -#include -#include -#include "quadrotor_msgs/PolynomialTrajectory.h" -#include "quadrotor_msgs/PositionCommand.h" - -using namespace my_planner; -my_planner::PlanVisual::Ptr visual; -nav_msgs::Odometry odom; - -void goal_visual_cb(const geometry_msgs::PoseArray::ConstPtr &msg) -{ - visual->deleteAllMarker(); - Eigen::Vector3d goalPoint; - for (int i = 0; i < int(msg->poses.size()); i++) - { - goalPoint << msg->poses[i].position.x, msg->poses[i].position.y, msg->poses[i].position.z; - visual->displayGoalPoint(goalPoint, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, i); - } -} - -void pos_cmd_visual_cb(const quadrotor_msgs::PositionCommand::ConstPtr &msg) -{ - Eigen::Vector3d start, end; - start(0) = odom.pose.pose.position.x; - start(1) = odom.pose.pose.position.y; - start(2) = odom.pose.pose.position.z; - - end(0) = start(0) + msg->velocity.x; - end(1) = start(1) + msg->velocity.y; - end(2) = start(2) + msg->velocity.z; - - visual->displayArrow(visual->pub_type::VEL, start, end, Eigen::Vector4d(0.2, 0.8, 0, 1), 0); - - end(0) = start(0) + msg->acceleration.x; - end(1) = start(1) + msg->acceleration.y; - end(2) = start(2) + msg->acceleration.z; - - visual->displayArrow(visual->pub_type::ACC, start, end, Eigen::Vector4d(0.2, 0, 0.8, 1), 0); - - end(0) = start(0) + msg->jerk.x; - end(1) = start(1) + msg->jerk.y; - end(2) = start(2) + msg->jerk.z; - - visual->displayArrow(visual->pub_type::JERK, start, end, Eigen::Vector4d(0.8, 0, 0.2, 1), 0); -} - -void poly_traj_visual_cb(const geometry_msgs::PoseArray::ConstPtr &msg) -{ - std::vector list; - Eigen::Vector3d pt; - - for (int i = 0; i < int(msg->poses.size()); i++) - { - pt(0) = msg->poses[i].position.x; - pt(1) = msg->poses[i].position.y; - pt(2) = msg->poses[i].position.z; - list.push_back(pt); - } - - visual->displayTraj(list, 0); -} - -void odom_cb(const nav_msgs::Odometry::ConstPtr &msg) -{ - odom = *msg; -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "min_snap_visual"); - ros::NodeHandle nh("~"); - - ros::Rate rate(100.0); - - visual.reset(new my_planner::PlanVisual(nh)); - - ros::Subscriber Goal_rviz_sub = nh.subscribe("/goal_list", 10, goal_visual_cb); - ros::Subscriber Pos_cmd_sub = nh.subscribe("/position_cmd", 10, pos_cmd_visual_cb); - ros::Subscriber Odom_sub = nh.subscribe("/odometry", 10, odom_cb); - ros::Subscriber Poly_coef_sub = nh.subscribe("/traj_pts", 10, poly_traj_visual_cb); - - while (ros::ok()) - { - ros::spinOnce(); - rate.sleep(); - } - - return 0; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/obvp_visual.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/obvp_visual.cpp deleted file mode 100644 index 1f2bf5d0..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/obvp_visual.cpp +++ /dev/null @@ -1,90 +0,0 @@ -#include -#include -#include -#include -#include -#include "quadrotor_msgs/PolynomialTrajectory.h" -#include "quadrotor_msgs/PositionCommand.h" - -using namespace my_planner; -my_planner::PlanVisual::Ptr visual; -nav_msgs::Odometry odom; - -void goal_visual_cb(const geometry_msgs::PoseStamped::ConstPtr &msg) -{ - Eigen::Vector3d goalPoint; - goalPoint << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z >= 0 ? msg->pose.position.z : 0; - - visual->displayGoalPoint(goalPoint, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 0); -} - -void pos_cmd_visual_cb(const quadrotor_msgs::PositionCommand::ConstPtr &msg) -{ - Eigen::Vector3d start, end; - start(0) = odom.pose.pose.position.x; - start(1) = odom.pose.pose.position.y; - start(2) = odom.pose.pose.position.z; - - end(0) = start(0) + msg->velocity.x; - end(1) = start(1) + msg->velocity.y; - end(2) = start(2) + msg->velocity.z; - - visual->displayArrow(visual->pub_type::VEL, start, end, Eigen::Vector4d(0.2, 0.8, 0, 1), 0); - - end(0) = start(0) + msg->acceleration.x; - end(1) = start(1) + msg->acceleration.y; - end(2) = start(2) + msg->acceleration.z; - - visual->displayArrow(visual->pub_type::ACC, start, end, Eigen::Vector4d(0.2, 0, 0.8, 1), 0); - - end(0) = start(0) + msg->jerk.x; - end(1) = start(1) + msg->jerk.y; - end(2) = start(2) + msg->jerk.z; - - visual->displayArrow(visual->pub_type::JERK, start, end, Eigen::Vector4d(0.8, 0, 0.2, 1), 0); -} - -void poly_traj_visual_cb(const geometry_msgs::PoseArray::ConstPtr &msg) -{ - std::vector list; - Eigen::Vector3d pt; - - for (int i = 0; i < int(msg->poses.size()); i++) - { - pt(0) = msg->poses[i].position.x; - pt(1) = msg->poses[i].position.y; - pt(2) = msg->poses[i].position.z; - list.push_back(pt); - } - - visual->displayTraj(list, 0); -} - -void odom_cb(const nav_msgs::Odometry::ConstPtr &msg) -{ - odom = *msg; -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "obvp_visual"); - ros::NodeHandle nh("~"); - - ros::Rate rate(100.0); - - visual.reset(new my_planner::PlanVisual(nh)); - - ros::Subscriber Goal_cmd_sub = nh.subscribe("/goal_point1", 10, goal_visual_cb); - ros::Subscriber Goal_rviz_sub = nh.subscribe("/goal_point2", 10, goal_visual_cb); - ros::Subscriber Pos_cmd_sub = nh.subscribe("/position_cmd", 10, pos_cmd_visual_cb); - ros::Subscriber Odom_sub = nh.subscribe("/odometry", 10, odom_cb); - ros::Subscriber Poly_coef_sub = nh.subscribe("/traj_pts", 10, poly_traj_visual_cb); - - while (ros::ok()) - { - ros::spinOnce(); - rate.sleep(); - } - - return 0; -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/plan_visual.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/plan_visual.cpp deleted file mode 100644 index 602fffa7..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/plan_visual.cpp +++ /dev/null @@ -1,162 +0,0 @@ -#include - -namespace my_planner -{ - PlanVisual::PlanVisual(ros::NodeHandle &node) - { - nh = node; - - goal_point_pub = nh.advertise("goal_point", 10); - jerk_pub = nh.advertise("jerk_dir", 10); - acc_pub = nh.advertise("acc_dir", 10); - vel_pub = nh.advertise("vel_dir", 10); - traj_pub = nh.advertise("poly_traj", 10); - ROS_INFO("[Visual]: Init"); - } - - void PlanVisual::visualInit(ros::NodeHandle &node) - { - nh = node; - - goal_point_pub = nh.advertise("goal_point", 10); - jerk_pub = nh.advertise("jerk_dir", 10); - acc_pub = nh.advertise("acc_dir", 10); - vel_pub = nh.advertise("vel_dir", 10); - traj_pub = nh.advertise("poly_traj", 10); - - ROS_INFO("[Visual]: Init"); - } - - void PlanVisual::displayMakerList(ros::Publisher pub, const std::vector &list, Eigen::Vector4d color, const double scale, int id) - { - visualization_msgs::Marker sphere, line_strip; - sphere.header.frame_id = line_strip.header.frame_id = "world"; - sphere.header.stamp = line_strip.header.stamp = ros::Time::now(); - sphere.type = visualization_msgs::Marker::SPHERE_LIST; - line_strip.type = visualization_msgs::Marker::LINE_STRIP; - sphere.action = line_strip.action = visualization_msgs::Marker::ADD; - sphere.id = id; - line_strip.id = id + 1000; - - sphere.pose.orientation.w = line_strip.pose.orientation.w = 1.0; - sphere.color.r = line_strip.color.r = color(0); - sphere.color.g = line_strip.color.g = color(1); - sphere.color.b = line_strip.color.b = color(2); - sphere.color.a = line_strip.color.a = color(3) > 1e-5 ? color(3) : 1.0; - sphere.scale.x = scale; - sphere.scale.y = scale; - sphere.scale.z = scale; - line_strip.scale.x = scale / 2; - - geometry_msgs::Point pt; - - for (int i = 0; i < int(list.size()); i++) - { - pt.x = list[i](0); - pt.y = list[i](1); - pt.z = list[i](2); - sphere.points.push_back(pt); - line_strip.points.push_back(pt); - } - pub.publish(sphere); - pub.publish(line_strip); - } - - void PlanVisual::displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id) - { - visualization_msgs::Marker sphere; - sphere.header.frame_id = "world"; - sphere.header.stamp = ros::Time::now(); - sphere.type = visualization_msgs::Marker::SPHERE; - sphere.action = visualization_msgs::Marker::ADD; - sphere.id = id; - - sphere.pose.orientation.w = 1.0; - sphere.color.r = color(0); - sphere.color.g = color(1); - sphere.color.b = color(2); - sphere.color.a = color(3); - sphere.scale.x = scale; - sphere.scale.y = scale; - sphere.scale.z = scale; - sphere.pose.position.x = goal_point(0); - sphere.pose.position.y = goal_point(1); - sphere.pose.position.z = goal_point(2); - sphere.lifetime = ros::Duration(); - goal_point_pub.publish(sphere); - } - - void PlanVisual::displayArrow(PlanVisual::pub_type type_id, Eigen::Vector3d start, Eigen::Vector3d end, Eigen::Vector4d color, int id) - { - visualization_msgs::Marker sphere; - sphere.header.frame_id = "world"; - sphere.header.stamp = ros::Time::now(); - sphere.type = visualization_msgs::Marker::ARROW; - sphere.action = visualization_msgs::Marker::ADD; - sphere.id = id; - sphere.lifetime = ros::Duration(); - sphere.pose.orientation.w = 1.0; - sphere.color.r = color(0); - sphere.color.g = color(1); - sphere.color.b = color(2); - sphere.color.a = color(3); - sphere.scale.x = 0.1; - sphere.scale.y = 0.2; - sphere.scale.z = 0.1; - - geometry_msgs::Point point; - point.x = start(0); - point.y = start(1); - point.z = start(2); - sphere.points.push_back(point); - point.x = end(0); - point.y = end(1); - point.z = end(2); - sphere.points.push_back(point); - switch (type_id) - { - case VEL: - vel_pub.publish(sphere); - break; - case ACC: - acc_pub.publish(sphere); - break; - case JERK: - jerk_pub.publish(sphere); - } - } - - void PlanVisual::displayTraj(std::vector &list, int id) - { - if (traj_pub.getNumSubscribers() == 0) - { - return; - } - - displayMakerList(traj_pub, list, Eigen::Vector4d(1, 0, 0, 1), 0.15, id); - } - - void PlanVisual::deleteAllMarker() - { - visualization_msgs::Marker sphere; - sphere.header.frame_id = "world"; - sphere.header.stamp = ros::Time::now(); - sphere.type = visualization_msgs::Marker::SPHERE; - sphere.action = visualization_msgs::Marker::DELETEALL; - sphere.id = 0; - - sphere.pose.orientation.w = 1.0; - sphere.color.r = 0; - sphere.color.g = 0; - sphere.color.b = 0; - sphere.color.a = 1; - sphere.scale.x = 0; - sphere.scale.y = 0; - sphere.scale.z = 0; - sphere.pose.position.x = 0; - sphere.pose.position.y = 0; - sphere.pose.position.z = 0; - sphere.lifetime = ros::Duration(); - goal_point_pub.publish(sphere); - } -} \ No newline at end of file diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/CMakeLists.txt b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/CMakeLists.txt deleted file mode 100644 index bb6b0c7a..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/CMakeLists.txt +++ /dev/null @@ -1,237 +0,0 @@ -#cmake_minimum_required(VERSION 2.4.6) -#include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -# Set the build type. Options are: -# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage -# Debug : w/ debug symbols, w/o optimization -# Release : w/o debug symbols, w/ optimization -# RelWithDebInfo : w/ debug symbols, w/ optimization -# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries -#set(ROS_BUILD_TYPE RelWithDebInfo) - -#rosbuild_init() - -#set the default path for built executables to the "bin" directory -#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -#set the default path for built libraries to the "lib" directory -#set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) - -#uncomment if you have defined messages -#rosbuild_genmsg() -#uncomment if you have defined services -#rosbuild_gensrv() - -#common commands for building c++ executables and libraries -#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) -#target_link_libraries(${PROJECT_NAME} another_library) -#rosbuild_add_boost_directories() -#rosbuild_link_boost(${PROJECT_NAME} thread) -#rosbuild_add_executable(example examples/example.cpp) -#target_link_libraries(example ${PROJECT_NAME}) - -#list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) -#find_package(Eigen3 REQUIRED) - -#include_directories(${EIGEN3_INCLUDE_DIR}) - -#rosbuild_add_library(SO3Control src/SO3Control.cpp) -#rosbuild_add_link_flags(SO3Control -Wl,--as-needed) - -#rosbuild_add_library(so3_control_nodelet src/so3_control_nodelet.cpp) -#target_link_libraries(so3_control_nodelet SO3Control) -#rosbuild_add_link_flags(so3_control_nodelet -Wl,--as-needed) - -#---------------------------------- -cmake_minimum_required(VERSION 2.8.3) -project(so3_control) - -set(CMAKE_BUILD_TYPE "Release") -set(CMAKE_CXX_FLAGS "-std=c++11") -set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - nav_msgs - quadrotor_msgs - tf - nodelet - cmake_utils -) -set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependencies might have been -## pulled in transitively but can be declared for certainty nonetheless: -## * add a build_depend tag for "message_generation" -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -#add_message_files( -# FILES -# MultiOccupancyGrid.msg -# MultiSparseMap3D.msg -# SparseMap3D.msg -# VerticalOccupancyGridList.msg - #plan_cmd.msg -#) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -#generate_messages( -# DEPENDENCIES -# geometry_msgs -# nav_msgs -#) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES irobot_msgs -# CATKIN_DEPENDS geometry_msgs nav_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### -#find_package(Armadillo REQUIRED) -#include_directories(${ARMADILLO_INCLUDE_DIRS}) - -find_package(Eigen3 REQUIRED) - -include_directories(${EIGEN3_INCLUDE_DIR}) - - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a cpp library -# add_library(irobot_msgs -# src/${PROJECT_NAME}/irobot_msgs.cpp -# ) - -## Declare a cpp executable -#add_executable(odom_visualization src/odom_visualization.cpp) -add_library(SO3Control src/SO3Control.cpp) -add_library(so3_control_nodelet src/so3_control_nodelet.cpp) - -target_link_libraries(so3_control_nodelet - ${catkin_LIBRARIES} - SO3Control -) - - -add_executable(control_example src/control_example.cpp) -target_link_libraries(control_example -${catkin_LIBRARIES} -) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(multi_map_visualization multi_map_server_messages_cpp) - -## Specify libraries to link a library or executable target against -#target_link_libraries(odom_visualization -# ${catkin_LIBRARIES} -# ${ARMADILLO_LIBRARIES} -# pose_utils -#) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS irobot_msgs irobot_msgs_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_irobot_msgs.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/corrections_hummingbird.yaml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/corrections_hummingbird.yaml deleted file mode 100644 index aeadaab3..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/corrections_hummingbird.yaml +++ /dev/null @@ -1,4 +0,0 @@ -corrections: - z: 0.0 - r: 0.0 - p: 0.0 diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/corrections_pelican.yaml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/corrections_pelican.yaml deleted file mode 100644 index aeadaab3..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/corrections_pelican.yaml +++ /dev/null @@ -1,4 +0,0 @@ -corrections: - z: 0.0 - r: 0.0 - p: 0.0 diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains.yaml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains.yaml deleted file mode 100644 index ffde85a8..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains.yaml +++ /dev/null @@ -1,6 +0,0 @@ -# Gains for Laser-based Pelican -gains: - pos: {x: 5.0, y: 5.0, z: 15.0} - vel: {x: 5.0, y: 5.0, z: 5.0} - rot: {x: 3.5, y: 3.5, z: 1.0} - ang: {x: 0.4, y: 0.4, z: 0.1} diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains_hummingbird.yaml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains_hummingbird.yaml deleted file mode 100644 index a1d016a6..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains_hummingbird.yaml +++ /dev/null @@ -1,6 +0,0 @@ -# Vision Gain for Hummingbird -gains: - pos: {x: 2.0, y: 2.0, z: 3.5} - vel: {x: 1.8, y: 1.8, z: 2.0} - rot: {x: 1.0, y: 1.0, z: 0.3} - ang: {x: 0.07, y: 0.07, z: 0.02} diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains_pelican.yaml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains_pelican.yaml deleted file mode 100644 index ffde85a8..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains_pelican.yaml +++ /dev/null @@ -1,6 +0,0 @@ -# Gains for Laser-based Pelican -gains: - pos: {x: 5.0, y: 5.0, z: 15.0} - vel: {x: 5.0, y: 5.0, z: 5.0} - rot: {x: 3.5, y: 3.5, z: 1.0} - ang: {x: 0.4, y: 0.4, z: 0.1} diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/include/so3_control/SO3Control.h b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/include/so3_control/SO3Control.h deleted file mode 100644 index afc2d959..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/include/so3_control/SO3Control.h +++ /dev/null @@ -1,42 +0,0 @@ -#ifndef __SO3_CONTROL_H__ -#define __SO3_CONTROL_H__ - -#include - -class SO3Control -{ -public: - SO3Control(); - - void setMass(const double mass); - void setGravity(const double g); - void setPosition(const Eigen::Vector3d& position); - void setVelocity(const Eigen::Vector3d& velocity); - void setAcc(const Eigen::Vector3d& acc); - - void calculateControl(const Eigen::Vector3d& des_pos, - const Eigen::Vector3d& des_vel, - const Eigen::Vector3d& des_acc, const double des_yaw, - const double des_yaw_dot, const Eigen::Vector3d& kx, - const Eigen::Vector3d& kv); - - const Eigen::Vector3d& getComputedForce(void); - const Eigen::Quaterniond& getComputedOrientation(void); - - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -private: - // Inputs for the controller - double mass_; - double g_; - Eigen::Vector3d pos_; - Eigen::Vector3d vel_; - Eigen::Vector3d acc_; - - // Outputs of the controller - Eigen::Vector3d force_; - Eigen::Quaterniond orientation_; -}; - -#endif diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/mainpage.dox b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/mainpage.dox deleted file mode 100644 index 101a5b1f..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b so3_control - - - ---> - - -*/ diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/nodelet_plugin.xml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/nodelet_plugin.xml deleted file mode 100644 index af407f6c..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/nodelet_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - so3_control - - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/package.xml b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/package.xml deleted file mode 100644 index c9273a42..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/package.xml +++ /dev/null @@ -1,35 +0,0 @@ - - 0.0.0 - so3_control - - - so3_control - - - Kartik Mohta - BSD - http://ros.org/wiki/so3_control - - - catkin - - roscpp - nav_msgs - quadrotor_msgs - tf - nodelet - cmake_utils - - roscpp - nav_msgs - quadrotor_msgs - tf - nodelet - cmake_utils - - - - - - - diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/SO3Control.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/SO3Control.cpp deleted file mode 100644 index 9d512b87..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/SO3Control.cpp +++ /dev/null @@ -1,124 +0,0 @@ -#include -#include - -#include - -SO3Control::SO3Control() - : mass_(0.5) - , g_(9.81) -{ - acc_.setZero(); -} - -void -SO3Control::setMass(const double mass) -{ - mass_ = mass; -} - -void -SO3Control::setGravity(const double g) -{ - g_ = g; -} - -void -SO3Control::setPosition(const Eigen::Vector3d& position) -{ - pos_ = position; -} - -void -SO3Control::setVelocity(const Eigen::Vector3d& velocity) -{ - vel_ = velocity; -} - -void -SO3Control::calculateControl(const Eigen::Vector3d& des_pos, - const Eigen::Vector3d& des_vel, - const Eigen::Vector3d& des_acc, - const double des_yaw, const double des_yaw_dot, - const Eigen::Vector3d& kx, - const Eigen::Vector3d& kv) -{ - // ROS_INFO("Error %lf %lf %lf", (des_pos - pos_).norm(), - // (des_vel - vel_).norm(), (des_acc - acc_).norm()); - - bool flag_use_pos = !(std::isnan(des_pos(0)) || std::isnan(des_pos(1)) || std::isnan(des_pos(2))); - bool flag_use_vel = !(std::isnan(des_vel(0)) || std::isnan(des_vel(1)) || std::isnan(des_vel(2))); - bool flag_use_acc = !(std::isnan(des_acc(0)) || std::isnan(des_acc(1)) || std::isnan(des_acc(2))); - - Eigen::Vector3d totalError(Eigen::Vector3d::Zero()); - if ( flag_use_pos ) totalError.noalias() += des_pos - pos_; - if ( flag_use_vel ) totalError.noalias() += des_vel - vel_; - if ( flag_use_acc ) totalError.noalias() += des_acc - acc_; - - Eigen::Vector3d ka(fabs(totalError[0]) > 3 ? 0 : (fabs(totalError[0]) * 0.2), - fabs(totalError[1]) > 3 ? 0 : (fabs(totalError[1]) * 0.2), - fabs(totalError[2]) > 3 ? 0 : (fabs(totalError[2]) * 0.2)); - - // std::cout << des_pos.transpose() << std::endl; - // std::cout << des_vel.transpose() << std::endl; - // std::cout << des_acc.transpose() << std::endl; - // std::cout << des_yaw << std::endl; - // std::cout << pos_.transpose() << std::endl; - // std::cout << vel_.transpose() << std::endl; - // std::cout << acc_.transpose() << std::endl; - - - force_ = mass_ * g_ * Eigen::Vector3d(0, 0, 1); - if ( flag_use_pos ) force_.noalias() += kx.asDiagonal() * (des_pos - pos_); - if ( flag_use_vel ) force_.noalias() += kv.asDiagonal() * (des_vel - vel_); - if ( flag_use_acc ) force_.noalias() += mass_ * ka.asDiagonal() * (des_acc - acc_) + mass_ * (des_acc); - - // Limit control angle to 45 degree - double theta = M_PI / 2; - double c = cos(theta); - Eigen::Vector3d f; - f.noalias() = force_ - mass_ * g_ * Eigen::Vector3d(0, 0, 1); - if (Eigen::Vector3d(0, 0, 1).dot(force_ / force_.norm()) < c) - { - double nf = f.norm(); - double A = c * c * nf * nf - f(2) * f(2); - double B = 2 * (c * c - 1) * f(2) * mass_ * g_; - double C = (c * c - 1) * mass_ * mass_ * g_ * g_; - double s = (-B + sqrt(B * B - 4 * A * C)) / (2 * A); - force_.noalias() = s * f + mass_ * g_ * Eigen::Vector3d(0, 0, 1); - } - // Limit control angle to 45 degree - - Eigen::Vector3d b1c, b2c, b3c; - Eigen::Vector3d b1d(cos(des_yaw), sin(des_yaw), 0); - - if (force_.norm() > 1e-6) - b3c.noalias() = force_.normalized(); - else - b3c.noalias() = Eigen::Vector3d(0, 0, 1); - - b2c.noalias() = b3c.cross(b1d).normalized(); - b1c.noalias() = b2c.cross(b3c).normalized(); - - Eigen::Matrix3d R; - R << b1c, b2c, b3c; - - orientation_ = Eigen::Quaterniond(R); -} - -const Eigen::Vector3d& -SO3Control::getComputedForce(void) -{ - return force_; -} - -const Eigen::Quaterniond& -SO3Control::getComputedOrientation(void) -{ - return orientation_; -} - -void -SO3Control::setAcc(const Eigen::Vector3d& acc) -{ - acc_ = acc; -} diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/control_example.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/control_example.cpp deleted file mode 100644 index 15a1738d..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/control_example.cpp +++ /dev/null @@ -1,78 +0,0 @@ -#include -#include -#include - -int main(int argc, char **argv) -{ - - ros::init(argc, argv, "quad_sim_example"); - ros::NodeHandle nh("~"); - - ros::Publisher cmd_pub = nh.advertise("/position_cmd", 10); - - ros::Duration(2.0).sleep(); - - while (ros::ok()) - { - - /*** example 1: position control ***/ - std::cout << "\033[42m" - << "Position Control to (2,0,1) meters" - << "\033[0m" << std::endl; - for (int i = 0; i < 500; i++) - { - quadrotor_msgs::PositionCommand cmd; - cmd.position.x = 2.0; - cmd.position.y = 0.0; - cmd.position.z = 1.0; - cmd_pub.publish(cmd); - - ros::Duration(0.01).sleep(); - ros::spinOnce(); - } - - /*** example 1: position control ***/ - std::cout << "\033[42m" - << "Velocity Control to (-1,0,0) meters/second" - << "\033[0m" << std::endl; - for (int i = 0; i < 500; i++) - { - quadrotor_msgs::PositionCommand cmd; - cmd.position.x = std::numeric_limits::quiet_NaN(); // lower-order commands must be disabled by nan - cmd.position.y = std::numeric_limits::quiet_NaN(); // lower-order commands must be disabled by nan - cmd.position.z = std::numeric_limits::quiet_NaN(); // lower-order commands must be disabled by nan - cmd.velocity.x = -1.0; - cmd.velocity.y = 0.0; - cmd.velocity.z = 0.0; - cmd_pub.publish(cmd); - - ros::Duration(0.01).sleep(); - ros::spinOnce(); - } - - /*** example 1: accelleration control ***/ - std::cout << "\033[42m" - << "Accelleration Control to (1,0,0) meters/second^2" - << "\033[0m" << std::endl; - for (int i = 0; i < 500; i++) - { - quadrotor_msgs::PositionCommand cmd; - cmd.position.x = std::numeric_limits::quiet_NaN(); // lower-order commands must be disabled by nan - cmd.position.y = std::numeric_limits::quiet_NaN(); // lower-order commands must be disabled by nan - cmd.position.z = std::numeric_limits::quiet_NaN(); // lower-order commands must be disabled by nan - cmd.velocity.x = std::numeric_limits::quiet_NaN(); - cmd.velocity.y = std::numeric_limits::quiet_NaN(); - cmd.velocity.z = std::numeric_limits::quiet_NaN(); - cmd.acceleration.x = 1.0; - cmd.acceleration.y = 0.0; - cmd.acceleration.z = 0.0; - cmd_pub.publish(cmd); - - ros::Duration(0.01).sleep(); - ros::spinOnce(); - } - - } - - return 0; -} diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/so3_control_nodelet.cpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/so3_control_nodelet.cpp deleted file mode 100644 index 8135ed2d..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/so3_control_nodelet.cpp +++ /dev/null @@ -1,245 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class SO3ControlNodelet : public nodelet::Nodelet -{ -public: - SO3ControlNodelet() - : position_cmd_updated_(false) - , position_cmd_init_(false) - , des_yaw_(0) - , des_yaw_dot_(0) - , current_yaw_(0) - , enable_motors_(true) - , // FIXME - use_external_yaw_(false) - { - } - - void onInit(void); - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -private: - void publishSO3Command(void); - void position_cmd_callback( - const quadrotor_msgs::PositionCommand::ConstPtr& cmd); - void odom_callback(const nav_msgs::Odometry::ConstPtr& odom); - void enable_motors_callback(const std_msgs::Bool::ConstPtr& msg); - void corrections_callback(const quadrotor_msgs::Corrections::ConstPtr& msg); - void imu_callback(const sensor_msgs::Imu& imu); - - SO3Control controller_; - ros::Publisher so3_command_pub_; - ros::Subscriber odom_sub_; - ros::Subscriber position_cmd_sub_; - ros::Subscriber enable_motors_sub_; - ros::Subscriber corrections_sub_; - ros::Subscriber imu_sub_; - - bool position_cmd_updated_, position_cmd_init_; - std::string frame_id_; - - Eigen::Vector3d des_pos_, des_vel_, des_acc_, kx_, kv_; - double des_yaw_, des_yaw_dot_; - double current_yaw_; - bool enable_motors_; - bool use_external_yaw_; - double kR_[3], kOm_[3], corrections_[3]; - double init_x_, init_y_, init_z_; -}; - -void -SO3ControlNodelet::publishSO3Command(void) -{ - controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_yaw_, - des_yaw_dot_, kx_, kv_); - - const Eigen::Vector3d& force = controller_.getComputedForce(); - const Eigen::Quaterniond& orientation = controller_.getComputedOrientation(); - - quadrotor_msgs::SO3Command::Ptr so3_command( - new quadrotor_msgs::SO3Command); //! @note memory leak? - so3_command->header.stamp = ros::Time::now(); - so3_command->header.frame_id = frame_id_; - so3_command->force.x = force(0); - so3_command->force.y = force(1); - so3_command->force.z = force(2); - so3_command->orientation.x = orientation.x(); - so3_command->orientation.y = orientation.y(); - so3_command->orientation.z = orientation.z(); - so3_command->orientation.w = orientation.w(); - for (int i = 0; i < 3; i++) - { - so3_command->kR[i] = kR_[i]; - so3_command->kOm[i] = kOm_[i]; - } - so3_command->aux.current_yaw = current_yaw_; - so3_command->aux.kf_correction = corrections_[0]; - so3_command->aux.angle_corrections[0] = corrections_[1]; - so3_command->aux.angle_corrections[1] = corrections_[2]; - so3_command->aux.enable_motors = enable_motors_; - so3_command->aux.use_external_yaw = use_external_yaw_; - so3_command_pub_.publish(so3_command); -} - -void -SO3ControlNodelet::position_cmd_callback( - const quadrotor_msgs::PositionCommand::ConstPtr& cmd) -{ - des_pos_ = Eigen::Vector3d(cmd->position.x, cmd->position.y, cmd->position.z); - des_vel_ = Eigen::Vector3d(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z); - des_acc_ = Eigen::Vector3d(cmd->acceleration.x, cmd->acceleration.y, - cmd->acceleration.z); - - if ( cmd->kx[0] > 1e-5 || cmd->kx[1] > 1e-5 || cmd->kx[2] > 1e-5 ) - { - kx_ = Eigen::Vector3d(cmd->kx[0], cmd->kx[1], cmd->kx[2]); - } - if ( cmd->kv[0] > 1e-5 || cmd->kv[1] > 1e-5 || cmd->kv[2] > 1e-5 ) - { - kv_ = Eigen::Vector3d(cmd->kv[0], cmd->kv[1], cmd->kv[2]); - } - - des_yaw_ = cmd->yaw; - des_yaw_dot_ = cmd->yaw_dot; - position_cmd_updated_ = true; - position_cmd_init_ = true; - - publishSO3Command(); -} - -void -SO3ControlNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr& odom) -{ - const Eigen::Vector3d position(odom->pose.pose.position.x, - odom->pose.pose.position.y, - odom->pose.pose.position.z); - const Eigen::Vector3d velocity(odom->twist.twist.linear.x, - odom->twist.twist.linear.y, - odom->twist.twist.linear.z); - - current_yaw_ = tf::getYaw(odom->pose.pose.orientation); - - controller_.setPosition(position); - controller_.setVelocity(velocity); - - if (position_cmd_init_) - { - // We set position_cmd_updated_ = false and expect that the - // position_cmd_callback would set it to true since typically a position_cmd - // message would follow an odom message. If not, the position_cmd_callback - // hasn't been called and we publish the so3 command ourselves - // TODO: Fallback to hover if position_cmd hasn't been received for some - // time - if (!position_cmd_updated_) - publishSO3Command(); - position_cmd_updated_ = false; - } - else if ( init_z_ > -9999.0 ) - { - des_pos_ = Eigen::Vector3d(init_x_, init_y_, init_z_); - des_vel_ = Eigen::Vector3d(0,0,0); - des_acc_ = Eigen::Vector3d(0,0,0); - publishSO3Command(); - } - -} - -void -SO3ControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPtr& msg) -{ - if (msg->data) - ROS_INFO("Enabling motors"); - else - ROS_INFO("Disabling motors"); - - enable_motors_ = msg->data; -} - -void -SO3ControlNodelet::corrections_callback( - const quadrotor_msgs::Corrections::ConstPtr& msg) -{ - corrections_[0] = msg->kf_correction; - corrections_[1] = msg->angle_corrections[0]; - corrections_[2] = msg->angle_corrections[1]; -} - -void -SO3ControlNodelet::imu_callback(const sensor_msgs::Imu& imu) -{ - const Eigen::Vector3d acc(imu.linear_acceleration.x, - imu.linear_acceleration.y, - imu.linear_acceleration.z); - controller_.setAcc(acc); -} - -void -SO3ControlNodelet::onInit(void) -{ - ros::NodeHandle n(getPrivateNodeHandle()); - - std::string quadrotor_name; - n.param("quadrotor_name", quadrotor_name, std::string("quadrotor")); - frame_id_ = "/" + quadrotor_name; - - double mass; - n.param("mass", mass, 0.5); - controller_.setMass(mass); - - n.param("use_external_yaw", use_external_yaw_, true); - - n.param("gains/rot/x", kR_[0], 1.5); - n.param("gains/rot/y", kR_[1], 1.5); - n.param("gains/rot/z", kR_[2], 1.0); - n.param("gains/ang/x", kOm_[0], 0.13); - n.param("gains/ang/y", kOm_[1], 0.13); - n.param("gains/ang/z", kOm_[2], 0.1); - n.param("gains/kx/x", kx_[0], 5.7); - n.param("gains/kx/y", kx_[1], 5.7); - n.param("gains/kx/z", kx_[2], 6.2); - n.param("gains/kv/x", kv_[0], 3.4); - n.param("gains/kv/y", kv_[1], 3.4); - n.param("gains/kv/z", kv_[2], 4.0); - - n.param("corrections/z", corrections_[0], 0.0); - n.param("corrections/r", corrections_[1], 0.0); - n.param("corrections/p", corrections_[2], 0.0); - - n.param("so3_control/init_state_x", init_x_, 0.0); - n.param("so3_control/init_state_y", init_y_, 0.0); - n.param("so3_control/init_state_z", init_z_, -10000.0); - - so3_command_pub_ = n.advertise("so3_cmd", 10); - - odom_sub_ = n.subscribe("odom", 10, &SO3ControlNodelet::odom_callback, this, - ros::TransportHints().tcpNoDelay()); - position_cmd_sub_ = - n.subscribe("position_cmd", 10, &SO3ControlNodelet::position_cmd_callback, - this, ros::TransportHints().tcpNoDelay()); - - enable_motors_sub_ = - n.subscribe("motors", 2, &SO3ControlNodelet::enable_motors_callback, this, - ros::TransportHints().tcpNoDelay()); - corrections_sub_ = - n.subscribe("corrections", 10, &SO3ControlNodelet::corrections_callback, - this, ros::TransportHints().tcpNoDelay()); - - imu_sub_ = n.subscribe("imu", 10, &SO3ControlNodelet::imu_callback, this, - ros::TransportHints().tcpNoDelay()); -} - -#include -//PLUGINLIB_DECLARE_CLASS(so3_control, SO3ControlNodelet, SO3ControlNodelet, -// nodelet::Nodelet); -PLUGINLIB_EXPORT_CLASS(SO3ControlNodelet, nodelet::Nodelet); diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/CMakeLists.txt b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/CMakeLists.txt deleted file mode 100644 index 6fd4e2fd..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/CMakeLists.txt +++ /dev/null @@ -1,51 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(so3_quadrotor_simulator) - -add_compile_options(-std=c++11) - -set(CMAKE_BUILD_TYPE "Release") -set(CMAKE_CXX_FLAGS "-std=c++11") -set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") - -find_package(catkin REQUIRED COMPONENTS - roscpp - quadrotor_msgs - uav_utils - cmake_utils -) - -########### -## Build ## -########### - -find_package(Eigen3 REQUIRED) - -include_directories(${EIGEN3_INCLUDE_DIR}) -include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/ode) -## -find_package(Armadillo REQUIRED) -include_directories(${ARMADILLO_INCLUDE_DIRS}) - -catkin_package( - INCLUDE_DIRS include -# LIBRARIES irobot_msgs - CATKIN_DEPENDS roscpp quadrotor_msgs uav_utils - DEPENDS Eigen3 system_lib -) - -include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -add_library(quadrotor_dynamics src/dynamics/Quadrotor.cpp) - -## Declare a cpp executable -#add_executable(odom_visualization src/odom_visualization.cpp) -add_executable(quadrotor_simulator_so3 - src/quadrotor_simulator_so3.cpp) - -target_link_libraries(quadrotor_simulator_so3 - ${catkin_LIBRARIES} - quadrotor_dynamics -) diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/config/rviz.rviz b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/config/rviz.rviz deleted file mode 100644 index 0a8060b0..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/config/rviz.rviz +++ /dev/null @@ -1,212 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /UAV1 - Splitter Ratio: 0.5 - Tree Height: 703 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - - /3D Nav Goal1 - Name: Tool Properties - Splitter Ratio: 0.588679016 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 138; 138; 138 - Enabled: true - Line Style: - Line Width: 0.0299999993 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization/robot - Name: UAV - Namespaces: - mesh: true - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 85; 255 - Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 - Line Style: Lines - Line Width: 0.0299999993 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 - Topic: /odom_visualization_1/path - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.100000001 - Class: rviz/Pose - Color: 0; 255; 0 - Enabled: true - Head Length: 0.300000012 - Head Radius: 0.100000001 - Name: Pose - Shaft Length: 1 - Shaft Radius: 0.0500000007 - Shape: Arrow - Topic: /odom_visualization_1/pose - Unreliable: false - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /odom_visualization_1/velocity - Name: Velocity - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 85; 255 - Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 - Line Style: Lines - Line Width: 0.0299999993 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 - Topic: /CircleNode_1/path - Unreliable: false - Value: true - - Class: rviz/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.100000001 - Reference Frame: - Value: true - Enabled: true - Global Options: - Background Color: 89; 89; 89 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - - Class: rviz_plugins/GameLikeInput - RangeX_max: 4 - RangeX_min: -4 - RangeY_max: 2.5 - RangeY_min: -2.5 - RangeZ_max: 1.70000005 - RangeZ_min: 0.800000012 - TopicPoint: point_list - TopicSelect: select_list - TopicSwarm: swarm - - Class: rviz_plugins/Goal3DTool - Topic: goal - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 14.550209 - Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.054024078 - Y: -0.301004827 - Z: 2.69711113 - Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.759796858 - Target Frame: - Value: Orbit (rviz) - Yaw: 4.71485233 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 984 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000016a0000034efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000034e000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005400000003efc0100000002fb0000000800540069006d00650100000000000005400000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003d00000034e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1344 - X: 565 - Y: 24 diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/CHANGELOG b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/CHANGELOG deleted file mode 100644 index b1537eb1..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/CHANGELOG +++ /dev/null @@ -1,9 +0,0 @@ -odeint 2.1 - -* versioning system -* generation functions -* bugfixing - -odeint 2.2 (still running) - -* removing same_size and resize from state_wrapper into separate functions diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/Jamroot b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/Jamroot deleted file mode 100644 index db8bfbce..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/Jamroot +++ /dev/null @@ -1,44 +0,0 @@ -# Copyright 2009 Karsten Ahnert and Mario Mulansky. -# Distributed under the Boost Software License, Version 1.0. (See -# accompanying file LICENSE_1_0.txt or copy at -# http://www.boost.org/LICENSE_1_0.txt) - -import os ; -import modules ; -import path ; - -path-constant BOOST_ROOT : [ os.environ BOOST_ROOT ] ; - -project - : requirements - $(BOOST_ROOT) - clang:-Wno-unused-variable - ; - -# tests, regression tests and examples -build-project libs/numeric/odeint/test ; -build-project libs/numeric/odeint/examples ; - - -# additional tests with external libraries : -# build-project libs/numeric/odeint/test_external/gmp ; -# build-project libs/numeric/odeint/test_external/mkl ; -# build-project libs/numeric/odeint/test_external/gsl ; - - -# docs: -# build-project libs/numeric/odeint/doc ; - - - - - - -###### The following is copied from another sandbox project ##### -###### to get the quickbook and boostbook working ... ##### - -# local boost-root = [ modules.peek : BOOST_ROOT ] ; -# local explore-header-include = $(top)/../.. ; -# use-project /boost/regex : $(boost-root)/libs/regex/build ; - -################################################################## diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/README b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/README deleted file mode 100644 index f34601f5..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/README +++ /dev/null @@ -1 +0,0 @@ -odeint is a highly flexible library for solving ordinary differential equations. diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint.hpp deleted file mode 100644 index 3200fbe1..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint.hpp +++ /dev/null @@ -1,75 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint.hpp - - [begin_description] - Forward include for odeint. Includes nearly everything. - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_HPP_INCLUDED - -#include -#include - -// start with ublas wrapper because we need its specializations before including state_wrapper.hpp -#include - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -#include - -#ifndef __CUDACC__ -/* Bulirsch Stoer with Dense Output does not compile with nvcc - * because of the binomial library used there which relies on unsupported SSE functions - */ -#include -#endif - -#include -#include - -#include - -#include -#include -#include -#include - -/* - * Including this algebra slows down the compilation time - */ -// #include -#include - -#include -#include -#include -#include -#include - -#include - -#include - - -#endif // BOOST_NUMERIC_ODEINT_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/array_algebra.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/array_algebra.hpp deleted file mode 100644 index 096e427f..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/array_algebra.hpp +++ /dev/null @@ -1,265 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/algebra/array_algebra.hpp - - [begin_description] - Algebra for boost::array. Highly specialized for odeint. Const arguments are introduce to work with odeint. - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_ARRAY_ALGEBRA_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_ALGEBRA_ARRAY_ALGEBRA_HPP_INCLUDED - -#include - -namespace boost { -namespace numeric { -namespace odeint { - -struct array_algebra -{ - template< typename T , size_t dim , class Op > - static void for_each1( boost::array< T , dim > &s1 , Op op ) - { - for( size_t i=0 ; i - static void for_each2( boost::array< T1 , dim > &s1 , - const boost::array< T2 , dim > &s2 , Op op ) - { - for( size_t i=0 ; i - static void for_each3( boost::array< T , dim > &s1 , - const boost::array< T , dim > &s2 , - const boost::array< T , dim > &s3 , Op op ) - { - for( size_t i=0 ; i - static void for_each3( boost::array< T , dim > &s1 , - boost::array< T , dim > &s2 , - const boost::array< T , dim > &s3 , Op op ) - { - for( size_t i=0 ; i - static void for_each4( boost::array< T , dim > &s1 , - const boost::array< T , dim > &s2 , - const boost::array< T , dim > &s3 , - const boost::array< T , dim > &s4 , Op op ) - { - for( size_t i=0 ; i - static void for_each5( boost::array< T , dim > &s1 , - const boost::array< T , dim > &s2 , - const boost::array< T , dim > &s3 , - const boost::array< T , dim > &s4 , - const boost::array< T , dim > &s5 , Op op ) - { - for( size_t i=0 ; i - static void for_each6( boost::array< T , dim > &s1 , - const boost::array< T , dim > &s2 , - const boost::array< T , dim > &s3 , - const boost::array< T , dim > &s4 , - const boost::array< T , dim > &s5 , - const boost::array< T , dim > &s6 , Op op ) - { - for( size_t i=0 ; i - static void for_each7( boost::array< T , dim > &s1 , - const boost::array< T , dim > &s2 , - const boost::array< T , dim > &s3 , - const boost::array< T , dim > &s4 , - const boost::array< T , dim > &s5 , - const boost::array< T , dim > &s6 , - const boost::array< T , dim > &s7 , Op op ) - { - for( size_t i=0 ; i - static void for_each8( boost::array< T , dim > &s1 , - const boost::array< T , dim > &s2 , - const boost::array< T , dim > &s3 , - const boost::array< T , dim > &s4 , - const boost::array< T , dim > &s5 , - const boost::array< T , dim > &s6 , - const boost::array< T , dim > &s7 , - const boost::array< T , dim > &s8 , Op op ) - { - for( size_t i=0 ; i - static void for_each9( boost::array< T , dim > &s1 , - const boost::array< T , dim > &s2 , - const boost::array< T , dim > &s3 , - const boost::array< T , dim > &s4 , - const boost::array< T , dim > &s5 , - const boost::array< T , dim > &s6 , - const boost::array< T , dim > &s7 , - const boost::array< T , dim > &s8 , - const boost::array< T , dim > &s9 , Op op ) - { - for( size_t i=0 ; i - static void for_each10( boost::array< T , dim > &s1 , - const boost::array< T , dim > &s2 , - const boost::array< T , dim > &s3 , - const boost::array< T , dim > &s4 , - const boost::array< T , dim > &s5 , - const boost::array< T , dim > &s6 , - const boost::array< T , dim > &s7 , - const boost::array< T , dim > &s8 , - const boost::array< T , dim > &s9 , - const boost::array< T , dim > &s10 , Op op ) - { - for( size_t i=0 ; i - static void for_each11( boost::array< T , dim > &s1 , - const boost::array< T , dim > &s2 , - const boost::array< T , dim > &s3 , - const boost::array< T , dim > &s4 , - const boost::array< T , dim > &s5 , - const boost::array< T , dim > &s6 , - const boost::array< T , dim > &s7 , - const boost::array< T , dim > &s8 , - const boost::array< T , dim > &s9 , - const boost::array< T , dim > &s10 , - const boost::array< T , dim > &s11 , Op op ) - { - for( size_t i=0 ; i - static void for_each12( boost::array< T , dim > &s1 , - const boost::array< T , dim > &s2 , - const boost::array< T , dim > &s3 , - const boost::array< T , dim > &s4 , - const boost::array< T , dim > &s5 , - const boost::array< T , dim > &s6 , - const boost::array< T , dim > &s7 , - const boost::array< T , dim > &s8 , - const boost::array< T , dim > &s9 , - const boost::array< T , dim > &s10 , - const boost::array< T , dim > &s11 , - const boost::array< T , dim > &s12 , Op op ) - { - for( size_t i=0 ; i - static void for_each13( boost::array< T , dim > &s1 , - const boost::array< T , dim > &s2 , - const boost::array< T , dim > &s3 , - const boost::array< T , dim > &s4 , - const boost::array< T , dim > &s5 , - const boost::array< T , dim > &s6 , - const boost::array< T , dim > &s7 , - const boost::array< T , dim > &s8 , - const boost::array< T , dim > &s9 , - const boost::array< T , dim > &s10 , - const boost::array< T , dim > &s11 , - const boost::array< T , dim > &s12 , - const boost::array< T , dim > &s13 , Op op ) - { - for( size_t i=0 ; i - static void for_each14( boost::array< T , dim > &s1 , - const boost::array< T , dim > &s2 , - const boost::array< T , dim > &s3 , - const boost::array< T , dim > &s4 , - const boost::array< T , dim > &s5 , - const boost::array< T , dim > &s6 , - const boost::array< T , dim > &s7 , - const boost::array< T , dim > &s8 , - const boost::array< T , dim > &s9 , - const boost::array< T , dim > &s10 , - const boost::array< T , dim > &s11 , - const boost::array< T , dim > &s12 , - const boost::array< T , dim > &s13 , - const boost::array< T , dim > &s14 , Op op ) - { - for( size_t i=0 ; i - static void for_each15( boost::array< T , dim > &s1 , - const boost::array< T , dim > &s2 , - const boost::array< T , dim > &s3 , - const boost::array< T , dim > &s4 , - const boost::array< T , dim > &s5 , - const boost::array< T , dim > &s6 , - const boost::array< T , dim > &s7 , - const boost::array< T , dim > &s8 , - const boost::array< T , dim > &s9 , - const boost::array< T , dim > &s10 , - const boost::array< T , dim > &s11 , - const boost::array< T , dim > &s12 , - const boost::array< T , dim > &s13 , - const boost::array< T , dim > &s14 , - const boost::array< T , dim > &s15 , Op op ) - { - for( size_t i=0 ; i - static Value reduce( const boost::array< T , dim > &s , Red red , Value init) - { - for( size_t i=0 ; i ... - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_DEFAULT_OPERATIONS_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_ALGEBRA_DEFAULT_OPERATIONS_HPP_INCLUDED - -#include - -#include -#include - -#include - - -namespace boost { -namespace numeric { -namespace odeint { - - - -/* - * Notes: - * - * * the results structs are needed in order to work with fusion_algebra - */ -struct default_operations -{ - - template< class Fac1 = double > - struct scale - { - const Fac1 m_alpha1; - - scale( Fac1 alpha1 ) : m_alpha1( alpha1 ) { } - - template< class T1 > - void operator()( T1 &t1 ) const - { - t1 *= m_alpha1; - } - - typedef void result_type; - }; - - template< class Fac1 = double > - struct scale_sum1 - { - const Fac1 m_alpha1; - - scale_sum1( Fac1 alpha1 ) : m_alpha1( alpha1 ) { } - - template< class T1 , class T2 > - void operator()( T1 &t1 , const T2 &t2 ) const - { - t1 = m_alpha1 * t2; - } - - typedef void result_type; - }; - - - template< class Fac1 = double , class Fac2 = Fac1 > - struct scale_sum2 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - - scale_sum2( Fac1 alpha1 , Fac2 alpha2 ) : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) { } - - template< class T1 , class T2 , class T3 > - void operator()( T1 &t1 , const T2 &t2 , const T3 &t3) const - { - t1 = m_alpha1 * t2 + m_alpha2 * t3; - } - - typedef void result_type; - }; - - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 > - struct scale_sum3 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - - scale_sum3( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) { } - - template< class T1 , class T2 , class T3 , class T4 > - void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 ) const - { - t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4; - } - - typedef void result_type; - }; - - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 > - struct scale_sum4 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - - scale_sum4( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) { } - - template< class T1 , class T2 , class T3 , class T4 , class T5 > - void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5) const - { - t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5; - } - - typedef void result_type; - }; - - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 > - struct scale_sum5 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - const Fac5 m_alpha5; - - scale_sum5( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , Fac5 alpha5 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) { } - - template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 > - void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6) const - { - t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6; - } - - typedef void result_type; - }; - - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 > - struct scale_sum6 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - const Fac5 m_alpha5; - const Fac6 m_alpha6; - - scale_sum6( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , Fac5 alpha5 , Fac6 alpha6 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ){ } - - template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 > - void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 ,const T7 &t7) const - { - t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7; - } - - typedef void result_type; - }; - - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 > - struct scale_sum7 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - const Fac5 m_alpha5; - const Fac6 m_alpha6; - const Fac7 m_alpha7; - - scale_sum7( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , - Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) { } - - template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 > - void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 ) const - { - t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8; - } - - typedef void result_type; - }; - - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 > - struct scale_sum8 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - const Fac5 m_alpha5; - const Fac6 m_alpha6; - const Fac7 m_alpha7; - const Fac8 m_alpha8; - - scale_sum8( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , - Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) { } - - template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 > - void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 ) const - { - t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9; - } - - typedef void result_type; - }; - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 > - struct scale_sum9 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - const Fac5 m_alpha5; - const Fac6 m_alpha6; - const Fac7 m_alpha7; - const Fac8 m_alpha8; - const Fac9 m_alpha9; - - scale_sum9( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , - Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) { } - - template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 > - void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 ) const - { - t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10; - } - - typedef void result_type; - }; - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 , class Fac10 = Fac9 > - struct scale_sum10 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - const Fac5 m_alpha5; - const Fac6 m_alpha6; - const Fac7 m_alpha7; - const Fac8 m_alpha8; - const Fac9 m_alpha9; - const Fac10 m_alpha10; - - scale_sum10( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , - Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 , Fac10 alpha10 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) , m_alpha10( alpha10 ) { } - - template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 , class T11 > - void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 , const T11 &t11 ) const - { - t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10 + m_alpha10 * t11; - } - - typedef void result_type; - }; - - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 , class Fac10 = Fac9 , class Fac11 = Fac10 > - struct scale_sum11 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - const Fac5 m_alpha5; - const Fac6 m_alpha6; - const Fac7 m_alpha7; - const Fac8 m_alpha8; - const Fac9 m_alpha9; - const Fac10 m_alpha10; - const Fac11 m_alpha11; - - scale_sum11( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , - Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 , - Fac10 alpha10 , Fac11 alpha11 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) , m_alpha10( alpha10 ) , m_alpha11( alpha11 ) { } - - template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 , class T11 , class T12 > - void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 , const T11 &t11 , const T12 &t12 ) const - { - t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10 + m_alpha10 * t11 + m_alpha11 * t12; - } - - typedef void result_type; - }; - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 , class Fac10 = Fac9 , class Fac11 = Fac10 , class Fac12 = Fac11 > - struct scale_sum12 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - const Fac5 m_alpha5; - const Fac6 m_alpha6; - const Fac7 m_alpha7; - const Fac8 m_alpha8; - const Fac9 m_alpha9; - const Fac10 m_alpha10; - const Fac11 m_alpha11; - const Fac12 m_alpha12; - - scale_sum12( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , - Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 , - Fac10 alpha10 , Fac11 alpha11 , Fac12 alpha12 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) , m_alpha10( alpha10 ) , m_alpha11( alpha11 ) , m_alpha12( alpha12 ) { } - - template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 , class T11 , class T12 , class T13 > - void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 , const T11 &t11 , const T12 &t12 , const T13 &t13 ) const - { - t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10 + m_alpha10 * t11 + m_alpha11 * t12 + m_alpha12 * t13; - } - - typedef void result_type; - }; - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 , class Fac10 = Fac9 , class Fac11 = Fac10 , class Fac12 = Fac11 , class Fac13 = Fac12 > - struct scale_sum13 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - const Fac5 m_alpha5; - const Fac6 m_alpha6; - const Fac7 m_alpha7; - const Fac8 m_alpha8; - const Fac9 m_alpha9; - const Fac10 m_alpha10; - const Fac11 m_alpha11; - const Fac12 m_alpha12; - const Fac13 m_alpha13; - - scale_sum13( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , - Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 , - Fac10 alpha10 , Fac11 alpha11 , Fac12 alpha12 , Fac13 alpha13 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) , m_alpha10( alpha10 ) , m_alpha11( alpha11 ) , m_alpha12( alpha12 ) , m_alpha13( alpha13 ) { } - - template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 , class T11 , class T12 , class T13 , class T14 > - void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 , const T11 &t11 , const T12 &t12 , const T13 &t13 , const T14 &t14 ) const - { - t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10 + m_alpha10 * t11 + m_alpha11 * t12 + m_alpha12 * t13 + m_alpha13 * t14; - } - - typedef void result_type; - }; - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 , class Fac10 = Fac9 , class Fac11 = Fac10 , class Fac12 = Fac11 , class Fac13 = Fac12 , class Fac14 = Fac13 > - struct scale_sum14 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - const Fac5 m_alpha5; - const Fac6 m_alpha6; - const Fac7 m_alpha7; - const Fac8 m_alpha8; - const Fac9 m_alpha9; - const Fac10 m_alpha10; - const Fac11 m_alpha11; - const Fac12 m_alpha12; - const Fac13 m_alpha13; - const Fac14 m_alpha14; - - scale_sum14( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , - Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 , - Fac10 alpha10 , Fac11 alpha11 , Fac12 alpha12 , Fac13 alpha13 , Fac14 alpha14 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) , m_alpha10( alpha10 ) , m_alpha11( alpha11 ) , m_alpha12( alpha12 ) , m_alpha13( alpha13 ) , m_alpha14( alpha14 ) { } - - template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 , class T11 , class T12 , class T13 , class T14 , class T15 > - void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 , const T11 &t11 , const T12 &t12 , const T13 &t13 , const T14 &t14 , const T15 &t15 ) const - { - t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10 + m_alpha10 * t11 + m_alpha11 * t12 + m_alpha12 * t13 + m_alpha13 * t14 + m_alpha14 * t15; - } - - typedef void result_type; - }; - - template< class Fac1 = double , class Fac2 = Fac1 > - struct scale_sum_swap2 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - - scale_sum_swap2( Fac1 alpha1 , Fac2 alpha2 ) : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) { } - - template< class T1 , class T2 , class T3 > - void operator()( T1 &t1 , T2 &t2 , const T3 &t3) const - { - const T1 tmp( t1 ); - t1 = m_alpha1 * t2 + m_alpha2 * t3; - t2 = tmp; - } - - typedef void result_type; - }; - - /* - * for usage in for_each2 - * - * Works with boost::units by eliminating the unit - */ - template< class Fac1 = double > - struct rel_error - { - const Fac1 m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt; - - rel_error( Fac1 eps_abs , Fac1 eps_rel , Fac1 a_x , Fac1 a_dxdt ) - : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , m_a_dxdt( a_dxdt ) { } - - - template< class T1 , class T2 , class T3 > - void operator()( T3 &t3 , const T1 &t1 , const T2 &t2 ) const - { - using std::abs; - set_unit_value( t3 , abs( get_unit_value( t3 ) ) / ( m_eps_abs + m_eps_rel * ( m_a_x * abs( get_unit_value( t1 ) ) + m_a_dxdt * abs( get_unit_value( t2 ) ) ) ) ); - } - - typedef void result_type; - }; - - - /* - * for usage in for_each3 - * - * used in the controller for the rosenbrock4 method - * - * Works with boost::units by eliminating the unit - */ - template< class Fac1 = double > - struct default_rel_error - { - const Fac1 m_eps_abs , m_eps_rel ; - - default_rel_error( Fac1 eps_abs , Fac1 eps_rel ) - : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) { } - - - /* - * xerr = xerr / ( eps_abs + eps_rel * max( x , x_old ) ) - */ - template< class T1 , class T2 , class T3 > - void operator()( T3 &t3 , const T1 &t1 , const T2 &t2 ) const - { - BOOST_USING_STD_MAX(); - using std::abs; - Fac1 x1 = abs( get_unit_value( t1 ) ) , x2 = abs( get_unit_value( t2 ) ); - set_unit_value( t3 , abs( get_unit_value( t3 ) ) / ( m_eps_abs + m_eps_rel * max BOOST_PREVENT_MACRO_SUBSTITUTION ( x1 , x2 ) ) ); - } - - typedef void result_type; - }; - - - - /* - * for usage in reduce - */ - - template< class Value > - struct maximum - { - template< class Fac1 , class Fac2 > - Value operator()( Fac1 t1 , const Fac2 t2 ) const - { - using std::abs; - Value a1 = abs( get_unit_value( t1 ) ) , a2 = abs( get_unit_value( t2 ) ); - return ( a1 < a2 ) ? a2 : a1 ; - } - - typedef Value result_type; - }; - - - - - - template< class Fac1 = double > - struct rel_error_max - { - const Fac1 m_eps_abs , m_eps_rel; - - rel_error_max( Fac1 eps_abs , Fac1 eps_rel ) - : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) - { } - - template< class Res , class T1 , class T2 , class T3 > - Res operator()( Res r , const T1 &x_old , const T2 &x , const T3 &x_err ) - { - BOOST_USING_STD_MAX(); - using std::abs; - Res tmp = abs( get_unit_value( x_err ) ) / ( m_eps_abs + m_eps_rel * max BOOST_PREVENT_MACRO_SUBSTITUTION ( abs( x_old ) , abs( x ) ) ); - return max BOOST_PREVENT_MACRO_SUBSTITUTION ( r , tmp ); - } - }; - - - template< class Fac1 = double > - struct rel_error_max2 - { - const Fac1 m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt; - - rel_error_max2( Fac1 eps_abs , Fac1 eps_rel , Fac1 a_x , Fac1 a_dxdt ) - : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , m_a_dxdt( a_dxdt ) - { } - - template< class Res , class T1 , class T2 , class T3 , class T4 > - Res operator()( Res r , const T1 &x_old , const T2 &x , const T3 &dxdt_old , const T4 &x_err ) - { - BOOST_USING_STD_MAX(); - using std::abs; - Res tmp = abs( get_unit_value( x_err ) ) / - ( m_eps_abs + m_eps_rel * ( m_a_x * abs( get_unit_value( x_old ) ) + m_a_dxdt * abs( get_unit_value( dxdt_old ) ) ) ); - return max BOOST_PREVENT_MACRO_SUBSTITUTION ( r , tmp ); - } - }; - - - - - template< class Fac1 = double > - struct rel_error_l2 - { - const Fac1 m_eps_abs , m_eps_rel; - - rel_error_l2( Fac1 eps_abs , Fac1 eps_rel ) - : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) - { } - - template< class Res , class T1 , class T2 , class T3 > - Res operator()( Res r , const T1 &x_old , const T2 &x , const T3 &x_err ) - { - BOOST_USING_STD_MAX(); - using std::abs; - Res tmp = abs( get_unit_value( x_err ) ) / ( m_eps_abs + m_eps_rel * max BOOST_PREVENT_MACRO_SUBSTITUTION ( abs( x_old ) , abs( x ) ) ); - return r + tmp * tmp; - } - }; - - - - - template< class Fac1 = double > - struct rel_error_l2_2 - { - const Fac1 m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt; - - rel_error_l2_2( Fac1 eps_abs , Fac1 eps_rel , Fac1 a_x , Fac1 a_dxdt ) - : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , m_a_dxdt( a_dxdt ) - { } - - template< class Res , class T1 , class T2 , class T3 , class T4 > - Res operator()( Res r , const T1 &x_old , const T2 &x , const T3 &dxdt_old , const T4 &x_err ) - { - using std::abs; - Res tmp = abs( get_unit_value( x_err ) ) / - ( m_eps_abs + m_eps_rel * ( m_a_x * abs( get_unit_value( x_old ) ) + m_a_dxdt * abs( get_unit_value( dxdt_old ) ) ) ); - return r + tmp * tmp; - } - }; - - - - - - -}; - - -} // odeint -} // numeric -} // boost - - -#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_DEFAULT_OPERATIONS_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/for_each.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/for_each.hpp deleted file mode 100644 index 8693e845..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/for_each.hpp +++ /dev/null @@ -1,165 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/algebra/detail/for_each.hpp - - [begin_description] - Default for_each implementations. - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_FOR_EACH_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_FOR_EACH_HPP_INCLUDED - -namespace boost { -namespace numeric { -namespace odeint { -namespace detail { - - - template< class Iterator1 , class Operation > - inline void for_each1( Iterator1 first1 , Iterator1 last1 , Operation op ) - { - for( ; first1 != last1 ; ) - op( *first1++ ); - } - - - template< class Iterator1 , class Iterator2 , class Operation > - inline void for_each2( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Operation op ) - { - for( ; first1 != last1 ; ) - op( *first1++ , *first2++ ); - } - - - template< class Iterator1 , class Iterator2 , class Iterator3 , class Operation > - inline void for_each3( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, Operation op ) - { - for( ; first1 != last1 ; ) - op( *first1++ , *first2++ , *first3++ ); - } - - - template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Operation > - inline void for_each4( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, Iterator4 first4, Operation op ) - { - for( ; first1 != last1 ; ) - op( *first1++ , *first2++ , *first3++ , *first4++ ); - } - - - template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Operation > - inline void for_each5( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, - Iterator4 first4, Iterator5 first5, Operation op ) - { - for( ; first1 != last1 ; ) - op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ ); - } - - - template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Operation > - inline void for_each6( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, - Iterator4 first4, Iterator5 first5, Iterator6 first6 , Operation op ) - { - for( ; first1 != last1 ; ) - op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ ); - } - - - template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Operation > - inline void for_each7( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, - Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Operation op ) - { - for( ; first1 != last1 ; ) - op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ ); - } - - template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Operation > - inline void for_each8( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, - Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 , Operation op ) - { - for( ; first1 != last1 ; ) - op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ ); - } - - template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Operation > - inline void for_each9( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, - Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 , - Iterator9 first9 , Operation op ) - { - for( ; first1 != last1 ; ) - op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ ); - } - - template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Operation > - inline void for_each10( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, - Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 , - Iterator9 first9 , Iterator10 first10 , Operation op ) - { - for( ; first1 != last1 ; ) - op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ ); - } - - template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Iterator11 , class Operation > - inline void for_each11( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, - Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 , - Iterator9 first9 , Iterator10 first10 , Iterator11 first11 , Operation op ) - { - for( ; first1 != last1 ; ) - op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ , *first11++ ); - } - - template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Iterator11 , class Iterator12 , class Operation > - inline void for_each12( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, - Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 , - Iterator9 first9 , Iterator10 first10 , Iterator11 first11 , Iterator12 first12 , Operation op ) - { - for( ; first1 != last1 ; ) - op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ , *first11++ , *first12++ ); - } - - template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Iterator11 , class Iterator12 , class Iterator13 , class Operation > - inline void for_each13( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, - Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 , - Iterator9 first9 , Iterator10 first10 , Iterator11 first11 , Iterator12 first12 , Iterator13 first13 , Operation op ) - { - for( ; first1 != last1 ; ) - op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ , *first11++ , *first12++ , *first13++ ); - } - - template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Iterator11 , class Iterator12 , class Iterator13 , class Iterator14 , class Operation > - inline void for_each14( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, - Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 , - Iterator9 first9 , Iterator10 first10 , Iterator11 first11 , Iterator12 first12 , Iterator13 first13 , - Iterator14 first14 , Operation op ) - { - for( ; first1 != last1 ; ) - op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ , *first11++ , *first12++ , *first13++ , *first14++ ); - } - - template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Iterator11 , class Iterator12 , class Iterator13 , class Iterator14 , class Iterator15 , class Operation > - inline void for_each15( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, - Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 , - Iterator9 first9 , Iterator10 first10 , Iterator11 first11 , Iterator12 first12 , Iterator13 first13 , - Iterator14 first14 , Iterator15 first15 , Operation op ) - { - for( ; first1 != last1 ; ) - op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ , *first11++ , *first12++ , *first13++ , *first14++ , *first15++ ); - } - - -} // detail -} // odeint -} // numeric -} // boost - - -#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_FOR_EACH_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/macros.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/macros.hpp deleted file mode 100644 index ef97327c..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/macros.hpp +++ /dev/null @@ -1,43 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/algebra/detail/macros.hpp - - [begin_description] - Some macros for type checking. - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_MACROS_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_MACROS_HPP_INCLUDED - - -//type traits aren't working with nvcc -#ifndef __CUDACC__ -#include -#include - -#define BOOST_ODEINT_CHECK_CONTAINER_TYPE( Type1 , Type2 ) \ - BOOST_STATIC_ASSERT(( boost::is_same< typename boost::remove_const< Type1 >::type , Type2 >::value )) - -#else -//empty macro for nvcc -#define BOOST_ODEINT_CHECK_CONTAINER_TYPE( Type1 , Type2 ) - -#endif // __CUDACC__ - - - -/* -#define BOOST_ODEINT_CHECK_OPERATION_ARITY( Operation , Arity ) \ - BOOST_STATIC_ASSERT(( boost::function_traits< Operation >::arity == Arity )) - */ - -#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_MACROS_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/reduce.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/reduce.hpp deleted file mode 100644 index 247d03df..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/reduce.hpp +++ /dev/null @@ -1,67 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/algebra/detail/reduce.hpp - - [begin_description] - Default reduce implementation. - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_REDUCE_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_REDUCE_HPP_INCLUDED - - -namespace boost { -namespace numeric { -namespace odeint { -namespace detail { - -template< class ValueType , class Iterator1 , class Reduction > -inline ValueType reduce( Iterator1 first1 , Iterator1 last1 , Reduction red, ValueType init) -{ - for( ; first1 != last1 ; ) - init = red( init , *first1++ ); - return init; -} - - -template< class ValueType , class Iterator1 , class Iterator2 , class Reduction > -inline ValueType reduce2( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Reduction red, ValueType init) -{ - for( ; first1 != last1 ; ) - init = red( init , *first1++ , *first2++ ); - return init; -} - -template< class ValueType , class Iterator1 , class Iterator2 , class Iterator3 , class Reduction > -inline ValueType reduce3( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3 , Reduction red, ValueType init) -{ - for( ; first1 != last1 ; ) - init = red( init , *first1++ , *first2++ , *first3++ ); - return init; -} - -template< class ValueType , class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Reduction > -inline ValueType reduce4( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3 , Iterator4 first4 , Reduction red, ValueType init) -{ - for( ; first1 != last1 ; ) - init = red( init , *first1++ , *first2++ , *first3++ , *first4++ ); - return init; -} - - -} // detail -} // odeint -} // numeric -} // boost - - -#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_REDUCE_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/fusion_algebra.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/fusion_algebra.hpp deleted file mode 100644 index 81c4070f..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/fusion_algebra.hpp +++ /dev/null @@ -1,186 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/algebra/fusion_algebra.hpp - - [begin_description] - Algebra for boost::fusion sequences. - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_FUSION_ALGEBRA_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_ALGEBRA_FUSION_ALGEBRA_HPP_INCLUDED - - -#include -#include -#include -#include -#include - -namespace boost { -namespace numeric { -namespace odeint { - - -struct fusion_algebra -{ - template< class S1 , class Op > - static void for_each1( S1 &s1 , Op op ) - { - boost::fusion::for_each( s1 , op ); - }; - - - template< class S1 , class S2 , class Op > - static void for_each2( S1 &s1 , S2 &s2 , Op op ) - { - typedef boost::fusion::vector< S1& , S2& > Sequences; - Sequences sequences( s1 , s2 ); - boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); - } - - - template< class S1 , class S2 , class S3 , class Op > - static void for_each3( S1 &s1 , S2 &s2 , S3 &s3 , Op op ) - { - typedef boost::fusion::vector< S1& , S2& , S3& > Sequences; - Sequences sequences( s1 , s2 , s3 ); - boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); - } - - template< class S1 , class S2 , class S3 , class S4 , class Op > - static void for_each4( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , Op op ) - { - typedef boost::fusion::vector< S1& , S2& , S3& , S4& > Sequences; - Sequences sequences( s1 , s2 , s3 , s4 ); - boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); - } - - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class Op > - static void for_each5( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , Op op ) - { - typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& > Sequences; - Sequences sequences( s1 , s2 , s3 , s4 , s5 ); - boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); - } - - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class Op > - static void for_each6( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , Op op ) - { - typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& > Sequences; - Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 ); - boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); - } - - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class Op > - static void for_each7( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , Op op ) - { - typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& > Sequences; - Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 ); - boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); - } - - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class Op > - static void for_each8( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , Op op ) - { - BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 8 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" ); - typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& > Sequences; - Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 ); - boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class Op > - static void for_each9( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , Op op ) - { - BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 9 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" ); - typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& > Sequences; - Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 ); - boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class Op > - static void for_each10( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , Op op ) - { - BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 10 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" ); - typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& > Sequences; - Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 ); - boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class S11 , class Op > - static void for_each11( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , Op op ) - { - BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 11 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" ); - BOOST_STATIC_ASSERT_MSG( BOOST_RESULT_OF_NUM_ARGS >= 11 , "Macro Parameter BOOST_RESULT_OF_NUM_ARGS to small!" ); - typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& , S11& > Sequences; - Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 ); - boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class Op > - static void for_each12( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , Op op ) - { - BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 12 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" ); - BOOST_STATIC_ASSERT_MSG( BOOST_RESULT_OF_NUM_ARGS >= 12 , "Macro Parameter BOOST_RESULT_OF_NUM_ARGS to small!" ); - typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& , S11& , S12& > Sequences; - Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 ); - boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class Op > - static void for_each13( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , Op op ) - { - BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 13 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" ); - BOOST_STATIC_ASSERT_MSG( BOOST_RESULT_OF_NUM_ARGS >= 13 , "Macro Parameter BOOST_RESULT_OF_NUM_ARGS to small!" ); - typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& , S11& , S12& , S13& > Sequences; - Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 ); - boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class Op > - static void for_each14( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , Op op ) - { - BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 14 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" ); - BOOST_STATIC_ASSERT_MSG( BOOST_RESULT_OF_NUM_ARGS >= 14 , "Macro Parameter BOOST_RESULT_OF_NUM_ARGS to small!" ); - typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& , S11& , S12& , S13& , S14& > Sequences; - Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 , s14 ); - boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class S15 , class Op > - static void for_each15( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , S15 &s15 , Op op ) - { - BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 15 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" ); - BOOST_STATIC_ASSERT_MSG( BOOST_RESULT_OF_NUM_ARGS >= 15 , "Macro Parameter BOOST_RESULT_OF_NUM_ARGS to small!" ); - typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& , S11& , S12& , S13& , S14& , S15& > Sequences; - Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 , s14 , s15 ); - boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); - } - - template< class Value , class S , class Reduction > - static Value reduce( const S &s , Reduction red , Value init) - { - return boost::fusion::accumulate( s , init , red ); - } -}; - - - -} // odeint -} // numeric -} // boost - - -#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_FUSION_ALGEBRA_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/range_algebra.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/range_algebra.hpp deleted file mode 100644 index c54e09bf..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/range_algebra.hpp +++ /dev/null @@ -1,159 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/algebra/range_algebra.hpp - - [begin_description] - Default algebra, which works with the most state types, like vector< double >, boost::array< double >, boost::range. - Internally is uses boost::range to obtain the begin and end iterator of the according sequence. - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_RANGE_ALGEBRA_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_ALGEBRA_RANGE_ALGEBRA_HPP_INCLUDED - -#include -#include - -#include -#include -#include - -namespace boost { -namespace numeric { -namespace odeint { - -struct range_algebra -{ - template< class S1 , class Op > - static void for_each1( S1 &s1 , Op op ) - { - detail::for_each1( boost::begin( s1 ) , boost::end( s1 ) , - op ); - } - - template< class S1 , class S2 , class Op > - static void for_each2( S1 &s1 , S2 &s2 , Op op ) - { - detail::for_each2( boost::begin( s1 ) , boost::end( s1 ) , - boost::begin( s2 ) , op ); - } - - template< class S1 , class S2 , class S3 , class Op > - static void for_each3( S1 &s1 , S2 &s2 , S3 &s3 , Op op ) - { - detail::for_each3( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , op ); - } - - template< class S1 , class S2 , class S3 , class S4 , class Op > - static void for_each4( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , Op op ) - { - detail::for_each4( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , op ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class Op > - static void for_each5( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , Op op ) - { - detail::for_each5( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , op ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class Op > - static void for_each6( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , Op op ) - { - detail::for_each6( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , op ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class Op > - static void for_each7( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , Op op ) - { - detail::for_each7( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , op ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class Op > - static void for_each8( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , Op op ) - { - detail::for_each8( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , op ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class Op > - static void for_each9( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , Op op ) - { - detail::for_each9( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , op ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class Op > - static void for_each10( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , Op op ) - { - detail::for_each10( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , op ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class Op > - static void for_each11( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , Op op ) - { - detail::for_each11( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , boost::begin( s11 ) , op ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class Op > - static void for_each12( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , Op op ) - { - detail::for_each12( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , boost::begin( s11 ) , boost::begin( s12 ) , op ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class Op > - static void for_each13( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , Op op ) - { - detail::for_each13( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , boost::begin( s11 ) , boost::begin( s12 ) , boost::begin( s13 ) , op ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class Op > - static void for_each14( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , Op op ) - { - detail::for_each14( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , boost::begin( s11 ) , boost::begin( s12 ) , boost::begin( s13 ) , boost::begin( s14 ) , op ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class S15 , class Op > - static void for_each15( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , S15 &s15 , Op op ) - { - detail::for_each15( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , boost::begin( s11 ) , boost::begin( s12 ) , boost::begin( s13 ) , boost::begin( s14 ) , boost::begin( s15 ) , op ); - } - - template< class Value , class S , class Red > - static Value reduce( const S &s , Red red , Value init) - { - return detail::reduce( boost::begin( s ) , boost::end( s ) , red , init ); - } - - template< class Value , class S1 , class S2 , class Red > - static Value reduce2( const S1 &s1 , const S2 &s2 , Red red , Value init ) - { - return detail::reduce2( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , red , init ); - } - - template< class Value , class S1 , class S2 , class S3 , class Red > - static Value reduce3( const S1 &s1 , const S2 &s2 , const S3 &s3 , Red red , Value init ) - { - return detail::reduce3( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , red , init ); - } - - template< class Value , class S1 , class S2 , class S3 , class S4 , class Red > - static Value reduce4( const S1 &s1 , const S2 &s2 , const S3 &s3 , const S4 &s4 , Red red , Value init ) - { - return detail::reduce4( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , red , init ); - } - - -}; - -} // odeint -} // numeric -} // boost - - -#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_RANGE_ALGEBRA_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/vector_space_algebra.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/vector_space_algebra.hpp deleted file mode 100644 index 4673b9b2..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/vector_space_algebra.hpp +++ /dev/null @@ -1,156 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/algebra/vector_space_algebra.hpp - - [begin_description] - An algebra for types which have vector space semantics, hence types on which the operators +,-,* are well defined. - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_VECTOR_SPACE_ALGEBRA_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_ALGEBRA_VECTOR_SPACE_ALGEBRA_HPP_INCLUDED - -#include - - -namespace boost { -namespace numeric { -namespace odeint { - - -/* - * This class template has to be overload in order to call vector_space_algebra::reduce - */ -template< class State > struct vector_space_reduce; - -/* - * Example: instantiation for sole doubles - */ -template<> -struct vector_space_reduce< double > -{ - template< class Op > - double operator()( double x , Op op , double init ) const - { - init = op( init , x ); - return init; - } -}; - - -struct vector_space_algebra -{ - template< class S1 , class Op > - static void for_each1( S1 &s1 , Op op ) - { - // ToDo : build checks, that the +-*/ operators are well defined - op( s1 ); - } - - template< class S1 , class S2 , class Op > - static void for_each2( S1 &s1 , S2 &s2 , Op op ) - { - op( s1 , s2 ); - } - - template< class S1 , class S2 , class S3 , class Op > - static void for_each3( S1 &s1 , S2 &s2 , S3 &s3 , Op op ) - { - op( s1 , s2 , s3 ); - } - - template< class S1 , class S2 , class S3 , class S4 , class Op > - static void for_each4( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , Op op ) - { - op( s1 , s2 , s3 , s4 ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class Op > - static void for_each5( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , Op op ) - { - op( s1 , s2 , s3 , s4 , s5 ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class Op > - static void for_each6( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , Op op ) - { - op( s1 , s2 , s3 , s4 , s5 , s6 ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class Op > - static void for_each7( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , Op op ) - { - op( s1 , s2 , s3 , s4 , s5 , s6 , s7 ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class Op > - static void for_each8( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , Op op ) - { - op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class Op > - static void for_each9( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , Op op ) - { - op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class Op > - static void for_each10( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , Op op ) - { - op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class Op > - static void for_each11( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , Op op ) - { - op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class Op > - static void for_each12( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , Op op ) - { - op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class Op > - static void for_each13( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , Op op ) - { - op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class Op > - static void for_each14( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , Op op ) - { - op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 , s14 ); - } - - template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class S15 , class Op > - static void for_each15( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , S15 &s15 , Op op ) - { - op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 , s14 , s15 ); - } - - template< class Value , class S , class Red > - static Value reduce( const S &s , Red red , Value init ) - { - boost::numeric::odeint::vector_space_reduce< S > r; - return r( s , red , init ); - } -}; - - -} // odeint -} // numeric -} // boost - - -#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_VECTOR_SPACE_ALGEBRA_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/config.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/config.hpp deleted file mode 100644 index 0d08982b..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/config.hpp +++ /dev/null @@ -1,47 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/config.hpp - - [begin_description] - Sets configurations for odeint and used libraries. Should be included before any other odeint library - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - -#ifndef BOOST_NUMERIC_ODEINT_CONFIG_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_CONFIG_HPP_INCLUDED - -//increase macro variable to allow rk78 scheme -#ifndef FUSION_MAX_VECTOR_SIZE -#define FUSION_MAX_VECTOR_SIZE 15 -#endif - -/* - * the following definitions are only required if fusion vectors are used as state types - * in the rk78 scheme - * they should be defined by the user if required, see e.g. libs/numeric/examples/harmonic_oscillator_units.cpp - */ -#ifndef BOOST_FUSION_INVOKE_MAX_ARITY -#define BOOST_FUSION_INVOKE_MAX_ARITY 15 -#endif - -#ifndef BOOST_RESULT_OF_NUM_ARGS -#define BOOST_RESULT_OF_NUM_ARGS 15 -#endif -/* - */ - -#include - -#if __cplusplus >= 201103L -#define BOOST_NUMERIC_ODEINT_CXX11 1 -#endif - - -#endif // BOOST_NUMERIC_ODEINT_CONFIG_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/gsl/gsl_wrapper.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/gsl/gsl_wrapper.hpp deleted file mode 100644 index c961a894..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/gsl/gsl_wrapper.hpp +++ /dev/null @@ -1,229 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/external/gsl/gsl_wrapper.hpp - - [begin_description] - Wrapper for gsl_vector. - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_GSL_GSL_WRAPPER_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_EXTERNAL_GSL_GSL_WRAPPER_HPP_INCLUDED - -#include - -#include - -#include -#include -#include - - -#include -#include -#include - -class const_gsl_vector_iterator; - -/* - * defines an iterator for gsl_vector - */ -class gsl_vector_iterator : public boost::iterator_facade< gsl_vector_iterator , double , boost::random_access_traversal_tag > -{ -public : - - gsl_vector_iterator( void ): m_p(0) , m_stride( 0 ) { } - explicit gsl_vector_iterator( gsl_vector *p ) : m_p( p->data ) , m_stride( p->stride ) { } - friend gsl_vector_iterator end_iterator( gsl_vector * ); - -private : - - friend class boost::iterator_core_access; - friend class const_gsl_vector_iterator; - - void increment( void ) { m_p += m_stride; } - void decrement( void ) { m_p -= m_stride; } - void advance( ptrdiff_t n ) { m_p += n*m_stride; } - bool equal( const gsl_vector_iterator &other ) const { return this->m_p == other.m_p; } - bool equal( const const_gsl_vector_iterator &other ) const; - double& dereference( void ) const { return *m_p; } - - double *m_p; - size_t m_stride; -}; - - - -/* - * defines an const iterator for gsl_vector - */ -class const_gsl_vector_iterator : public boost::iterator_facade< const_gsl_vector_iterator , const double , boost::random_access_traversal_tag > -{ -public : - - const_gsl_vector_iterator( void ): m_p(0) , m_stride( 0 ) { } - explicit const_gsl_vector_iterator( const gsl_vector *p ) : m_p( p->data ) , m_stride( p->stride ) { } - const_gsl_vector_iterator( const gsl_vector_iterator &p ) : m_p( p.m_p ) , m_stride( p.m_stride ) { } - -private : - - friend class boost::iterator_core_access; - friend class gsl_vector_iterator; - friend const_gsl_vector_iterator end_iterator( const gsl_vector * ); - - void increment( void ) { m_p += m_stride; } - void decrement( void ) { m_p -= m_stride; } - void advance( ptrdiff_t n ) { m_p += n*m_stride; } - bool equal( const const_gsl_vector_iterator &other ) const { return this->m_p == other.m_p; } - bool equal( const gsl_vector_iterator &other ) const { return this->m_p == other.m_p; } - const double& dereference( void ) const { return *m_p; } - - const double *m_p; - size_t m_stride; -}; - - -bool gsl_vector_iterator::equal( const const_gsl_vector_iterator &other ) const { return this->m_p == other.m_p; } - - -gsl_vector_iterator end_iterator( gsl_vector *x ) -{ - gsl_vector_iterator iter( x ); - iter.m_p += iter.m_stride * x->size; - return iter; -} - -const_gsl_vector_iterator end_iterator( const gsl_vector *x ) -{ - const_gsl_vector_iterator iter( x ); - iter.m_p += iter.m_stride * x->size; - return iter; -} - - - - -namespace boost -{ -template<> -struct range_mutable_iterator< gsl_vector* > -{ - typedef gsl_vector_iterator type; -}; - -template<> -struct range_const_iterator< gsl_vector* > -{ - typedef const_gsl_vector_iterator type; -}; -} // namespace boost - - - - -// template<> -inline gsl_vector_iterator range_begin( gsl_vector *x ) -{ - return gsl_vector_iterator( x ); -} - -// template<> -inline const_gsl_vector_iterator range_begin( const gsl_vector *x ) -{ - return const_gsl_vector_iterator( x ); -} - -// template<> -inline gsl_vector_iterator range_end( gsl_vector *x ) -{ - return end_iterator( x ); -} - -// template<> -inline const_gsl_vector_iterator range_end( const gsl_vector *x ) -{ - return end_iterator( x ); -} - - - - - - - -namespace boost { -namespace numeric { -namespace odeint { - - -template<> -struct is_resizeable< gsl_vector* > -{ - //struct type : public boost::true_type { }; - typedef boost::true_type type; - const static bool value = type::value; -}; - -template <> -struct same_size_impl< gsl_vector* , gsl_vector* > -{ - static bool same_size( const gsl_vector* x , const gsl_vector* y ) - { - return x->size == y->size; - } -}; - -template <> -struct resize_impl< gsl_vector* , gsl_vector* > -{ - static void resize( gsl_vector* x , const gsl_vector* y ) - { - gsl_vector_free( x ); - x = gsl_vector_alloc( y->size ); - } -}; - -template<> -struct state_wrapper< gsl_vector* > -{ - typedef double value_type; - typedef gsl_vector* state_type; - typedef state_wrapper< gsl_vector* > state_wrapper_type; - - state_type m_v; - - state_wrapper( ) - { - m_v = gsl_vector_alloc( 1 ); - } - - state_wrapper( const state_wrapper_type &x ) - { - resize( m_v , x.m_v ); - gsl_vector_memcpy( m_v , x.m_v ); - } - - - ~state_wrapper() - { - gsl_vector_free( m_v ); - } - -}; - -} // odeint -} // numeric -} // boost - - - - -#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_GSL_GSL_WRAPPER_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mkl/mkl_operations.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mkl/mkl_operations.hpp deleted file mode 100644 index c1e08914..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mkl/mkl_operations.hpp +++ /dev/null @@ -1,181 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/external/mkl/mkl_operations.hpp - - [begin_description] - Wrapper classes for intel math kernel library types. - Get a free, non-commercial download of MKL at - http://software.intel.com/en-us/articles/non-commercial-software-download/ - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_MKL_MKL_OPERATIONS_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_EXTERNAL_MKL_MKL_OPERATIONS_HPP_INCLUDED - -#include - -#include -#include - -/* exemplary example for writing bindings to the Intel MKL library - * see test/mkl for how to use mkl with odeint - * this is a quick and dirty implementation showing the general possibility. - * It works only with containers based on double and sequential memory allocation. - */ - -namespace boost { -namespace numeric { -namespace odeint { - -/* only defined for doubles */ -struct mkl_operations -{ - //template< class Fac1 , class Fac2 > struct scale_sum2; - - template< class F1 = double , class F2 = F1 > - struct scale_sum2 - { - typedef double Fac1; - typedef double Fac2; - const Fac1 m_alpha1; - const Fac2 m_alpha2; - - scale_sum2( const Fac1 alpha1 , const Fac2 alpha2 ) : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) { } - - template< class T1 , class T2 , class T3 > - void operator()( T1 &t1 , const T2 &t2 , const T3 &t3) const - { // t1 = m_alpha1 * t2 + m_alpha2 * t3; - // we get Containers that have size() and [i]-access - - const int n = t1.size(); - //boost::numeric::odeint::copy( t1 , t3 ); - if( &(t2[0]) != &(t1[0]) ) - { - cblas_dcopy( n , &(t2[0]) , 1 , &(t1[0]) , 1 ); - } - cblas_dscal( n , m_alpha1 , &(t1[0]) , 1 ); - cblas_daxpy( n , m_alpha2 , &(t3[0]) , 1 , &(t1[0]) , 1 ); - //daxpby( &n , &m_alpha2 , &(t3[0]) , &one , &m_alpha1 , &(t1[0]) , &one ); - } - }; - - template< class F1 = double , class F2 = F1 , class F3 = F2 > - struct scale_sum3 - { - typedef double Fac1; - typedef double Fac2; - typedef double Fac3; - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - - scale_sum3( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) { } - - template< class T1 , class T2 , class T3 , class T4 > - void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 ) const - { // t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4; - // we get Containers that have size() and [i]-access - - const int n = t1.size(); - //boost::numeric::odeint::copy( t1 , t3 ); - if( &(t2[0]) != &(t1[0]) ) - { - cblas_dcopy( n , &(t2[0]) , 1 , &(t1[0]) , 1 ); - } - cblas_dscal( n , m_alpha1 , &(t1[0]) , 1 ); - cblas_daxpy( n , m_alpha2 , &(t3[0]) , 1 , &(t1[0]) , 1 ); - //daxpby( &n , &m_alpha2 , &(t3[0]) , &one , &m_alpha1 , &(t1[0]) , &one ); - cblas_daxpy( n , m_alpha3 , &(t4[0]) , 1 , &(t1[0]) , 1 ); - } - }; - - template< class F1 = double , class F2 = F1 , class F3 = F2 , class F4 = F3 > - struct scale_sum4 - { - typedef double Fac1; - typedef double Fac2; - typedef double Fac3; - typedef double Fac4; - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - - scale_sum4( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 , const Fac4 alpha4 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) { } - - template< class T1 , class T2 , class T3 , class T4 , class T5 > - void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 ) const - { // t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5; - // we get Containers that have size() and [i]-access - - const int n = t1.size(); - //boost::numeric::odeint::copy( t1 , t3 ); - if( &(t2[0]) != &(t1[0]) ) - { - cblas_dcopy( n , &(t2[0]) , 1 , &(t1[0]) , 1 ); - } - - cblas_dscal( n , m_alpha1 , &(t1[0]) , 1 ); - cblas_daxpy( n , m_alpha2 , &(t3[0]) , 1 , &(t1[0]) , 1 ); - //daxpby( &n , &m_alpha2 , &(t3[0]) , &one , &m_alpha1 , &(t1[0]) , &one ); - cblas_daxpy( n , m_alpha3 , &(t4[0]) , 1 , &(t1[0]) , 1 ); - cblas_daxpy( n , m_alpha4 , &(t5[0]) , 1 , &(t1[0]) , 1 ); - } - }; - - - template< class F1 = double , class F2 = F1 , class F3 = F2 , class F4 = F3 , class F5 = F4 > - struct scale_sum5 - { - typedef double Fac1; - typedef double Fac2; - typedef double Fac3; - typedef double Fac4; - typedef double Fac5; - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - const Fac5 m_alpha5; - - scale_sum5( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 , const Fac4 alpha4 , const Fac5 alpha5 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) { } - - template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 > - void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 ) const - { // t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6; - // we get Containers that have size() and [i]-access - - const int n = t1.size(); - //boost::numeric::odeint::copy( t1 , t3 ); - if( &(t2[0]) != &(t1[0]) ) - { - cblas_dcopy( n , &(t2[0]) , 1 , &(t1[0]) , 1 ); - } - - cblas_dscal( n , m_alpha1 , &(t1[0]) , 1 ); - cblas_daxpy( n , m_alpha2 , &(t3[0]) , 1 , &(t1[0]) , 1 ); - //daxpby( &n , &m_alpha2 , &(t3[0]) , &one , &m_alpha1 , &(t1[0]) , &one ); - cblas_daxpy( n , m_alpha3 , &(t4[0]) , 1 , &(t1[0]) , 1 ); - cblas_daxpy( n , m_alpha4 , &(t5[0]) , 1 , &(t1[0]) , 1 ); - cblas_daxpy( n , m_alpha5 , &(t6[0]) , 1 , &(t1[0]) , 1 ); - } - }; - -}; - -} // odeint -} // numeric -} // boost - -#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_MKL_MKL_OPERATIONS_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/implicit_euler_mtl4.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/implicit_euler_mtl4.hpp deleted file mode 100644 index 1e7afe6e..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/implicit_euler_mtl4.hpp +++ /dev/null @@ -1,162 +0,0 @@ -/* -[begin_description] -Modification of the implicit Euler method, works with the MTL4 matrix library only. -[end_description] - -Copyright 2009-2011 Karsten Ahnert -Copyright 2009-2011 Mario Mulansky -Copyright 2012 Andreas Angelopoulos - -Distributed under the Boost Software License, Version 1.0. -(See accompanying file LICENSE_1_0.txt or -copy at http://www.boost.org/LICENSE_1_0.txt) -*/ - - -#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_IMPLICIT_EULER_MTL4_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_EXTERNAL_IMPLICIT_EULER_MTL4_HPP_INCLUDED - - -#include - -#include -#include -#include - -#include - -#include -#include - - - - -namespace boost { -namespace numeric { -namespace odeint { - - -template< class ValueType , class Resizer = initially_resizer > -class implicit_euler_mtl4 -{ - -public: - - typedef ValueType value_type; - typedef value_type time_type; - typedef mtl::dense_vector state_type; - - typedef state_wrapper< state_type > wrapped_state_type; - typedef state_type deriv_type; - typedef state_wrapper< deriv_type > wrapped_deriv_type; - typedef mtl::compressed2D< value_type > matrix_type; - typedef state_wrapper< matrix_type > wrapped_matrix_type; - - typedef Resizer resizer_type; - typedef stepper_tag stepper_category; - - typedef implicit_euler_mtl4< ValueType , Resizer > stepper_type; - - - implicit_euler_mtl4( const value_type epsilon = 1E-6 ) - : m_epsilon( epsilon ) , m_resizer() , - m_dxdt() , m_x() , - m_identity() , m_jacobi() - { } - - - template< class System > - void do_step( System system , state_type &x , time_type t , time_type dt ) - { - typedef typename odeint::unwrap_reference< System >::type system_type; - typedef typename odeint::unwrap_reference< typename system_type::first_type >::type deriv_func_type; - typedef typename odeint::unwrap_reference< typename system_type::second_type >::type jacobi_func_type; - system_type &sys = system; - deriv_func_type &deriv_func = sys.first; - jacobi_func_type &jacobi_func = sys.second; - - m_resizer.adjust_size( x , detail::bind( - &stepper_type::template resize_impl< state_type > , detail::ref( *this ) , detail::_1 ) ); - - m_identity.m_v = 1; - - t += dt; - m_x.m_v = x; - - deriv_func( x , m_dxdt.m_v , t ); - jacobi_func( x , m_jacobi.m_v , t ); - - - m_dxdt.m_v *= -dt; - - m_jacobi.m_v *= dt; - m_jacobi.m_v -= m_identity.m_v ; - - - - // using ilu_0 preconditioning -incomplete LU factorisation - // itl::pc::diagonal L(m_jacobi.m_v); - itl::pc::ilu_0 L( m_jacobi.m_v ); - - solve( m_jacobi.m_v , m_x.m_v , m_dxdt.m_v , L ); - x+= m_x.m_v; - - - } - - - template< class StateType > - void adjust_size( const StateType &x ) - { - resize_impl( x ); - } - - -private: - - - /* - Applying approximate iterative linear solvers - default solver is Biconjugate gradient stabilized method - itl::bicgstab(A, x, b, L, iter); - */ - template < class LinearOperator, class HilbertSpaceX, class HilbertSpaceB, class Preconditioner> - void solve(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b, - const Preconditioner& L, int max_iteractions =500) - { - // Termination criterion: r < 1e-6 * b or N iterations - itl::basic_iteration< double > iter( b , max_iteractions , 1e-6 ); - itl::bicgstab( A , x , b , L , iter ); - - } - - - template< class StateIn > - bool resize_impl( const StateIn &x ) - { - bool resized = false; - resized |= adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable::type() ); - resized |= adjust_size_by_resizeability( m_x , x , typename is_resizeable::type() ); - resized |= adjust_size_by_resizeability( m_identity , x , typename is_resizeable::type() ); - resized |= adjust_size_by_resizeability( m_jacobi , x , typename is_resizeable::type() ); - return resized; - } - - -private: - - value_type m_epsilon; - resizer_type m_resizer; - wrapped_deriv_type m_dxdt; - wrapped_state_type m_x; - wrapped_matrix_type m_identity; - wrapped_matrix_type m_jacobi; -}; - - -} // odeint -} // numeric -} // boost - - -#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_IMPLICIT_EULER_MTL4_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/mtl4_resize.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/mtl4_resize.hpp deleted file mode 100644 index b7f94d1f..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/mtl4_resize.hpp +++ /dev/null @@ -1,133 +0,0 @@ -/* -[begin_description] -Modification of the implicit Euler method, works with the MTL4 matrix library only. -[end_description] - -Copyright 2009-2011 Karsten Ahnert -Copyright 2009-2011 Mario Mulansky -Copyright 2012 Andreas Angelopoulos - -Distributed under the Boost Software License, Version 1.0. -(See accompanying file LICENSE_1_0.txt or -copy at http://www.boost.org/LICENSE_1_0.txt) -*/ - - -#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_MTL4_RESIZE_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_EXTERNAL_MTL4_RESIZE_HPP_INCLUDED - -#include -#include -#include - -#include -#include -#include - - -namespace boost { -namespace numeric { -namespace odeint { - - -template< class Value , class Parameters > -struct is_resizeable< mtl::dense_vector< Value , Parameters > > -{ - typedef boost::true_type type; - const static bool value = type::value; -}; - -template< class Value , class Parameters > -struct is_resizeable< mtl::dense2D< Value , Parameters > > -{ - typedef boost::true_type type; - const static bool value = type::value; -}; - -template< class Value , class Parameters > -struct is_resizeable< mtl::compressed2D< Value , Parameters > > -{ - typedef boost::true_type type; - const static bool value = type::value; -}; - - - - -template< class Value , class Parameters > -struct same_size_impl< mtl::dense_vector< Value , Parameters > , mtl::dense_vector< Value , Parameters > > -{ - static bool same_size( const mtl::dense_vector< Value , Parameters > &v1 , - const mtl::dense_vector< Value , Parameters > &v2 ) - { - return mtl::size( v1 ) == mtl::size( v2 ); - } -}; - -template< class Value , class Parameters > -struct resize_impl< mtl::dense_vector< Value , Parameters > , mtl::dense_vector< Value , Parameters > > -{ - static void resize( mtl::dense_vector< Value , Parameters > &v1 , - const mtl::dense_vector< Value , Parameters > &v2 ) - { - v1.change_dim( mtl::size( v2 ) ); - } -}; - - - -template< class Value , class MatrixParameters , class VectorParameters > -struct same_size_impl< mtl::dense2D< Value , MatrixParameters > , mtl::dense_vector< Value , VectorParameters > > -{ - static bool same_size( const mtl::dense2D< Value , MatrixParameters > &m , - const mtl::dense_vector< Value , VectorParameters > &v ) - { - return ( ( mtl::size( v ) == m.num_cols() ) && ( mtl::size( v ) == m.num_rows() ) ); - } -}; - -template< class Value , class MatrixParameters , class VectorParameters > -struct resize_impl< mtl::dense2D< Value , MatrixParameters > , mtl::dense_vector< Value , VectorParameters > > -{ - static void resize( mtl::dense2D< Value , MatrixParameters > &m , - const mtl::dense_vector< Value , VectorParameters > &v ) - { - m.change_dim( mtl::size( v ) , mtl::size( v ) , false ); - } -}; - - - - -template< class Value , class MatrixParameters , class VectorParameters > -struct same_size_impl< mtl::compressed2D< Value , MatrixParameters > , mtl::dense_vector< Value , VectorParameters > > -{ - static bool same_size( const mtl::compressed2D< Value , MatrixParameters > &m , - const mtl::dense_vector< Value , VectorParameters > &v ) - { - return ( ( mtl::size( v ) == m.num_cols() ) && ( mtl::size( v ) == m.num_rows() ) ); - } -}; - -template< class Value , class MatrixParameters , class VectorParameters > -struct resize_impl< mtl::compressed2D< Value , MatrixParameters > , mtl::dense_vector< Value , VectorParameters > > -{ - static void resize( mtl::compressed2D< Value , MatrixParameters > &m , - const mtl::dense_vector< Value , VectorParameters > &v ) - { - m.change_dim( mtl::size( v ) , mtl::size( v ) ); - } -}; - - - - - - - - -} // namespace odeint -} // namespace numeric -} // namespace boost - -#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_MTL4_RESIZE_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_algebra.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_algebra.hpp deleted file mode 100644 index 0f6b5939..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_algebra.hpp +++ /dev/null @@ -1,198 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/external/thrust/thrust_algebra.hpp - - [begin_description] - An algebra for thrusts device_vectors. - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_ALGEBRA_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_ALGEBRA_HPP_INCLUDED - - -#include -#include -#include - -#include - -namespace boost { -namespace numeric { -namespace odeint { - - - -/** ToDO extend until for_each14 for rk78 */ - -/* - * The const versions are needed for boost.range to work, i.e. - * it allows you to do - * for_each1( make_pair( vec1.begin() , vec1.begin() + 10 ) , op ); - */ - -struct thrust_algebra -{ - template< class StateType , class Operation > - static void for_each1( StateType &s , Operation op ) - { - thrust::for_each( boost::begin(s) , boost::begin(s) , op ); - } - - template< class StateType1 , class StateType2 , class Operation > - static void for_each2( StateType1 &s1 , StateType2 &s2 , Operation op ) - { - thrust::for_each( - thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) , - boost::begin(s2) ) ) , - thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) , - boost::end(s2) ) ) , - op); - } - - template< class StateType1 , class StateType2 , class StateType3 , class Operation > - static void for_each3( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , Operation op ) - { - thrust::for_each( - thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) , - boost::begin(s2) , - boost::begin(s3) ) ) , - thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) , - boost::end(s2) , - boost::end(s3) ) ) , - op); - } - - template< class StateType1 , class StateType2 , class StateType3 , class StateType4 , - class Operation > - static void for_each4( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , StateType4 &s4 , - Operation op ) - { - thrust::for_each( - thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) , - boost::begin(s2) , - boost::begin(s3) , - boost::begin(s4) ) ) , - thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) , - boost::end(s2) , - boost::end(s3) , - boost::end(s4) ) ) , - op); - } - - template< class StateType1 , class StateType2 , class StateType3 , - class StateType4 , class StateType5 ,class Operation > - static void for_each5( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , StateType4 &s4 , - StateType5 &s5 , Operation op ) - { - thrust::for_each( - thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) , - boost::begin(s2) , - boost::begin(s3) , - boost::begin(s4) , - boost::begin(s5) ) ) , - thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) , - boost::end(s2) , - boost::end(s3) , - boost::end(s4) , - boost::end(s5) ) ) , - op); - } - - template< class StateType1 , class StateType2 , class StateType3 , - class StateType4 , class StateType5 , class StateType6 , class Operation > - static void for_each6( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , StateType4 &s4 , - StateType5 &s5 , StateType6 &s6 , Operation op ) - { - thrust::for_each( - thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) , - boost::begin(s2) , - boost::begin(s3) , - boost::begin(s4) , - boost::begin(s5) , - boost::begin(s6) ) ) , - thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) , - boost::end(s2) , - boost::end(s3) , - boost::end(s4) , - boost::end(s5) , - boost::end(s6) ) ) , - op); - } - - template< class StateType1 , class StateType2 , class StateType3 , class StateType4 , - class StateType5 , class StateType6 , class StateType7 , class Operation > - static void for_each7( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , StateType4 &s4 , - StateType5 &s5 , StateType6 &s6 , StateType7 &s7 , Operation op ) - { - thrust::for_each( - thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) , - boost::begin(s2) , - boost::begin(s3) , - boost::begin(s4) , - boost::begin(s5) , - boost::begin(s6) , - boost::begin(s7) ) ) , - thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) , - boost::end(s2) , - boost::end(s3) , - boost::end(s4) , - boost::end(s5) , - boost::end(s6) , - boost::end(s7) ) ) , - op); - } - - template< class StateType1 , class StateType2 , class StateType3 , class StateType4 , - class StateType5 , class StateType6 , class StateType7 , class StateType8 , class Operation > - static void for_each8( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , StateType4 &s4 , - StateType5 &s5 , StateType6 &s6 , StateType7 &s7 , StateType8 &s8 , Operation op ) - { - thrust::for_each( - thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) , - boost::begin(s2) , - boost::begin(s3) , - boost::begin(s4) , - boost::begin(s5) , - boost::begin(s6) , - boost::begin(s7) , - boost::begin(s8) ) ) , - thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) , - boost::end(s2) , - boost::end(s3) , - boost::end(s4) , - boost::end(s5) , - boost::end(s6) , - boost::end(s7) , - boost::end(s8) ) ) , - op); - } - - - template< class Value , class S , class Red > - Value reduce( const S &s , Red red , Value init) - { - return thrust::reduce( boost::begin( s ) , boost::end( s ) , init , red ); - } - - - - -}; - - -} // odeint -} // numeric -} // boost - - - -#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_ALGEBRA_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_operations.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_operations.hpp deleted file mode 100644 index 710657ae..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_operations.hpp +++ /dev/null @@ -1,252 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/external/thrust/thrust_operations.hpp - - [begin_description] - Operations of thrust zipped iterators. Is the counterpart of the thrust_algebra. - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_OPERATIONS_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_OPERATIONS_HPP_INCLUDED - -namespace boost { -namespace numeric { -namespace odeint { - -#include -#include - -/**ToDo extend to scale_sum13 for rk78 */ - -struct thrust_operations -{ - template< class Fac1 = double , class Fac2 = Fac1 > - struct scale_sum2 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - - scale_sum2( const Fac1 alpha1 , const Fac2 alpha2 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) { } - - template< class Tuple > - __host__ __device__ - void operator()( Tuple t ) const - { - thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) + m_alpha2 * thrust::get<2>(t); - } - }; - - template< class Fac1 = double , class Fac2 = Fac1 > - struct scale_sum_swap2 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - - scale_sum_swap2( const Fac1 alpha1 , const Fac2 alpha2 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) { } - - template< class Tuple > - __host__ __device__ - void operator()( Tuple t ) const - { - typename thrust::tuple_element<0,Tuple>::type tmp = thrust::get<0>(t); - thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) + m_alpha2 * thrust::get<2>(t); - thrust::get<1>(t) = tmp; - } - }; - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 > - struct scale_sum3 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - - scale_sum3( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) { } - - template< class Tuple > - __host__ __device__ - void operator()( Tuple t ) const - { - thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) + - m_alpha2 * thrust::get<2>(t) + - m_alpha3 * thrust::get<3>(t); - } - }; - - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 > - struct scale_sum4 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - - scale_sum4( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 , const Fac4 alpha4 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ){ } - - template< class Tuple > - __host__ __device__ - void operator()( Tuple t ) const - { - thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) + - m_alpha2 * thrust::get<2>(t) + - m_alpha3 * thrust::get<3>(t) + - m_alpha4 * thrust::get<4>(t); - } - }; - - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , - class Fac4 = Fac3 , class Fac5 = Fac4 > - struct scale_sum5 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - const Fac5 m_alpha5; - - scale_sum5( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 , - const Fac4 alpha4 , const Fac5 alpha5 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , - m_alpha4( alpha4 ) , m_alpha5( alpha5 ) { } - - template< class Tuple > - __host__ __device__ - void operator()( Tuple t ) const - { - thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) + - m_alpha2 * thrust::get<2>(t) + - m_alpha3 * thrust::get<3>(t) + - m_alpha4 * thrust::get<4>(t) + - m_alpha5 * thrust::get<5>(t); - } - }; - - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , - class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 > - struct scale_sum6 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - const Fac5 m_alpha5; - const Fac6 m_alpha6; - - scale_sum6( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 , - const Fac4 alpha4 , const Fac5 alpha5 , const Fac6 alpha6 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , - m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) { } - - template< class Tuple > - __host__ __device__ - void operator()( Tuple t ) const - { - thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) + - m_alpha2 * thrust::get<2>(t) + - m_alpha3 * thrust::get<3>(t) + - m_alpha4 * thrust::get<4>(t) + - m_alpha5 * thrust::get<5>(t) + - m_alpha6 * thrust::get<6>(t); - } - }; - - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , - class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 > - struct scale_sum7 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - const Fac5 m_alpha5; - const Fac6 m_alpha6; - const Fac7 m_alpha7; - - scale_sum7( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 , - const Fac4 alpha4 , const Fac5 alpha5 , const Fac6 alpha6 , const Fac7 alpha7 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , - m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) { } - - template< class Tuple > - __host__ __device__ - void operator()( Tuple t ) const - { - thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) + - m_alpha2 * thrust::get<2>(t) + - m_alpha3 * thrust::get<3>(t) + - m_alpha4 * thrust::get<4>(t) + - m_alpha5 * thrust::get<5>(t) + - m_alpha6 * thrust::get<6>(t) + - m_alpha7 * thrust::get<7>(t) ; - } - }; - - - - - template< class Fac1 = double > - struct rel_error - { - const Fac1 m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt; - - rel_error( const Fac1 eps_abs , const Fac1 eps_rel , const Fac1 a_x , const Fac1 a_dxdt ) - : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , m_a_dxdt( a_dxdt ) { } - - - template< class Tuple > - __host__ __device__ - void operator()( Tuple t ) const - { - using std::abs; - thrust::get< 0 >( t ) = abs( thrust::get< 0 >( t ) ) / - ( m_eps_abs + m_eps_rel * ( m_a_x * abs( thrust::get< 1 >( t ) + m_a_dxdt * abs( thrust::get< 2 >( t ) ) ) ) ); - } - - typedef void result_type; - }; - - - /* - * for usage in reduce - */ - - template< class Value > - struct maximum - { - template< class Fac1 , class Fac2 > - __host__ __device__ - Value operator()( const Fac1 t1 , const Fac2 t2 ) const - { - using std::max; - return ( abs( t1 ) < abs( t2 ) ) ? t2 : t1 ; - } - - typedef Value result_type; - }; - - -}; - -} // odeint -} // numeric -} // boost - - -#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_OPERATIONS_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_resize.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_resize.hpp deleted file mode 100644 index 843a1003..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_resize.hpp +++ /dev/null @@ -1,119 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/external/thrust/thrust_resize.hpp - - [begin_description] - Enable resizing for thrusts device and host_vector. - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_RESIZE_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_RESIZE_HPP_INCLUDED - - -#include -#include - -#include - -namespace boost { -namespace numeric { -namespace odeint { - -template< class T > -struct is_resizeable< thrust::device_vector< T > > -{ - struct type : public boost::true_type { }; - const static bool value = type::value; -}; - -template< class T > -struct same_size_impl< thrust::device_vector< T > , thrust::device_vector< T > > -{ - static bool same_size( const thrust::device_vector< T > &x , const thrust::device_vector< T > &y ) - { - return x.size() == y.size(); - } -}; - -template< class T > -struct resize_impl< thrust::device_vector< T > , thrust::device_vector< T > > -{ - static void resize( thrust::device_vector< T > &x , const thrust::device_vector< T > &y ) - { - x.resize( y.size() ); - } -}; - - -template< class T > -struct is_resizeable< thrust::host_vector< T > > -{ - struct type : public boost::true_type { }; - const static bool value = type::value; -}; - -template< class T > -struct same_size_impl< thrust::host_vector< T > , thrust::host_vector< T > > -{ - static bool same_size( const thrust::host_vector< T > &x , const thrust::host_vector< T > &y ) - { - return x.size() == y.size(); - } -}; - -template< class T > -struct resize_impl< thrust::host_vector< T > , thrust::host_vector< T > > -{ - static void resize( thrust::host_vector< T > &x , const thrust::host_vector< T > &y ) - { - x.resize( y.size() ); - } -}; - - - -template< class Container1, class Value > -struct copy_impl< Container1 , thrust::device_vector< Value > > -{ - static void copy( const Container1 &from , thrust::device_vector< Value > &to ) - { - thrust::copy( boost::begin( from ) , boost::end( from ) , boost::begin( to ) ); - } -}; - -template< class Value , class Container2 > -struct copy_impl< thrust::device_vector< Value > , Container2 > -{ - static void copy( const thrust::device_vector< Value > &from , Container2 &to ) - { - thrust::copy( boost::begin( from ) , boost::end( from ) , boost::begin( to ) ); - } -}; - -template< class Value > -struct copy_impl< thrust::device_vector< Value > , thrust::device_vector< Value > > -{ - static void copy( const thrust::device_vector< Value > &from , thrust::device_vector< Value > &to ) - { - thrust::copy( boost::begin( from ) , boost::end( from ) , boost::begin( to ) ); - } -}; - - - - -} // odeint -} // numeric -} // boost - - -#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_RESIZE_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/vexcl/vexcl_resize.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/vexcl/vexcl_resize.hpp deleted file mode 100644 index e9cbea31..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/vexcl/vexcl_resize.hpp +++ /dev/null @@ -1,92 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/external/vexcl/vexcl_resize.hpp - - [begin_description] - Enable resizing for vexcl vector and multivector. - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_RESIZE_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_RESIZE_HPP_INCLUDED - -#include - -#include -#include -#include - -namespace boost { -namespace numeric { -namespace odeint { - - - -/* - * specializations for vex::vector< T > - */ -template< typename T > -struct is_resizeable< vex::vector< T > > : boost::true_type { }; - -template< typename T > -struct resize_impl< vex::vector< T > , vex::vector< T > > -{ - static void resize( vex::vector< T > &x1 , const vex::vector< T > &x2 ) - { - x1.resize( x2.queue_list() , x2.size() ); - } -}; - -template< typename T > -struct same_size_impl< vex::vector< T > , vex::vector< T > > -{ - static bool same_size( const vex::vector< T > &x1 , const vex::vector< T > &x2 ) - { - return x1.size() == x2.size(); - } -}; - - - - - -/* - * specializations for vex::multivector< T > - */ -template< typename T , size_t N, bool own > -struct is_resizeable< vex::multivector< T , N , own > > : boost::true_type { }; - -template< typename T , size_t N, bool own > -struct resize_impl< vex::multivector< T , N , own > , vex::multivector< T , N , own > > -{ - static void resize( vex::multivector< T , N , own > &x1 , const vex::multivector< T , N , own > &x2 ) - { - x1.resize( x2.queue_list() , x2.size() ); - } -}; - -template< typename T , size_t N, bool own > -struct same_size_impl< vex::multivector< T , N , own > , vex::multivector< T , N , own > > -{ - static bool same_size( const vex::multivector< T , N , own > &x1 , const vex::multivector< T , N , own > &x2 ) - { - return x1.size() == x2.size(); - } -}; - - -} // namespace odeint -} // namespace numeric -} // namespace boost - - - -#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_RESIZE_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_operations.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_operations.hpp deleted file mode 100644 index d621b35e..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_operations.hpp +++ /dev/null @@ -1,245 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/external/viennacl_operations.hpp - - [begin_description] - ViennaCL operations. - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_OPERATIONS_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_OPERATIONS_HPP_INCLUDED - -#include -#include - -namespace boost { -namespace numeric { -namespace odeint { - - - -struct viennacl_operations -{ - - template< class Fac1 = double , class Fac2 = Fac1 > - struct scale_sum2 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - - scale_sum2( Fac1 alpha1 , Fac2 alpha2 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) - { } - - template< class T1 , class T2 , class T3 > - void operator()( viennacl::vector &v1 , - const viennacl::vector &v2 , - const viennacl::vector &v3 - ) const - { - using namespace viennacl; - - static generator::symbolic_vector <0, T1> sym_v1; - static generator::symbolic_vector <1, T2> sym_v2; - static generator::symbolic_vector <2, T3> sym_v3; - static generator::cpu_symbolic_scalar<3, Fac1> sym_a1; - static generator::cpu_symbolic_scalar<4, Fac2> sym_a2; - - static generator::custom_operation op( - sym_v1 = sym_a1 * sym_v2 - + sym_a2 * sym_v3 - ); - - ocl::enqueue( op(v1, - const_cast< viennacl::vector& >(v2), - const_cast< viennacl::vector& >(v3), - const_cast< Fac1& >(m_alpha1), - const_cast< Fac2& >(m_alpha2) - ) ); - } - - typedef void result_type; - }; - - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 > - struct scale_sum3 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - - scale_sum3( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) - { } - - template< class T1 , class T2 , class T3 , class T4 > - void operator()( viennacl::vector &v1 , - const viennacl::vector &v2 , - const viennacl::vector &v3 , - const viennacl::vector &v4 - ) const - { - using namespace viennacl; - - static generator::symbolic_vector <0, T1> sym_v1; - static generator::symbolic_vector <1, T2> sym_v2; - static generator::symbolic_vector <2, T3> sym_v3; - static generator::symbolic_vector <3, T4> sym_v4; - static generator::cpu_symbolic_scalar<4, Fac1> sym_a1; - static generator::cpu_symbolic_scalar<5, Fac2> sym_a2; - static generator::cpu_symbolic_scalar<6, Fac3> sym_a3; - - static generator::custom_operation op( - sym_v1 = sym_a1 * sym_v2 - + sym_a2 * sym_v3 - + sym_a3 * sym_v4 - ); - - ocl::enqueue( op(v1, - const_cast< viennacl::vector& >(v2), - const_cast< viennacl::vector& >(v3), - const_cast< viennacl::vector& >(v4), - const_cast< Fac1& >(m_alpha1), - const_cast< Fac2& >(m_alpha2), - const_cast< Fac3& >(m_alpha3) - ) ); - } - - typedef void result_type; - }; - - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 > - struct scale_sum4 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - - scale_sum4( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) { } - - template< class T1 , class T2 , class T3 , class T4 , class T5 > - void operator()( viennacl::vector &v1 , - const viennacl::vector &v2 , - const viennacl::vector &v3 , - const viennacl::vector &v4 , - const viennacl::vector &v5 - ) const - { - using namespace viennacl; - - static generator::symbolic_vector <0, T1> sym_v1; - static generator::symbolic_vector <1, T2> sym_v2; - static generator::symbolic_vector <2, T3> sym_v3; - static generator::symbolic_vector <3, T4> sym_v4; - static generator::symbolic_vector <4, T5> sym_v5; - static generator::cpu_symbolic_scalar<5, Fac1> sym_a1; - static generator::cpu_symbolic_scalar<6, Fac2> sym_a2; - static generator::cpu_symbolic_scalar<7, Fac3> sym_a3; - static generator::cpu_symbolic_scalar<8, Fac4> sym_a4; - - static generator::custom_operation op( - sym_v1 = sym_a1 * sym_v2 - + sym_a2 * sym_v3 - + sym_a3 * sym_v4 - + sym_a4 * sym_v5 - ); - - ocl::enqueue( op(v1, - const_cast< viennacl::vector& >(v2), - const_cast< viennacl::vector& >(v3), - const_cast< viennacl::vector& >(v4), - const_cast< viennacl::vector& >(v5), - const_cast< Fac1& >(m_alpha1), - const_cast< Fac2& >(m_alpha2), - const_cast< Fac3& >(m_alpha3), - const_cast< Fac4& >(m_alpha4) - ) ); - } - - typedef void result_type; - }; - - - template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 > - struct scale_sum5 - { - const Fac1 m_alpha1; - const Fac2 m_alpha2; - const Fac3 m_alpha3; - const Fac4 m_alpha4; - const Fac5 m_alpha5; - - scale_sum5( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , Fac5 alpha5 ) - : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) { } - - template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 > - void operator()( viennacl::vector &v1 , - const viennacl::vector &v2 , - const viennacl::vector &v3 , - const viennacl::vector &v4 , - const viennacl::vector &v5 , - const viennacl::vector &v6 - ) const - { - using namespace viennacl; - - static generator::symbolic_vector < 0, T1> sym_v1; - static generator::symbolic_vector < 1, T2> sym_v2; - static generator::symbolic_vector < 2, T3> sym_v3; - static generator::symbolic_vector < 3, T4> sym_v4; - static generator::symbolic_vector < 4, T5> sym_v5; - static generator::symbolic_vector < 5, T6> sym_v6; - static generator::cpu_symbolic_scalar< 6, Fac1> sym_a1; - static generator::cpu_symbolic_scalar< 7, Fac2> sym_a2; - static generator::cpu_symbolic_scalar< 8, Fac3> sym_a3; - static generator::cpu_symbolic_scalar< 9, Fac4> sym_a4; - static generator::cpu_symbolic_scalar<10, Fac5> sym_a5; - - static generator::custom_operation op( - sym_v1 = sym_a1 * sym_v2 - + sym_a2 * sym_v3 - + sym_a3 * sym_v4 - + sym_a4 * sym_v5 - + sym_a5 * sym_v6 - ); - - ocl::enqueue( op(v1, - const_cast< viennacl::vector& >(v2), - const_cast< viennacl::vector& >(v3), - const_cast< viennacl::vector& >(v4), - const_cast< viennacl::vector& >(v5), - const_cast< viennacl::vector& >(v6), - const_cast< Fac1& >(m_alpha1), - const_cast< Fac2& >(m_alpha2), - const_cast< Fac3& >(m_alpha3), - const_cast< Fac4& >(m_alpha4), - const_cast< Fac5& >(m_alpha5) - ) ); - } - - typedef void result_type; - }; - - -}; - - -} // odeint -} // numeric -} // boost - - -#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_OPERATIONS_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_resize.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_resize.hpp deleted file mode 100644 index 677cfce8..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_resize.hpp +++ /dev/null @@ -1,65 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/external/viennacl/viennacl_resize.hpp - - [begin_description] - Enable resizing for viennacl vector. - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_RESIZE_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_RESIZE_HPP_INCLUDED - -#include - -#include -#include -#include - -namespace boost { -namespace numeric { -namespace odeint { - - - -/* - * specializations for viennacl::vector< T > - */ -template< typename T > -struct is_resizeable< viennacl::vector< T > > : boost::true_type { }; - -template< typename T > -struct resize_impl< viennacl::vector< T > , viennacl::vector< T > > -{ - static void resize( viennacl::vector< T > &x1 , const viennacl::vector< T > &x2 ) - { - x1.resize( x2.size() , false ); - } -}; - -template< typename T > -struct same_size_impl< viennacl::vector< T > , viennacl::vector< T > > -{ - static bool same_size( const viennacl::vector< T > &x1 , const viennacl::vector< T > &x2 ) - { - return x1.size() == x2.size(); - } -}; - - - -} // namespace odeint -} // namespace numeric -} // namespace boost - - - -#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_RESIZE_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp deleted file mode 100644 index 071159f7..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp +++ /dev/null @@ -1,154 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp - - [begin_description] - Default Integrate adaptive implementation. - [end_description] - - Copyright 2009-2011 Karsten Ahnert - Copyright 2009-2011 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - - -#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_ADAPTIVE_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_ADAPTIVE_HPP_INCLUDED - -#include - -#include -#include -#include -#include -#include -#include - -#include - - -#include - -namespace boost { -namespace numeric { -namespace odeint { -namespace detail { - -// forward declaration -template< class Stepper , class System , class State , class Time , class Observer> -size_t integrate_const( - Stepper stepper , System system , State &start_state , - Time start_time , Time end_time , Time dt , - Observer observer , stepper_tag ); - -/* - * integrate_adaptive for simple stepper is basically an integrate_const + some last step - */ -template< class Stepper , class System , class State , class Time , class Observer > -size_t integrate_adaptive( - Stepper stepper , System system , State &start_state , - Time start_time , Time end_time , Time dt , - Observer observer , stepper_tag -) -{ - size_t steps = detail::integrate_const( stepper , system , start_state , start_time , - end_time , dt , observer , stepper_tag() ); - Time end = start_time + dt*steps; - if( less_with_sign( end , end_time , dt ) ) - { //make a last step to end exactly at end_time - stepper.do_step( system , start_state , end , end_time - end ); - steps++; - typename odeint::unwrap_reference< Observer >::type &obs = observer; - obs( start_state , end_time ); - } - return steps; -} - - -/* - * classical integrate adaptive - */ -template< class Stepper , class System , class State , class Time , class Observer > -size_t integrate_adaptive( - Stepper stepper , System system , State &start_state , - Time &start_time , Time end_time , Time &dt , - Observer observer , controlled_stepper_tag -) -{ - typename odeint::unwrap_reference< Observer >::type &obs = observer; - - const size_t max_attempts = 1000; - const char *error_string = "Integrate adaptive : Maximal number of iterations reached. A step size could not be found."; - size_t count = 0; - while( less_with_sign( start_time , end_time , dt ) ) - { - obs( start_state , start_time ); - if( less_with_sign( end_time , start_time + dt , dt ) ) - { - dt = end_time - start_time; - } - - size_t trials = 0; - controlled_step_result res = success; - do - { - res = stepper.try_step( system , start_state , start_time , dt ); - ++trials; - } - while( ( res == fail ) && ( trials < max_attempts ) ); - if( trials == max_attempts ) throw std::overflow_error( error_string ); - - ++count; - } - obs( start_state , start_time ); - return count; -} - - -/* - * integrate adaptive for dense output steppers - * - * step size control is used if the stepper supports it - */ -template< class Stepper , class System , class State , class Time , class Observer > -size_t integrate_adaptive( - Stepper stepper , System system , State &start_state , - Time start_time , Time end_time , Time dt , - Observer observer , dense_output_stepper_tag ) -{ - typename odeint::unwrap_reference< Observer >::type &obs = observer; - - size_t count = 0; - stepper.initialize( start_state , start_time , dt ); - - while( less_with_sign( stepper.current_time() , end_time , stepper.current_time_step() ) ) - { - while( less_eq_with_sign( stepper.current_time() + stepper.current_time_step() , - end_time , - stepper.current_time_step() ) ) - { //make sure we don't go beyond the end_time - obs( stepper.current_state() , stepper.current_time() ); - stepper.do_step( system ); - ++count; - } - stepper.initialize( stepper.current_state() , stepper.current_time() , end_time - stepper.current_time() ); - } - obs( stepper.current_state() , stepper.current_time() ); - // overwrite start_state with the final point - boost::numeric::odeint::copy( stepper.current_state() , start_state ); - return count; -} - - - - -} // namespace detail -} // namespace odeint -} // namespace numeric -} // namespace boost - - -#endif // BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_ADAPTIVE_HPP_INCLUDED diff --git a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_const.hpp b/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_const.hpp deleted file mode 100644 index 2eb7ae0a..00000000 --- a/src/无人机端代码/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_const.hpp +++ /dev/null @@ -1,159 +0,0 @@ -/* - [auto_generated] - boost/numeric/odeint/integrate/detail/integrate_const.hpp - - [begin_description] - integrate const implementation - [end_description] - - Copyright 2009-2012 Karsten Ahnert - Copyright 2009-2012 Mario Mulansky - - Distributed under the Boost Software License, Version 1.0. - (See accompanying file LICENSE_1_0.txt or - copy at http://www.boost.org/LICENSE_1_0.txt) - */ - -#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_CONST_HPP_INCLUDED -#define BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_CONST_HPP_INCLUDED - -#include -#include -#include -#include - -#include - -namespace boost { -namespace numeric { -namespace odeint { -namespace detail { - -// forward declaration -template< class Stepper , class System , class State , class Time , class Observer > -size_t integrate_adaptive( - Stepper stepper , System system , State &start_state , - Time &start_time , Time end_time , Time &dt , - Observer observer , controlled_stepper_tag -); - - -template< class Stepper , class System , class State , class Time , class Observer > -size_t integrate_const( - Stepper stepper , System system , State &start_state , - Time start_time , Time end_time , Time dt , - Observer observer , stepper_tag -) -{ - typename odeint::unwrap_reference< Observer >::type &obs = observer; - - Time time = start_time; - int step = 0; - - while( less_eq_with_sign( time+dt , end_time , dt ) ) - { - obs( start_state , time ); - stepper.do_step( system , start_state , time , dt ); - // direct computation of the time avoids error propagation happening when using time += dt - // we need clumsy type analysis to get boost units working here - ++step; - time = start_time + static_cast< typename unit_value_type