diff --git a/src/Prometheus/.gitignore b/src/Prometheus/.gitignore new file mode 100644 index 00000000..56481e16 --- /dev/null +++ b/src/Prometheus/.gitignore @@ -0,0 +1,44 @@ +# 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/Prometheus/.gitmodules b/src/Prometheus/.gitmodules new file mode 100644 index 00000000..36ec78b3 --- /dev/null +++ b/src/Prometheus/.gitmodules @@ -0,0 +1,6 @@ +[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/Prometheus/Experiment/CMakeLists.txt b/src/Prometheus/Experiment/CMakeLists.txt new file mode 100644 index 00000000..bd32d374 --- /dev/null +++ b/src/Prometheus/Experiment/CMakeLists.txt @@ -0,0 +1,118 @@ +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/Prometheus/Experiment/config/px4_config.yaml b/src/Prometheus/Experiment/config/px4_config.yaml new file mode 100644 index 00000000..d9fd0eee --- /dev/null +++ b/src/Prometheus/Experiment/config/px4_config.yaml @@ -0,0 +1,260 @@ +# 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/Prometheus/Experiment/config/px4_pluginlists.yaml b/src/Prometheus/Experiment/config/px4_pluginlists.yaml new file mode 100644 index 00000000..145d2629 --- /dev/null +++ b/src/Prometheus/Experiment/config/px4_pluginlists.yaml @@ -0,0 +1,33 @@ +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/Prometheus/Experiment/config/uav_control_indoor.yaml b/src/Prometheus/Experiment/config/uav_control_indoor.yaml new file mode 100644 index 00000000..4e751727 --- /dev/null +++ b/src/Prometheus/Experiment/config/uav_control_indoor.yaml @@ -0,0 +1,29 @@ +## 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/Prometheus/Experiment/launch/prometheus_experiment.launch b/src/Prometheus/Experiment/launch/prometheus_experiment.launch new file mode 100755 index 00000000..831b026a --- /dev/null +++ b/src/Prometheus/Experiment/launch/prometheus_experiment.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Experiment/package.xml b/src/Prometheus/Experiment/package.xml new file mode 100644 index 00000000..de76d1d5 --- /dev/null +++ b/src/Prometheus/Experiment/package.xml @@ -0,0 +1,29 @@ + + + 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/Prometheus/LICENSE b/src/Prometheus/LICENSE new file mode 100644 index 00000000..7a61a724 --- /dev/null +++ b/src/Prometheus/LICENSE @@ -0,0 +1,60 @@ +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/Prometheus/Modules/common/CMakeLists.txt b/src/Prometheus/Modules/common/CMakeLists.txt new file mode 120000 index 00000000..66dd650a --- /dev/null +++ b/src/Prometheus/Modules/common/CMakeLists.txt @@ -0,0 +1 @@ +/opt/ros/melodic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/src/Prometheus/Modules/common/include/geometry_utils.h b/src/Prometheus/Modules/common/include/geometry_utils.h new file mode 100644 index 00000000..a9b77726 --- /dev/null +++ b/src/Prometheus/Modules/common/include/geometry_utils.h @@ -0,0 +1,248 @@ +#ifndef __GEOMETRY_UTILS_H +#define __GEOMETRY_UTILS_H + +#include + +/* clang-format off */ +namespace geometry_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 && "[geometry_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 geometry_utils +/* clang-format on */ + +#endif diff --git a/src/Prometheus/Modules/common/include/math_utils.h b/src/Prometheus/Modules/common/include/math_utils.h new file mode 100644 index 00000000..d46a57ed --- /dev/null +++ b/src/Prometheus/Modules/common/include/math_utils.h @@ -0,0 +1,127 @@ +#ifndef MATH_UTILS_H +#define MATH_UTILS_H + +#include +#include + +using namespace std; + +// 四元数转欧拉角 +Eigen::Vector3d quaternion_to_rpy2(const Eigen::Quaterniond &q) +{ + // YPR - ZYX + return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse(); +} + +// 从(roll,pitch,yaw)创建四元数 by a 3-2-1 intrinsic Tait-Bryan rotation sequence +Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy) +{ + // YPR - ZYX + return Eigen::Quaterniond( + Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()) + ); +} + +// 将四元数转换至(roll,pitch,yaw) by a 3-2-1 intrinsic Tait-Bryan rotation sequence +// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles +// q0 q1 q2 q3 +// w x y z +Eigen::Vector3d quaternion_to_euler(const Eigen::Quaterniond &q) +{ + float quat[4]; + quat[0] = q.w(); + quat[1] = q.x(); + quat[2] = q.y(); + quat[3] = q.z(); + + Eigen::Vector3d ans; + ans[0] = atan2(2.0 * (quat[3] * quat[2] + quat[0] * quat[1]), 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])); + ans[1] = asin(2.0 * (quat[2] * quat[0] - quat[3] * quat[1])); + ans[2] = atan2(2.0 * (quat[3] * quat[0] + quat[1] * quat[2]), 1.0 - 2.0 * (quat[2] * quat[2] + quat[3] * quat[3])); + return ans; +} + +//旋转矩阵转欧拉角 +void rotation_to_euler(const Eigen::Matrix3d& dcm, Eigen::Vector3d& euler_angle) +{ + double phi_val = atan2(dcm(2, 1), dcm(2, 2)); + double theta_val = asin(-dcm(2, 0)); + double psi_val = atan2(dcm(1, 0), dcm(0, 0)); + double pi = M_PI; + + if (fabs(theta_val - pi / 2.0) < 1.0e-3) { + phi_val = 0.0; + psi_val = atan2(dcm(1, 2), dcm(0, 2)); + + } else if (fabs(theta_val + pi / 2.0) < 1.0e-3) { + phi_val = 0.0; + psi_val = atan2(-dcm(1, 2), -dcm(0, 2)); + } + + euler_angle(0) = phi_val; + euler_angle(1) = theta_val; + euler_angle(2) = psi_val; +} + +//constrain_function +float constrain_function(float data, float Max) +{ + if(abs(data)>Max) + { + return (data > 0) ? Max : -Max; + } + else + { + return data; + } +} + +//constrain_function2 +float constrain_function2(float data, float Min,float Max) +{ + if(data > Max) + { + return Max; + } + else if (data < Min) + { + return Min; + }else + { + return data; + } +} + +//sign_function +float sign_function(float data) +{ + if(data>0) + { + return 1.0; + } + else if(data<0) + { + return -1.0; + } + else if(data == 0) + { + return 0.0; + } +} + +// min function +float min(float data1,float data2) +{ + if(data1>=data2) + { + return data2; + } + else + { + return data1; + } +} + +#endif \ No newline at end of file diff --git a/src/Prometheus/Modules/common/include/printf_utils.h b/src/Prometheus/Modules/common/include/printf_utils.h new file mode 100644 index 00000000..941fbf68 --- /dev/null +++ b/src/Prometheus/Modules/common/include/printf_utils.h @@ -0,0 +1,132 @@ +#ifndef PRINTF_UTILS_H +#define PRINTF_UTILS_H + +#include +#include +#include +#include +#include +using namespace std; + +#define NUM_POINT 2 // 打印小数点 + +#define RED "\033[0;1;31m" +#define GREEN "\033[0;1;32m" +#define YELLOW "\033[0;1;33m" +#define BLUE "\033[0;1;34m" +#define PURPLE "\033[0;1;35m" +#define DEEPGREEN "\033[0;1;36m" +#define WHITE "\033[0;1;37m" + +#define RED_IN_WHITE "\033[0;47;31m" +#define GREEN_IN_WHITE "\033[0;47;32m" +#define YELLOW_IN_WHITE "\033[0;47;33m" + +#define TAIL "\033[0m" + +class Print +{ +public: + Print(float interval = 0, std::string color = TAIL) + : interval(interval), past_ts(std::chrono::system_clock::now()), color(color) + { + //固定的浮点显示 + std::cout.setf(ios::fixed); + // setprecision(n) 设显示小数精度为n位 + std::cout << std::setprecision(2); + //左对齐 + std::cout.setf(ios::left); + // 强制显示小数点 + std::cout.setf(ios::showpoint); + // 强制显示符号 + std::cout.setf(ios::showpos); + }; + + template + void operator()(T &&msg) + { + std::chrono::system_clock::time_point now_ts = std::chrono::system_clock::now(); + auto dt = std::chrono::duration_cast(now_ts - past_ts).count(); + + if (this != s_object_name) + { + std::cout << std::endl; + s_object_name = this; + } + + if (interval >= 0) + { + // std::cout << this->interval << std::endl; + if (dt < this->interval * 1000) + return; + std::cout << color << msg << TAIL << std::endl; + } + else + { + if (dt < 0.1 * 1000) + return; + char now_char; + switch (times) + { + case 0: + now_char = '\\'; + break; + case 1: + now_char = '|'; + break; + case 2: + now_char = '/'; + break; + case 3: + now_char = '-'; + break; + } + times = ++times % 4; + std::cout << color << "\r " << now_char << " " << msg << TAIL << std::flush; + } + this->past_ts = now_ts; + }; + + float interval; + +private: + std::chrono::system_clock::time_point past_ts; + std::string color; + static void *s_object_name; + unsigned int times = 0; +}; + +void *Print::s_object_name = nullptr; + +#define PRINTF_UTILS_CONCAT_(x, y) x##y +#define PRINTF_UTILS_CONCAT(x, y) PRINTF_UTILS_CONCAT_(x, y) + +#define PRINTF_UTILS_PCOUT_(interval, color, msg, sign) \ + static Print PRINTF_UTILS_CONCAT(print, sign)(interval, color); \ + PRINTF_UTILS_CONCAT(print, sign) \ + (msg) + +#define PCOUT(interval, color, msg) PRINTF_UTILS_PCOUT_(interval, color, msg, __LINE__) + +// Example: +// cout << GREEN << "Test for Green text." << TAIL < Hightlight color.\033[0m"); +// 其中41的位置代表字的背景色, 30的位置是代表字的颜色，0 是字的一些特殊属性，0代表默认关闭，一些其他属性如闪烁、下划线等。 + +// ROS_INFO_STREAM_ONCE ("\033[1;32m---->Setting to OFFBOARD Mode....\033[0m");//绿色只打印一次 + +#endif diff --git a/src/Prometheus/Modules/common/prometheus_msgs/CMakeLists.txt b/src/Prometheus/Modules/common/prometheus_msgs/CMakeLists.txt new file mode 100644 index 00000000..3c852583 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/CMakeLists.txt @@ -0,0 +1,92 @@ +cmake_minimum_required(VERSION 2.8.3) +project(prometheus_msgs) + +find_package(catkin REQUIRED COMPONENTS + message_generation + roscpp + geometry_msgs + actionlib_msgs + sensor_msgs + nav_msgs + std_msgs + std_srvs + tf2_ros + tf2_eigen + mavros_msgs +) + +find_package(Boost REQUIRED COMPONENTS system) + +add_message_files( + DIRECTORY msg + FILES + UAVState.msg + MultiUAVState.msg + UAVCommand.msg + UAVControlState.msg + UAVSetup.msg + TextInfo.msg + GlobalAruco.msg + + ArucoInfo.msg + MultiArucoInfo.msg + DetectionInfo.msg + MultiDetectionInfo.msg + BoundingBox.msg + BoundingBoxes.msg + + SwarmCommand.msg + FormationAssign.msg + OffsetPose.msg + GPSData.msg + +#communication + DetectionInfoSub.msg + GimbalControl.msg + GimbalState.msg + MultiDetectionInfoSub.msg + RheaCommunication.msg + RheaGPS.msg + RheaState.msg + VisionDiff.msg + WindowPosition.msg +) + +add_action_files( + DIRECTORY action + FILES + CheckForObjects.action +) + +generate_messages( + DEPENDENCIES + actionlib_msgs + geometry_msgs + sensor_msgs + std_msgs +) + +catkin_package( + CATKIN_DEPENDS + message_runtime + actionlib_msgs + geometry_msgs + sensor_msgs + message_runtime + std_msgs +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) diff --git a/src/Prometheus/Modules/common/prometheus_msgs/action/CheckForObjects.action b/src/Prometheus/Modules/common/prometheus_msgs/action/CheckForObjects.action new file mode 100755 index 00000000..ab6b2b6a --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/action/CheckForObjects.action @@ -0,0 +1,13 @@ +# Check if objects in image + +# Goal definition +int16 id +sensor_msgs/Image image + +--- +# Result definition +int16 id +prometheus_msgs/BoundingBoxes bounding_boxes + +--- +# Feedback definition diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/ArucoInfo.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/ArucoInfo.msg new file mode 100755 index 00000000..afd65e01 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/ArucoInfo.msg @@ -0,0 +1,17 @@ +std_msgs/Header header + +## aruco编码 +int32 aruco_num + +## 是否检测到目标 +bool detected + +## 目标位置-相机坐标系-(x,y,z) +## 从相机往前看，物体在相机右方x为正，下方y为正，前方z为正 +float32[3] position + +## 目标姿态-四元数-(qx,qy,qz,qw) +float32[4] orientation + +## 视线角度-相机系下-(右方x角度为正,下方y角度为正) +float32[2] sight_angle diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/BoundingBox.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/BoundingBox.msg new file mode 100755 index 00000000..9b738721 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/BoundingBox.msg @@ -0,0 +1,8 @@ +# 目标框相关信息 +string Class # 类别 +float64 probability # 置信度 +int64 xmin # 右上角 +int64 ymin +int64 xmax # 坐下角 +int64 ymax + diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/BoundingBoxes.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/BoundingBoxes.msg new file mode 100755 index 00000000..3ec3fe82 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/BoundingBoxes.msg @@ -0,0 +1,3 @@ +Header header +Header image_header +BoundingBox[] bounding_boxes diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/DetectionInfo.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/DetectionInfo.msg new file mode 100644 index 00000000..452abd6a --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/DetectionInfo.msg @@ -0,0 +1,33 @@ +# 目标信息 + +std_msgs/Header header + +## 目标类别名称 +string object_name + +## 是否检测到目标 +bool detected + +## 0表示相机坐标系, 1表示机体坐标系, 2表示惯性坐标系 +int32 frame + +## 目标位置[相机系下：右方x为正，下方y为正，前方z为正] +float32[3] position + +## 目标姿态-欧拉角-(z,y,x) +float32[3] attitude + +## 目标姿态-四元数-(qx,qy,qz,qw) +float32[4] attitude_q + +## 视线角度[相机系下：右方x角度为正，下方y角度为正] +float32[2] sight_angle + +## 像素位置[相机系下：右方x为正，下方y为正] +int32[2] pixel_position + +## 偏航角误差 +float32 yaw_error + +## 类别 +int32 category diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/DetectionInfoSub.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/DetectionInfoSub.msg new file mode 100644 index 00000000..779a4bcd --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/DetectionInfoSub.msg @@ -0,0 +1,9 @@ +#目标框的位置（主要斜对角两个点） +float32 left +float32 top +float32 bot +float32 right + + +## TRACK TARGET(目标框ID) +int32 trackIds diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/FormationAssign.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/FormationAssign.msg new file mode 100644 index 00000000..64f7ccd3 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/FormationAssign.msg @@ -0,0 +1,5 @@ +#队形位置 +geometry_msgs/Point[] formation_poses + +#位置点是否选取 +bool[] assigned \ No newline at end of file diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/GPSData.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/GPSData.msg new file mode 100644 index 00000000..48cba1d6 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/GPSData.msg @@ -0,0 +1,3 @@ +float64 latitude +float64 longitude +float64 altitude \ No newline at end of file diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/GimbalControl.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/GimbalControl.msg new file mode 100644 index 00000000..0b32855a --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/GimbalControl.msg @@ -0,0 +1,33 @@ +Header header +uint8 Id +#control mode 0:nothong 1:angle 2:speed 3:home postion +uint8 rpyMode +uint8 manual = 1 +uint8 home = 2 +uint8 hold = 3 # 不控制 +uint8 fellow = 4 #fellow吊舱跟随无人机移动 + +uint8 roll +uint8 yaw +uint8 pitch + +uint8 noCtl = 0 +uint8 velocityCtl = 1 +uint8 angleCtl = 2 + +float32 rValue # deg 单位 +float32 yValue # deg +float32 pValue # deg + + +# focus +uint8 focusMode # 默认值 +uint8 focusStop = 1 +uint8 focusOut = 2 +uint8 focusIn = 3 + +# zoom +uint8 zoomMode # 默认值 +uint8 zoomStop = 1 +uint8 zoomOut = 2 +uint8 zoomIn = 3 \ No newline at end of file diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/GimbalState.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/GimbalState.msg new file mode 100644 index 00000000..1a579d2f --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/GimbalState.msg @@ -0,0 +1,40 @@ +Header header +uint8 Id + +# 0: 发一句,回一句: 此状态下，相机倍数zoomVal有效，imuAngleVel为估算直 +# 1: 发一句,一直回复: 此状态下，相机倍数zoomVal失效，imuAngleVel为真直 +uint8 feedbackMode + +#mode +# 0: 手动控制 +# 1: home +# 2: tracking +# 3: yaw follow 吊舱跟随无人机移动 +# 4: hold 吊舱不跟随无人机 +# 5: search 自动移动旋转 +uint8 mode + +# 是否视频录制 +bool isRecording + +# 是否开启自动缩放(VisionDiff需要指定面积才能生效) +# 0: 保持 +# 1: 放大 +# 2: 缩小 +# 3: 自动 +uint8 zoomState + +# 当前所处倍数 +float32 zoomVal + +#roll,pitch,yaw +float32[3] imuAngle + +#Current gimbal joint angles(roll,pitch,yaw), published at 30 Hz. +float32[3] rotorAngle + +# rpy_vel 角速度 +float32[3] imuAngleVel + +# rpy_tgt 目标角度 +float32[3] rotorAngleTarget \ No newline at end of file diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/GlobalAruco.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/GlobalAruco.msg new file mode 100755 index 00000000..328af1b1 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/GlobalAruco.msg @@ -0,0 +1,12 @@ + +std_msgs/Header header + +ArucoInfo Aruco1 +ArucoInfo Aruco2 +ArucoInfo Aruco3 +ArucoInfo Aruco4 +ArucoInfo Aruco5 +ArucoInfo Aruco6 +ArucoInfo Aruco7 +ArucoInfo Aruco8 +ArucoInfo Aruco9 diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/MultiArucoInfo.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/MultiArucoInfo.msg new file mode 100755 index 00000000..47310801 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/MultiArucoInfo.msg @@ -0,0 +1,7 @@ +std_msgs/Header header + +## 检测到的aruco码数量 +int32 num_arucos + +## 每个aruco码的检测结果 +ArucoInfo[] aruco_infos diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/MultiDetectionInfo.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/MultiDetectionInfo.msg new file mode 100644 index 00000000..640d91f2 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/MultiDetectionInfo.msg @@ -0,0 +1,10 @@ +Header header + +## 检测到的目标数量 +int32 num_objs + +## Detecting or Tracking (0:detect, 1:track) +int32 detect_or_track + +## 每个目标的检测结果 +DetectionInfo[] detection_infos diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/MultiDetectionInfoSub.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/MultiDetectionInfoSub.msg new file mode 100644 index 00000000..bc9cd466 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/MultiDetectionInfoSub.msg @@ -0,0 +1,10 @@ +std_msgs/Header header + +#模式：0：空闲 2.simaRPN 3.deepsort/sort +uint8 mode + +## 检测到的目标数量 +int32 num_objs + +## 每个目标的检测结果 +DetectionInfoSub[] detection_infos diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/MultiUAVState.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/MultiUAVState.msg new file mode 100755 index 00000000..81b2762e --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/MultiUAVState.msg @@ -0,0 +1,7 @@ +std_msgs/Header header + +## +int32 uav_num + +## 从1开始 +UAVState[] uav_state_all diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/OffsetPose.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/OffsetPose.msg new file mode 100644 index 00000000..776467e3 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/OffsetPose.msg @@ -0,0 +1,3 @@ +uint8 uav_id +float32 x +float32 y \ No newline at end of file diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/RheaCommunication.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/RheaCommunication.msg new file mode 100644 index 00000000..dff2f07e --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/RheaCommunication.msg @@ -0,0 +1,18 @@ +std_msgs/Header header + +uint8 Mode #控制模式 + +##控制模式类型枚举 +uint8 Stop=0 +uint8 Forward=1 +uint8 Left=2 +uint8 Right=3 +uint8 Back=4 +uint8 CMD=5 +uint8 Waypoint=6 + +float64 linear +float64 angular + +## 航点 +RheaGPS[] waypoint diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/RheaGPS.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/RheaGPS.msg new file mode 100644 index 00000000..48cba1d6 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/RheaGPS.msg @@ -0,0 +1,3 @@ +float64 latitude +float64 longitude +float64 altitude \ No newline at end of file diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/RheaState.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/RheaState.msg new file mode 100644 index 00000000..26cf3bb5 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/RheaState.msg @@ -0,0 +1,19 @@ +std_msgs/Header header +uint8 rhea_id + +## 速度 +float64 linear +float64 angular + +## 航向角 +float64 yaw + +## GPS +float32 latitude #纬度 +float32 longitude #经度 +float32 altitude #高度 + +float32[3] position + +## Status +float32 battery_voltage \ No newline at end of file diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/SwarmCommand.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/SwarmCommand.msg new file mode 100755 index 00000000..0182f750 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/SwarmCommand.msg @@ -0,0 +1,51 @@ +std_msgs/Header header + +## 消息来源 +string source + +## 编队套件数量 +uint8 swarm_num + +uint8 swarm_location_source + +# enum loc 定位来源枚举 +uint8 mocap = 0 +uint8 gps = 4 +uint8 rtk = 5 +uint8 uwb = 6 + +## 命令 +uint8 Swarm_CMD +# enum CMD 控制模式枚举 +uint8 Ready=0 +uint8 Init=1 +uint8 Start=2 +uint8 Hold=3 +uint8 Stop=4 +uint8 Formation=5 + +uint8 Follow=11 +uint8 Search=12 +uint8 Attack=13 + +## 编队控制参考量 +float32[3] leader_pos +float32[2] leader_vel +float32 swarm_size +uint8 swarm_shape +uint8 One_column=0 +uint8 Triangle=1 +uint8 Square=2 +uint8 Circular=3 + +## 搜索控制参考量 +float32 target_area_x_min ## [m] +float32 target_area_y_min ## [m] +float32 target_area_x_max ## [m] +float32 target_area_y_max ## [m] + +## 攻击控制参考量 +float32[3] attack_target_pos ## [m] + +#队形位置 +geometry_msgs/Point[] formation_poses diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/TextInfo.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/TextInfo.msg new file mode 100644 index 00000000..25035901 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/TextInfo.msg @@ -0,0 +1,12 @@ +#INFO:正常运行状况下反馈给地面站的信息,例如程序正常启动,状态切换的提示信息等. +uint8 INFO=0 +#WARN:无人机或软件程序出现意外情况,依然能正常启动或继续执行任务,小概率会出现危险状况,例如无人机RTK无法维持退出到GPS,视觉跟踪目标突然丢失重新搜寻目标等. +uint8 WARN=1 +#ERROR:无人机或软件程序出现重大意外情况,无法正常启动或继续执行任务,极有可能会出现危险状况,需要中断任务以及人为接管控制无人机,例如通信中断,无人机定位发散,ROS节点无法正常启动等. +uint8 ERROR=2 +#FATAL:任务执行过程中,软件崩溃或无人机飞控崩溃导致无人机完全失控,需要迅速人为接管控制无人机降落减少炸机损失. +uint8 FATAL=3 + +std_msgs/Header header +uint8 MessageType +string Message \ No newline at end of file diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/UAVCommand.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/UAVCommand.msg new file mode 100755 index 00000000..7bc358ca --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/UAVCommand.msg @@ -0,0 +1,38 @@ +std_msgs/Header header + +## 控制命令的模式 +uint8 Agent_CMD +# Agent_CMD 枚举 +uint8 Init_Pos_Hover=1 # home点上方悬停 +uint8 Current_Pos_Hover=2 # 当前位置上方悬停 +uint8 Land=3 +uint8 Move=4 +uint8 User_Mode1=5 + +## 移动命令下的子模式 +uint8 Move_mode +## 移动命令下的子模式枚举 +uint8 XYZ_POS = 0 ### 惯性系定点控制 +uint8 XY_VEL_Z_POS = 1 ### 惯性系定高速度控制 +uint8 XYZ_VEL = 2 ### 惯性系速度控制 +uint8 XYZ_POS_BODY = 3 ### 机体系位置控制 +uint8 XYZ_VEL_BODY = 4 ### 机体系速度控制 +uint8 XY_VEL_Z_POS_BODY = 5 ### 机体系定高速度控制 +uint8 TRAJECTORY = 6 ### 轨迹追踪控制 +uint8 XYZ_ATT = 7 ### 姿态控制（来自外部控制器） +uint8 LAT_LON_ALT = 8 ### 绝对坐标系下的经纬度 + +## 控制参考量 +float32[3] position_ref ## [m] +float32[3] velocity_ref ## [m/s] +float32[3] acceleration_ref ## [m/s^2] +float32 yaw_ref ## [rad] +bool Yaw_Rate_Mode ## True 代表控制偏航角速率 +float32 yaw_rate_ref ## [rad/s] +float32[4] att_ref ## [rad] + [0-1] +float64 latitude ## 无人机经度、纬度、高度 +float64 longitude ## 无人机经度、纬度、高度 +float64 altitude ## 无人机经度、纬度、高度 + +## 控制命令的编号 防止接收到错误命令， 编号应该逐次递加 +uint32 Command_ID \ No newline at end of file diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/UAVControlState.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/UAVControlState.msg new file mode 100755 index 00000000..02b0a369 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/UAVControlState.msg @@ -0,0 +1,23 @@ +std_msgs/Header header + +## 无人机编号 +uint8 uav_id + +## 无人机控制状态 +uint8 control_state +## 状态枚举 +uint8 INIT=0 +uint8 RC_POS_CONTROL=1 +uint8 COMMAND_CONTROL=2 +uint8 LAND_CONTROL=3 + +## 无人机控制器标志量 +uint8 pos_controller +## 状态枚举 +uint8 PX4_ORIGIN=0 +uint8 PID=1 +uint8 UDE=2 +uint8 NE=3 + +# 无人机安全保护触发标志量 +bool failsafe diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/UAVSetup.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/UAVSetup.msg new file mode 100644 index 00000000..982d6fd0 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/UAVSetup.msg @@ -0,0 +1,17 @@ +std_msgs/Header header + +## 无人机Setup类型（可用于模拟遥控器） +uint8 cmd +uint8 ARMING = 0 +uint8 SET_PX4_MODE = 1 +uint8 REBOOT_PX4 = 2 +uint8 SET_CONTROL_MODE = 3 + +bool arming + +## PX4模式名查询:http://wiki.ros.org/mavros/CustomModes +## 常用模式名:OFFBOARD,AUTO.LAND,AUTO.RTL,POSCTL +string px4_mode + +## INIT,MANUAL_CONTROL,HOVER_CONTROL,COMMAND_CONTROL,LAND_CONTROL +string control_state diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/UAVState.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/UAVState.msg new file mode 100755 index 00000000..1974c4f1 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/UAVState.msg @@ -0,0 +1,59 @@ +std_msgs/Header header + +## 无人机编号 +uint8 uav_id + +## 机载电脑是否连接上飞控，true已连接，false则不是 +bool connected +## 是否解锁，true为已解锁，false则不是 +bool armed +## PX4飞控当前飞行模式 int8 +string mode +## 无人机定位来源 +uint8 location_source +## 定位来源枚举 +uint8 MOCAP=0 +uint8 T265=1 +uint8 GAZEBO=2 +uint8 FAKE_ODOM=3 +uint8 GPS = 4 +uint8 RTK = 5 +uint8 UWB = 6 + +## odom标志位 +bool odom_valid + +## GPS状态,变量对应状态可参考mavros_msgs/GPSRAW中的fix_type +uint8 gps_status +## GPS状态枚举 +uint8 GPS_FIX_TYPE_NO_GPS=0 +uint8 GPS_FIX_TYPE_NO_FIX=1 +uint8 GPS_FIX_TYPE_2D_FIX=2 +uint8 GPS_FIX_TYPE_3D_FIX=3 +uint8 GPS_FIX_TYPE_DGPS=4 +uint8 GPS_FIX_TYPE_RTK_FLOATR=5 +uint8 GPS_FIX_TYPE_RTK_FIXEDR=6 +uint8 GPS_FIX_TYPE_STATIC=7 +uint8 GPS_FIX_TYPE_PPP=8 + +## 无人机位置、速度、姿态 +float32[3] position ## [m] +## 无人机经度、纬度、高度 +float32 latitude +float32 longitude +float32 altitude +## 无人机速度、姿态 +float32[3] velocity ## [m/s] +## 无人机姿态（欧拉角、四元数） +float32[3] attitude ## [rad] +geometry_msgs/Quaternion attitude_q ## 四元数 +## 无人机姿态角速度 +float32[3] attitude_rate ## [rad/s] +## 无人机电池状态 +float32 battery_state ## [V] +float32 battery_percetage ## [0-1] + + + + + \ No newline at end of file diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/VisionDiff.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/VisionDiff.msg new file mode 100644 index 00000000..d47e1aca --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/VisionDiff.msg @@ -0,0 +1,28 @@ +uint8 Id +uint8 detect + +uint16 objectX # 左上角 +uint16 objectY # 左上角 +uint16 objectWidth +uint16 objectHeight + +uint16 frameWidth +uint16 frameHeight + +# Gimbal 跟踪pid +float32 kp +float32 ki +float32 kd + +int8 ctlMode # 0: yaw+pitch, 1: roll+pitch 3:混合(未实现) +int8 yawPitch = 0 +int8 rollPitch = 1 +int8 mix=3 + +# 用于自动缩放 +float32 currSize #框选近大远小 +float32 maxSize +float32 minSize #框选大小 + +float32 trackIgnoreError +bool autoZoom \ No newline at end of file diff --git a/src/Prometheus/Modules/common/prometheus_msgs/msg/WindowPosition.msg b/src/Prometheus/Modules/common/prometheus_msgs/msg/WindowPosition.msg new file mode 100644 index 00000000..8b4bcb4b --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/msg/WindowPosition.msg @@ -0,0 +1,20 @@ +std_msgs/Header header + +#模式：0：空闲 1.kcf 3.deepsort/sort(点击的id) +uint8 mode + + +#波门位置X,#波门位置Y(kcf,点击画面的功能的时候使用),左上角为（0,0） +int16 origin_x +int16 origin_y +int16 width +int16 height + +#波门位置X,#波门位置Y +#int16 window_position_x = origin_x + width/2 +#int16 window_position_y = origin_y + height/2 +int16 window_position_x +int16 window_position_y + +#算法检测结果的ID +int32 track_id diff --git a/src/Prometheus/Modules/common/prometheus_msgs/package.xml b/src/Prometheus/Modules/common/prometheus_msgs/package.xml new file mode 100644 index 00000000..d9695c65 --- /dev/null +++ b/src/Prometheus/Modules/common/prometheus_msgs/package.xml @@ -0,0 +1,30 @@ + + + prometheus_msgs + 0.0.0 + The prometheus_msgs package + + Yuhua Qi + + TODO + + message_generation + message_runtime + catkin + std_msgs + actionlib_msgs + std_msgs + actionlib_msgs + std_msgs + actionlib_msgs + geometry_msgs + geometry_msgs + geometry_msgs + sensor_msgs + sensor_msgs + sensor_msgs + + + + + diff --git a/src/Prometheus/Modules/common/quadrotor_msgs/CMakeLists.txt b/src/Prometheus/Modules/common/quadrotor_msgs/CMakeLists.txt new file mode 100755 index 00000000..f82385d3 --- /dev/null +++ b/src/Prometheus/Modules/common/quadrotor_msgs/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 2.8.3) +project(quadrotor_msgs) + +find_package(catkin REQUIRED COMPONENTS + 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( + CATKIN_DEPENDS message_runtime +) + +find_package(Eigen3 REQUIRED) +include_directories(${EIGEN3_INCLUDE_DIR}) + + diff --git a/src/Prometheus/Modules/common/quadrotor_msgs/msg/AuxCommand.msg b/src/Prometheus/Modules/common/quadrotor_msgs/msg/AuxCommand.msg new file mode 100755 index 00000000..f59bf356 --- /dev/null +++ b/src/Prometheus/Modules/common/quadrotor_msgs/msg/AuxCommand.msg @@ -0,0 +1,5 @@ +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/Prometheus/Modules/common/quadrotor_msgs/msg/Corrections.msg b/src/Prometheus/Modules/common/quadrotor_msgs/msg/Corrections.msg new file mode 100755 index 00000000..e0f4e888 --- /dev/null +++ b/src/Prometheus/Modules/common/quadrotor_msgs/msg/Corrections.msg @@ -0,0 +1,2 @@ +float64 kf_correction +float64[2] angle_corrections diff --git a/src/Prometheus/Modules/common/quadrotor_msgs/msg/Gains.msg b/src/Prometheus/Modules/common/quadrotor_msgs/msg/Gains.msg new file mode 100755 index 00000000..f5d10a33 --- /dev/null +++ b/src/Prometheus/Modules/common/quadrotor_msgs/msg/Gains.msg @@ -0,0 +1,4 @@ +float64 Kp +float64 Kd +float64 Kp_yaw +float64 Kd_yaw diff --git a/src/Prometheus/Modules/common/quadrotor_msgs/msg/LQRTrajectory.msg b/src/Prometheus/Modules/common/quadrotor_msgs/msg/LQRTrajectory.msg new file mode 100755 index 00000000..0a34e9b6 --- /dev/null +++ b/src/Prometheus/Modules/common/quadrotor_msgs/msg/LQRTrajectory.msg @@ -0,0 +1,30 @@ +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/Prometheus/Modules/common/quadrotor_msgs/msg/Odometry.msg b/src/Prometheus/Modules/common/quadrotor_msgs/msg/Odometry.msg new file mode 100755 index 00000000..3272d71a --- /dev/null +++ b/src/Prometheus/Modules/common/quadrotor_msgs/msg/Odometry.msg @@ -0,0 +1,8 @@ +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/Prometheus/Modules/common/quadrotor_msgs/msg/OutputData.msg b/src/Prometheus/Modules/common/quadrotor_msgs/msg/OutputData.msg new file mode 100755 index 00000000..ac958880 --- /dev/null +++ b/src/Prometheus/Modules/common/quadrotor_msgs/msg/OutputData.msg @@ -0,0 +1,12 @@ +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/Prometheus/Modules/common/quadrotor_msgs/msg/PPROutputData.msg b/src/Prometheus/Modules/common/quadrotor_msgs/msg/PPROutputData.msg new file mode 100755 index 00000000..70434a02 --- /dev/null +++ b/src/Prometheus/Modules/common/quadrotor_msgs/msg/PPROutputData.msg @@ -0,0 +1,16 @@ +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/Prometheus/Modules/common/quadrotor_msgs/msg/PolynomialTrajectory.msg b/src/Prometheus/Modules/common/quadrotor_msgs/msg/PolynomialTrajectory.msg new file mode 100755 index 00000000..0aab297a --- /dev/null +++ b/src/Prometheus/Modules/common/quadrotor_msgs/msg/PolynomialTrajectory.msg @@ -0,0 +1,28 @@ +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/Prometheus/Modules/common/quadrotor_msgs/msg/PositionCommand.msg b/src/Prometheus/Modules/common/quadrotor_msgs/msg/PositionCommand.msg new file mode 100755 index 00000000..49c6fa1d --- /dev/null +++ b/src/Prometheus/Modules/common/quadrotor_msgs/msg/PositionCommand.msg @@ -0,0 +1,22 @@ +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/Prometheus/Modules/common/quadrotor_msgs/msg/Px4ctrlDebug.msg b/src/Prometheus/Modules/common/quadrotor_msgs/msg/Px4ctrlDebug.msg new file mode 100755 index 00000000..f28ba2c3 --- /dev/null +++ b/src/Prometheus/Modules/common/quadrotor_msgs/msg/Px4ctrlDebug.msg @@ -0,0 +1,37 @@ +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/Prometheus/Modules/common/quadrotor_msgs/msg/SO3Command.msg b/src/Prometheus/Modules/common/quadrotor_msgs/msg/SO3Command.msg new file mode 100755 index 00000000..d3868efb --- /dev/null +++ b/src/Prometheus/Modules/common/quadrotor_msgs/msg/SO3Command.msg @@ -0,0 +1,6 @@ +Header header +geometry_msgs/Vector3 force +geometry_msgs/Quaternion orientation +float64[3] kR +float64[3] kOm +quadrotor_msgs/AuxCommand aux diff --git a/src/Prometheus/Modules/common/quadrotor_msgs/msg/Serial.msg b/src/Prometheus/Modules/common/quadrotor_msgs/msg/Serial.msg new file mode 100755 index 00000000..5a54cce3 --- /dev/null +++ b/src/Prometheus/Modules/common/quadrotor_msgs/msg/Serial.msg @@ -0,0 +1,13 @@ +# 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/Prometheus/Modules/common/quadrotor_msgs/msg/StatusData.msg b/src/Prometheus/Modules/common/quadrotor_msgs/msg/StatusData.msg new file mode 100755 index 00000000..d176e4f0 --- /dev/null +++ b/src/Prometheus/Modules/common/quadrotor_msgs/msg/StatusData.msg @@ -0,0 +1,4 @@ +Header header +uint16 loop_rate +float64 voltage +uint8 seq diff --git a/src/Prometheus/Modules/common/quadrotor_msgs/msg/TRPYCommand.msg b/src/Prometheus/Modules/common/quadrotor_msgs/msg/TRPYCommand.msg new file mode 100755 index 00000000..0d471a62 --- /dev/null +++ b/src/Prometheus/Modules/common/quadrotor_msgs/msg/TRPYCommand.msg @@ -0,0 +1,6 @@ +Header header +float32 thrust +float32 roll +float32 pitch +float32 yaw +quadrotor_msgs/AuxCommand aux diff --git a/src/Prometheus/Modules/common/quadrotor_msgs/package.xml b/src/Prometheus/Modules/common/quadrotor_msgs/package.xml new file mode 100755 index 00000000..42e9f76e --- /dev/null +++ b/src/Prometheus/Modules/common/quadrotor_msgs/package.xml @@ -0,0 +1,20 @@ + + 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/Prometheus/Modules/communication/CMakeLists.txt b/src/Prometheus/Modules/communication/CMakeLists.txt new file mode 100644 index 00000000..e702198b --- /dev/null +++ b/src/Prometheus/Modules/communication/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.0.2) +project(prometheus_communication_bridge) + +## 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 + std_msgs + std_srvs + geometry_msgs + sensor_msgs + message_generation + tf2_msgs + visualization_msgs + mavros + mavros_msgs + prometheus_msgs +) + + +generate_messages( + DEPENDENCIES + std_msgs + std_srvs + geometry_msgs + sensor_msgs + tf2_msgs + visualization_msgs + mavros_msgs +) + +catkin_package( + INCLUDE_DIRS include + ##CATKIN_DEPENDS roscpp std_msgs sensor_msgs + CATKIN_DEPENDS + message_runtime + std_msgs + std_srvs + geometry_msgs + mavros_msgs +) + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include ${catkin_INCLUDE_DIRS} + include + ${catkin_INCLUDE_DIRS} + ${PROJECT_SOURCE_DIR}/shard/include +) + +file(GLOB_RECURSE CURRENT_INCLUDE include/*.hpp include/*.h) +file(GLOB_RECURSE CURRENT_SOURCE src/*.cpp) + +## Specify libraries to link a library or executable target against +add_executable(communication_bridge ${CURRENT_SOURCE} ${CURRENT_INCLUDE}) +add_dependencies(communication_bridge ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(communication_bridge ${catkin_LIBRARIES} ${PROJECT_SOURCE_DIR}/shard/libs/libcommunication.so boost_serialization) + diff --git a/src/Prometheus/Modules/communication/include/autonomous_landing_topic.hpp b/src/Prometheus/Modules/communication/include/autonomous_landing_topic.hpp new file mode 100644 index 00000000..cd7934ec --- /dev/null +++ b/src/Prometheus/Modules/communication/include/autonomous_landing_topic.hpp @@ -0,0 +1,50 @@ +#ifndef AUTONOMOUS_LANDING_TOPIC_HPP +#define AUTONOMOUS_LANDING_TOPIC_HPP + +#include +#include "communication.hpp" + +#include "gimbal_basic_topic.hpp" + +#include "std_srvs/SetBool.h" +#include "mavros_msgs/ParamSet.h" +#include "prometheus_msgs/RheaState.h" + + +class AutonomousLanding +{ +public: + AutonomousLanding(ros::NodeHandle &nh,Communication *communication); + + ~AutonomousLanding(); + + + void gimbalSearchServer(bool is); + + void gimbalRecordVideoServer(bool is); + + void gimbalTrackModeServer(bool is); + + void gimbalParamSetServer(struct GimbalParamSet param_set); + + void rheaStatePub(struct RheaState rhea_state); + +private: + + ros::Publisher ugv_state_pub_; + + ros::ServiceClient gimbal_search_client_; + ros::ServiceClient gimbal_record_video_client_; + ros::ServiceClient gimbal_track_mode_client_; + ros::ServiceClient param_set_client_; + + struct GimbalState gimbal_state_; + struct VisionDiff vision_diff_; + + int robot_id; + + Communication* communication_ = NULL; + std::string multicast_udp_ip; +}; + +#endif \ No newline at end of file diff --git a/src/Prometheus/Modules/communication/include/communication_bridge.hpp b/src/Prometheus/Modules/communication/include/communication_bridge.hpp new file mode 100644 index 00000000..fcf56d1b --- /dev/null +++ b/src/Prometheus/Modules/communication/include/communication_bridge.hpp @@ -0,0 +1,93 @@ +#ifndef COMUNICATION_BRIDGE_HPP +#define COMUNICATION_BRIDGE_HPP + +#include +#include "communication.hpp" + +#include + +#include +#include "Struct.hpp" + +#include "uav_basic_topic.hpp" +#include "ugv_basic_topic.hpp" +#include "swarm_control_topic.hpp" +#include "autonomous_landing_topic.hpp" +#include "gimbal_basic_topic.hpp" +#include "object_tracking_topic.hpp" + +#include +#include + +enum UserType +{ + UAV = 1, + UGV = 2 +}; + +class CommunicationBridge : public Communication +{ +public: + CommunicationBridge(ros::NodeHandle &nh); + + ~CommunicationBridge(); + + void serverFun(); + + //根据协议中MSG_ID的值，将数据段数据转化为正确的结构体 + void pubMsg(int msg_id); + + void recvData(struct UAVState uav_state); + void recvData(struct UAVCommand uav_cmd); + void recvData(struct SwarmCommand swarm_command); + void recvData(struct ConnectState connect_state); + void recvData(struct GimbalControl gimbal_control); + void recvData(struct GimbalService gimbal_service); + void recvData(struct GimbalParamSet param_set); + void recvData(struct WindowPosition window_position); + void recvData(struct RheaControl rhea_control); + void recvData(struct RheaState rhea_state); + void recvData(struct ImageData image_data); + void recvData(struct UAVSetup uav_setup); + void recvData(struct ModeSelection mode_selection); + + void modeSwitch(struct ModeSelection mode_selection); + + //接收组播地址的数据 + void multicastUdpFun(); + + //给地面站发送心跳包 + void toGroundStationFun(); + + void init(); + //ros::NodeHandle nh; + + void createImage(struct ImageData image_data); + + bool createMode(struct ModeSelection mode_selection); + + bool deleteMode(struct ModeSelection mode_selection); + +private: + //std::shared_ptr swarm_control_ ; + SwarmControl *swarm_control_ = NULL; + UGVBasic *ugv_basic_ = NULL; + UAVBasic *uav_basic_ = NULL; + //std::vector swarm_control_simulation_; + std::map swarm_control_simulation_; + AutonomousLanding *autonomous_landing_ = NULL; + GimbalBasic *gimbal_basic_ = NULL; + ObjectTracking *object_tracking_ = NULL; + + int current_mode_ = 0; + + int is_simulation_, swarm_num_, swarm_data_update_timeout_; + ros::NodeHandle nh_; + + bool is_heartbeat_ready_ = false; + + int user_type_; + +}; + +#endif \ No newline at end of file diff --git a/src/Prometheus/Modules/communication/include/gimbal_basic_topic.hpp b/src/Prometheus/Modules/communication/include/gimbal_basic_topic.hpp new file mode 100644 index 00000000..cf7b835d --- /dev/null +++ b/src/Prometheus/Modules/communication/include/gimbal_basic_topic.hpp @@ -0,0 +1,39 @@ +#ifndef GimbalBasic_HPP +#define GimbalBasic_HPP + +#include +#include "communication.hpp" + +#include "prometheus_msgs/GimbalState.h" +#include "prometheus_msgs/VisionDiff.h" +#include "prometheus_msgs/WindowPosition.h" +#include "prometheus_msgs/GimbalControl.h" + +class GimbalBasic +{ +public: + GimbalBasic(ros::NodeHandle &nh,Communication *communication); + ~GimbalBasic(); + + void stateCb(const prometheus_msgs::GimbalState::ConstPtr &msg); + + void trackCb(const prometheus_msgs::VisionDiff::ConstPtr &msg); + + void gimbalWindowPositionPub(struct WindowPosition window_position); + + void gimbalControlPub(struct GimbalControl gimbal_control); + +protected: + ros::Subscriber gimbal_state_sub_; + ros::Subscriber vision_diff_sub_; + ros::Publisher window_position_pub_; + ros::Publisher gimbal_control_pub_; + + struct GimbalState gimbal_state_; + struct VisionDiff vision_diff_; + + Communication* communication_ = NULL; + std::string multicast_udp_ip; +}; + +#endif \ No newline at end of file diff --git a/src/Prometheus/Modules/communication/include/object_tracking_topic.hpp b/src/Prometheus/Modules/communication/include/object_tracking_topic.hpp new file mode 100644 index 00000000..901c911e --- /dev/null +++ b/src/Prometheus/Modules/communication/include/object_tracking_topic.hpp @@ -0,0 +1,26 @@ +#ifndef OBJECT_TRACKING_TOPIC_HPP +#define OBJECT_TRACKING_TOPIC_HPP + +#include +#include "communication.hpp" +#include "prometheus_msgs/MultiDetectionInfoSub.h" +#include "prometheus_msgs/DetectionInfoSub.h" + +class ObjectTracking +{ +public: + ObjectTracking(ros::NodeHandle &nh,Communication *communication); + ~ObjectTracking(); + + void multiDetectionInfoCb(const prometheus_msgs::MultiDetectionInfoSub::ConstPtr &msg); + + +private: + Communication* communication_ = NULL; + + std::string multicast_udp_ip; + + ros::Subscriber multi_detection_info_sub_; +}; + +#endif diff --git a/src/Prometheus/Modules/communication/include/swarm_control_topic.hpp b/src/Prometheus/Modules/communication/include/swarm_control_topic.hpp new file mode 100644 index 00000000..04ba9960 --- /dev/null +++ b/src/Prometheus/Modules/communication/include/swarm_control_topic.hpp @@ -0,0 +1,90 @@ +#ifndef SWARM_CONTROL_TOPIC_HPP +#define SWARM_CONTROL_TOPIC_HPP + +#include +#include "communication.hpp" +#include "uav_basic_topic.hpp" + +#include "std_msgs/Bool.h" + +#include "prometheus_msgs/MultiUAVState.h" +#include "prometheus_msgs/SwarmCommand.h" +#include "prometheus_msgs/UAVState.h" +#include "prometheus_msgs/OffsetPose.h" + +#include + +using namespace std; + +//订阅: /prometheus/formation_assign/result +//发布: /Prometheus/formation_assign/info + +struct MultiUAVState +{ + int uav_num; + std::vector uav_state_all; +}; + +class SwarmControl//: public UAVBasic +{ +public: + //真机构造 + SwarmControl(ros::NodeHandle &nh, int id, int swarm_num,Communication *communication); + + //仿真构造 + SwarmControl(ros::NodeHandle &nh, int swarm_num,Communication *communication); + + ~SwarmControl(); + + void init(ros::NodeHandle &nh, int swarm_num,int id = 1); + + //更新全部飞机数据 + void updateAllUAVState(struct UAVState uav_state); + + //【发布】集群控制指令 + void swarmCommandPub(struct SwarmCommand swarm_command); + + //【发布】连接是否失效 + void communicationStatePub(bool communication); + + void communicationStatePub(bool communication,int id); + + //【发布】所有无人机状态 + void allUAVStatePub(struct MultiUAVState m_multi_uav_state); + + + void closeTopic(); + + inline struct MultiUAVState getMultiUAVState(){return this->multi_uav_state_;}; + + inline prometheus_msgs::UAVState getUAVStateMsg(){return this->uav_state_msg_;}; + + +private: + + struct MultiUAVState multi_uav_state_; + + Communication *communication_ = NULL; + + prometheus_msgs::UAVState uav_state_msg_; + + + //集群全部uav 状态 + ros::Publisher all_uav_state_pub_; + //控制指令 + ros::Publisher swarm_command_pub_; + //连接是否失效 + ros::Publisher communication_state_pub_; + + + //仿真 + std::vector simulation_communication_state_pub; + + bool is_simulation_; + + std::string udp_ip, multicast_udp_ip; + + std::string user_type_ = ""; +}; + +#endif \ No newline at end of file diff --git a/src/Prometheus/Modules/communication/include/uav_basic_topic.hpp b/src/Prometheus/Modules/communication/include/uav_basic_topic.hpp new file mode 100644 index 00000000..203c4568 --- /dev/null +++ b/src/Prometheus/Modules/communication/include/uav_basic_topic.hpp @@ -0,0 +1,75 @@ +#ifndef UAV_BASIC_TOPIC_HPP +#define UAV_BASIC_TOPIC_HPP + +#include +#include "communication.hpp" +#include "prometheus_msgs/UAVState.h" +#include "prometheus_msgs/TextInfo.h" +#include "prometheus_msgs/OffsetPose.h" +#include "prometheus_msgs/UAVCommand.h" +#include "prometheus_msgs/UAVSetup.h" +#include "prometheus_msgs/UAVControlState.h" + +class UAVBasic +{ +public: + UAVBasic(); + + UAVBasic(ros::NodeHandle &nh,int id,Communication *communication); + + ~UAVBasic(); + + inline int getRobotId(){return robot_id;}; + + void stateCb(const prometheus_msgs::UAVState::ConstPtr &msg); + + //【回调】uav反馈信息 + void textInfoCb(const prometheus_msgs::TextInfo::ConstPtr &msg); + + //【订阅】偏移量 + void offsetPoseCb(const prometheus_msgs::OffsetPose::ConstPtr &msg); + + void controlStateCb(const prometheus_msgs::UAVControlState::ConstPtr &msg); + + struct UAVState getUAVState(); + + void uavCmdPub(struct UAVCommand uav_cmd); + + void uavSetupPub(struct UAVSetup uav_setup); + + void setTimeStamp(uint time); + + uint getTimeStamp(); + +private: + ros::Subscriber uav_state_sub_; + + //反馈信息 + ros::Subscriber text_info_sub_; + //控制状态 + ros::Subscriber uav_control_state_sub_; + //偏移量订阅 + ros::Subscriber offset_pose_sub_; + + ros::Publisher uav_cmd_pub_; + + ros::Publisher uav_setup_pub_; + + int current_mode_; + + int robot_id; + + struct UAVState uav_state_; + struct TextInfo text_info_; + struct UAVControlState uav_control_state_; + + prometheus_msgs::OffsetPose offset_pose_; + + Communication *communication_ = NULL; + + std::string multicast_udp_ip; + + uint time_stamp_ = 0; +}; + +#endif \ No newline at end of file diff --git a/src/Prometheus/Modules/communication/include/ugv_basic_topic.hpp b/src/Prometheus/Modules/communication/include/ugv_basic_topic.hpp new file mode 100644 index 00000000..2a2fdf32 --- /dev/null +++ b/src/Prometheus/Modules/communication/include/ugv_basic_topic.hpp @@ -0,0 +1,63 @@ +#ifndef UGV_BASIC_TOPIC_HPP +#define UGV_BASIC_TOPIC_HPP + +#include +#include "communication.hpp" +#include "prometheus_msgs/RheaCommunication.h" +#include "prometheus_msgs/RheaState.h" +#include "prometheus_msgs/RheaGPS.h" +using namespace std; + +class UGVBasic +{ +public: + UGVBasic(ros::NodeHandle &nh,Communication *communication); + + ~UGVBasic(); + + // void scanCb(const sensor_msgs::LaserScan::ConstPtr &msg); + + // void scanMatchedPoints2Cb(const sensor_msgs::PointCloud2::ConstPtr &msg); + + // void tfCb(const tf2_msgs::TFMessage::ConstPtr &msg); + + // void tfStaticCb(const tf2_msgs::TFMessage::ConstPtr &msg); + + // void constraintListCb(const visualization_msgs::MarkerArray::ConstPtr &msg); + + // void landmarkPosesListCb(const visualization_msgs::MarkerArray::ConstPtr &msg); + + // void trajectoryNodeListCb(const visualization_msgs::MarkerArray::ConstPtr &msg); + + void rheaControlPub(struct RheaControl rhea_control); + + void rheaStateCb(const prometheus_msgs::RheaState::ConstPtr &msg); + + void setTimeStamp(uint time); + + uint getTimeStamp(); + +private: + //rviz + ros::Subscriber scan_matched_points2_sub_; + ros::Subscriber scan_sub_; + ros::Subscriber tf_static_sub_; + ros::Subscriber tf_sub_; + ros::Subscriber constraint_list_sub_; + ros::Subscriber landmark_poses_list_sub_; + ros::Subscriber trajectory_node_list_sub_; + // + ros::Publisher rhea_control_pub_; + ros::Subscriber rhea_state_sub_; + + ros::Subscriber cmd_vel_sub_; + + Communication* communication_ = NULL; + + std::string udp_ip; + std::string multicast_udp_ip; + + uint time_stamp_ = 0; +}; + +#endif \ No newline at end of file diff --git a/src/Prometheus/Modules/communication/launch/bridge.launch b/src/Prometheus/Modules/communication/launch/bridge.launch new file mode 100644 index 00000000..574ef1ca --- /dev/null +++ b/src/Prometheus/Modules/communication/launch/bridge.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/communication/package.xml b/src/Prometheus/Modules/communication/package.xml new file mode 100644 index 00000000..624633b5 --- /dev/null +++ b/src/Prometheus/Modules/communication/package.xml @@ -0,0 +1,34 @@ + + + prometheus_communication_bridge + 0.0.0 + The ground_station_bridge module + + Amov + Aomv + + catkin + roscpp + std_msgs + std_srvs + geometry_msgs + roscpp + std_msgs + std_srvs + geometry_msgs + roscpp + std_msgs + std_srvs + geometry_msgs + sensor_msgs + sensor_msgs + sensor_msgs + mavros_msgs + mavros_msgs + mavros_msgs + message_runtime + + + + + diff --git a/src/Prometheus/Modules/communication/shard/include/CRC.h b/src/Prometheus/Modules/communication/shard/include/CRC.h new file mode 100644 index 00000000..e20db354 --- /dev/null +++ b/src/Prometheus/Modules/communication/shard/include/CRC.h @@ -0,0 +1,2066 @@ +/** + @file CRC.h + @author Daniel Bahr + @version 1.1.0.0 + @copyright + @parblock + CRC++ + Copyright (c) 2021, Daniel Bahr + 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 CRC++ 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 HOLDER OR CONTRIBUTORS BE LIABLE + FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + @endparblock +*/ + +/* + CRC++ can be configured by setting various #defines before #including this header file: + + #define crcpp_uint8 - Specifies the type used to store CRCs that have a width of 8 bits or less. + This type is not used in CRC calculations. Defaults to ::std::uint8_t. + #define crcpp_uint16 - Specifies the type used to store CRCs that have a width between 9 and 16 bits (inclusive). + This type is not used in CRC calculations. Defaults to ::std::uint16_t. + #define crcpp_uint32 - Specifies the type used to store CRCs that have a width between 17 and 32 bits (inclusive). + This type is not used in CRC calculations. Defaults to ::std::uint32_t. + #define crcpp_uint64 - Specifies the type used to store CRCs that have a width between 33 and 64 bits (inclusive). + This type is not used in CRC calculations. Defaults to ::std::uint64_t. + #define crcpp_size - This type is used for loop iteration and function signatures only. Defaults to ::std::size_t. + #define CRCPP_USE_NAMESPACE - Define to place all CRC++ code within the ::CRCPP namespace. + #define CRCPP_BRANCHLESS - Define to enable a branchless CRC implementation. The branchless implementation uses a single integer + multiplication in the bit-by-bit calculation instead of a small conditional. The branchless implementation + may be faster on processor architectures which support single-instruction integer multiplication. + #define CRCPP_USE_CPP11 - Define to enables C++11 features (move semantics, constexpr, static_assert, etc.). + #define CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - Define to include definitions for little-used CRCs. +*/ + +#ifndef CRCPP_CRC_H_ +#define CRCPP_CRC_H_ + +#include // Includes CHAR_BIT +#ifdef CRCPP_USE_CPP11 +#include // Includes ::std::size_t +#include // Includes ::std::uint8_t, ::std::uint16_t, ::std::uint32_t, ::std::uint64_t +#else +#include // Includes size_t +#include // Includes uint8_t, uint16_t, uint32_t, uint64_t +#endif +#include // Includes ::std::numeric_limits +#include // Includes ::std::move + +#ifndef crcpp_uint8 +# ifdef CRCPP_USE_CPP11 + /// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. +# define crcpp_uint8 ::std::uint8_t +# else + /// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. +# define crcpp_uint8 uint8_t +# endif +#endif + +#ifndef crcpp_uint16 +# ifdef CRCPP_USE_CPP11 + /// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. +# define crcpp_uint16 ::std::uint16_t +# else + /// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. +# define crcpp_uint16 uint16_t +# endif +#endif + +#ifndef crcpp_uint32 +# ifdef CRCPP_USE_CPP11 + /// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. +# define crcpp_uint32 ::std::uint32_t +# else + /// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. +# define crcpp_uint32 uint32_t +# endif +#endif + +#ifndef crcpp_uint64 +# ifdef CRCPP_USE_CPP11 + /// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. +# define crcpp_uint64 ::std::uint64_t +# else + /// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. +# define crcpp_uint64 uint64_t +# endif +#endif + +#ifndef crcpp_size +# ifdef CRCPP_USE_CPP11 + /// @brief Unsigned size definition, used for specifying data sizes. +# define crcpp_size ::std::size_t +# else + /// @brief Unsigned size definition, used for specifying data sizes. +# define crcpp_size size_t +# endif +#endif + +#ifdef CRCPP_USE_CPP11 + /// @brief Compile-time expression definition. +# define crcpp_constexpr constexpr +#else + /// @brief Compile-time expression definition. +# define crcpp_constexpr const +#endif + +#ifdef CRCPP_USE_NAMESPACE +namespace CRCPP +{ +#endif + +/** + @brief Static class for computing CRCs. + @note This class supports computation of full and multi-part CRCs, using a bit-by-bit algorithm or a + byte-by-byte lookup table. The CRCs are calculated using as many optimizations as is reasonable. + If compiling with C++11, the constexpr keyword is used liberally so that many calculations are + performed at compile-time instead of at runtime. +*/ +class CRC +{ +public: + // Forward declaration + template + struct Table; + + /** + @brief CRC parameters. + */ + template + struct Parameters + { + CRCType polynomial; ///< CRC polynomial + CRCType initialValue; ///< Initial CRC value + CRCType finalXOR; ///< Value to XOR with the final CRC + bool reflectInput; ///< true to reflect all input bytes + bool reflectOutput; ///< true to reflect the output CRC (reflection occurs before the final XOR) + + Table MakeTable() const; + }; + + /** + @brief CRC lookup table. After construction, the CRC parameters are fixed. + @note A CRC table can be used for multiple CRC calculations. + */ + template + struct Table + { + // Constructors are intentionally NOT marked explicit. + Table(const Parameters & parameters); + +#ifdef CRCPP_USE_CPP11 + Table(Parameters && parameters); +#endif + + const Parameters & GetParameters() const; + + const CRCType * GetTable() const; + + CRCType operator[](unsigned char index) const; + + private: + void InitTable(); + + Parameters parameters; ///< CRC parameters used to construct the table + CRCType table[1 << CHAR_BIT]; ///< CRC lookup table + }; + + // The number of bits in CRCType must be at least as large as CRCWidth. + // CRCType must be an unsigned integer type or a custom type with operator overloads. + template + static CRCType Calculate(const void * data, crcpp_size size, const Parameters & parameters); + + template + static CRCType Calculate(const void * data, crcpp_size size, const Parameters & parameters, CRCType crc); + + template + static CRCType Calculate(const void * data, crcpp_size size, const Table & lookupTable); + + template + static CRCType Calculate(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc); + + template + static CRCType CalculateBits(const void * data, crcpp_size size, const Parameters & parameters); + + template + static CRCType CalculateBits(const void * data, crcpp_size size, const Parameters & parameters, CRCType crc); + + template + static CRCType CalculateBits(const void * data, crcpp_size size, const Table & lookupTable); + + template + static CRCType CalculateBits(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc); + + // Common CRCs up to 64 bits. + // Note: Check values are the computed CRCs when given an ASCII input of "123456789" (without null terminator) +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + static const Parameters< crcpp_uint8, 4> & CRC_4_ITU(); + static const Parameters< crcpp_uint8, 5> & CRC_5_EPC(); + static const Parameters< crcpp_uint8, 5> & CRC_5_ITU(); + static const Parameters< crcpp_uint8, 5> & CRC_5_USB(); + static const Parameters< crcpp_uint8, 6> & CRC_6_CDMA2000A(); + static const Parameters< crcpp_uint8, 6> & CRC_6_CDMA2000B(); + static const Parameters< crcpp_uint8, 6> & CRC_6_ITU(); + static const Parameters< crcpp_uint8, 6> & CRC_6_NR(); + static const Parameters< crcpp_uint8, 7> & CRC_7(); +#endif + static const Parameters< crcpp_uint8, 8> & CRC_8(); +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + static const Parameters< crcpp_uint8, 8> & CRC_8_EBU(); + static const Parameters< crcpp_uint8, 8> & CRC_8_MAXIM(); + static const Parameters< crcpp_uint8, 8> & CRC_8_WCDMA(); + static const Parameters< crcpp_uint8, 8> & CRC_8_LTE(); + static const Parameters & CRC_10(); + static const Parameters & CRC_10_CDMA2000(); + static const Parameters & CRC_11(); + static const Parameters & CRC_11_NR(); + static const Parameters & CRC_12_CDMA2000(); + static const Parameters & CRC_12_DECT(); + static const Parameters & CRC_12_UMTS(); + static const Parameters & CRC_13_BBC(); + static const Parameters & CRC_15(); + static const Parameters & CRC_15_MPT1327(); +#endif + static const Parameters & CRC_16_ARC(); + static const Parameters & CRC_16_BUYPASS(); + static const Parameters & CRC_16_CCITTFALSE(); +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + static const Parameters & CRC_16_CDMA2000(); + static const Parameters & CRC_16_CMS(); + static const Parameters & CRC_16_DECTR(); + static const Parameters & CRC_16_DECTX(); + static const Parameters & CRC_16_DNP(); +#endif + static const Parameters & CRC_16_GENIBUS(); + static const Parameters & CRC_16_KERMIT(); +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + static const Parameters & CRC_16_MAXIM(); + static const Parameters & CRC_16_MODBUS(); + static const Parameters & CRC_16_T10DIF(); + static const Parameters & CRC_16_USB(); +#endif + static const Parameters & CRC_16_X25(); + static const Parameters & CRC_16_XMODEM(); +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + static const Parameters & CRC_17_CAN(); + static const Parameters & CRC_21_CAN(); + static const Parameters & CRC_24(); + static const Parameters & CRC_24_FLEXRAYA(); + static const Parameters & CRC_24_FLEXRAYB(); + static const Parameters & CRC_24_LTEA(); + static const Parameters & CRC_24_LTEB(); + static const Parameters & CRC_24_NRC(); + static const Parameters & CRC_30(); +#endif + static const Parameters & CRC_32(); + static const Parameters & CRC_32_BZIP2(); +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + static const Parameters & CRC_32_C(); +#endif + static const Parameters & CRC_32_MPEG2(); + static const Parameters & CRC_32_POSIX(); +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + static const Parameters & CRC_32_Q(); + static const Parameters & CRC_40_GSM(); + static const Parameters & CRC_64(); +#endif + +#ifdef CRCPP_USE_CPP11 + CRC() = delete; + CRC(const CRC & other) = delete; + CRC & operator=(const CRC & other) = delete; + CRC(CRC && other) = delete; + CRC & operator=(CRC && other) = delete; +#endif + +private: +#ifndef CRCPP_USE_CPP11 + CRC(); + CRC(const CRC & other); + CRC & operator=(const CRC & other); +#endif + + template + static IntegerType Reflect(IntegerType value, crcpp_uint16 numBits); + + template + static CRCType Finalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); + + template + static CRCType UndoFinalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); + + template + static CRCType CalculateRemainder(const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder); + + template + static CRCType CalculateRemainder(const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder); + + template + static CRCType CalculateRemainderBits(unsigned char byte, crcpp_size numBits, const Parameters & parameters, CRCType remainder); +}; + +/** + @brief Returns a CRC lookup table construct using these CRC parameters. + @note This function primarily exists to allow use of the auto keyword instead of instantiating + a table directly, since template parameters are not inferred in constructors. + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC lookup table +*/ +template +inline CRC::Table CRC::Parameters::MakeTable() const +{ + // This should take advantage of RVO and optimize out the copy. + return CRC::Table(*this); +} + +/** + @brief Constructs a CRC table from a set of CRC parameters + @param[in] params CRC parameters + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC +*/ +template +inline CRC::Table::Table(const Parameters & params) : + parameters(params) +{ + InitTable(); +} + +#ifdef CRCPP_USE_CPP11 +/** + @brief Constructs a CRC table from a set of CRC parameters + @param[in] params CRC parameters + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC +*/ +template +inline CRC::Table::Table(Parameters && params) : + parameters(::std::move(params)) +{ + InitTable(); +} +#endif + +/** + @brief Gets the CRC parameters used to construct the CRC table + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC parameters +*/ +template +inline const CRC::Parameters & CRC::Table::GetParameters() const +{ + return parameters; +} + +/** + @brief Gets the CRC table + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC table +*/ +template +inline const CRCType * CRC::Table::GetTable() const +{ + return table; +} + +/** + @brief Gets an entry in the CRC table + @param[in] index Index into the CRC table + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC table entry +*/ +template +inline CRCType CRC::Table::operator[](unsigned char index) const +{ + return table[index]; +} + +/** + @brief Initializes a CRC table. + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC +*/ +template +inline void CRC::Table::InitTable() +{ + // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) + static crcpp_constexpr CRCType BIT_MASK((CRCType(1) << (CRCWidth - CRCType(1))) | + ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1))); + + // The conditional expression is used to avoid a -Wshift-count-overflow warning. + static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); + + CRCType crc; + unsigned char byte = 0; + + // Loop over each dividend (each possible number storable in an unsigned char) + do + { + crc = CRC::CalculateRemainder(&byte, sizeof(byte), parameters, CRCType(0)); + + // This mask might not be necessary; all unit tests pass with this line commented out, + // but that might just be a coincidence based on the CRC parameters used for testing. + // In any case, this is harmless to leave in and only adds a single machine instruction per loop iteration. + crc &= BIT_MASK; + + if (!parameters.reflectInput && CRCWidth < CHAR_BIT) + { + // Undo the special operation at the end of the CalculateRemainder() + // function for non-reflected CRCs < CHAR_BIT. + crc = static_cast(crc << SHIFT); + } + + table[byte] = crc; + } + while (++byte); +} + +/** + @brief Computes a CRC. + @param[in] data Data over which CRC will be computed + @param[in] size Size of the data, in bytes + @param[in] parameters CRC parameters + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC +*/ +template +inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Parameters & parameters) +{ + CRCType remainder = CalculateRemainder(data, size, parameters, parameters.initialValue); + + // No need to mask the remainder here; the mask will be applied in the Finalize() function. + + return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); +} +/** + @brief Appends additional data to a previous CRC calculation. + @note This function can be used to compute multi-part CRCs. + @param[in] data Data over which CRC will be computed + @param[in] size Size of the data, in bytes + @param[in] parameters CRC parameters + @param[in] crc CRC from a previous calculation + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC +*/ +template +inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Parameters & parameters, CRCType crc) +{ + CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + + remainder = CalculateRemainder(data, size, parameters, remainder); + + // No need to mask the remainder here; the mask will be applied in the Finalize() function. + + return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); +} + +/** + @brief Computes a CRC via a lookup table. + @param[in] data Data over which CRC will be computed + @param[in] size Size of the data, in bytes + @param[in] lookupTable CRC lookup table + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC +*/ +template +inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table & lookupTable) +{ + const Parameters & parameters = lookupTable.GetParameters(); + + CRCType remainder = CalculateRemainder(data, size, lookupTable, parameters.initialValue); + + // No need to mask the remainder here; the mask will be applied in the Finalize() function. + + return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); +} + +/** + @brief Appends additional data to a previous CRC calculation using a lookup table. + @note This function can be used to compute multi-part CRCs. + @param[in] data Data over which CRC will be computed + @param[in] size Size of the data, in bytes + @param[in] lookupTable CRC lookup table + @param[in] crc CRC from a previous calculation + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC +*/ +template +inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc) +{ + const Parameters & parameters = lookupTable.GetParameters(); + + CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + + remainder = CalculateRemainder(data, size, lookupTable, remainder); + + // No need to mask the remainder here; the mask will be applied in the Finalize() function. + + return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); +} + +/** + @brief Computes a CRC. + @param[in] data Data over which CRC will be computed + @param[in] size Size of the data, in bits + @param[in] parameters CRC parameters + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC +*/ +template +inline CRCType CRC::CalculateBits(const void * data, crcpp_size size, const Parameters & parameters) +{ + CRCType remainder = parameters.initialValue; + + // Calculate the remainder on a whole number of bytes first, then call + // a special-case function for the remaining bits. + crcpp_size wholeNumberOfBytes = size / CHAR_BIT; + if (wholeNumberOfBytes > 0) + { + remainder = CalculateRemainder(data, wholeNumberOfBytes, parameters, remainder); + } + + crcpp_size remainingNumberOfBits = size % CHAR_BIT; + if (remainingNumberOfBits != 0) + { + unsigned char lastByte = *(reinterpret_cast(data) + wholeNumberOfBytes); + remainder = CalculateRemainderBits(lastByte, remainingNumberOfBits, parameters, remainder); + } + + // No need to mask the remainder here; the mask will be applied in the Finalize() function. + + return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); +} +/** + @brief Appends additional data to a previous CRC calculation. + @note This function can be used to compute multi-part CRCs. + @param[in] data Data over which CRC will be computed + @param[in] size Size of the data, in bits + @param[in] parameters CRC parameters + @param[in] crc CRC from a previous calculation + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC +*/ +template +inline CRCType CRC::CalculateBits(const void * data, crcpp_size size, const Parameters & parameters, CRCType crc) +{ + CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + + // Calculate the remainder on a whole number of bytes first, then call + // a special-case function for the remaining bits. + crcpp_size wholeNumberOfBytes = size / CHAR_BIT; + if (wholeNumberOfBytes > 0) + { + remainder = CalculateRemainder(data, wholeNumberOfBytes, parameters, parameters.initialValue); + } + + crcpp_size remainingNumberOfBits = size % CHAR_BIT; + if (remainingNumberOfBits != 0) + { + unsigned char lastByte = *(reinterpret_cast(data) + wholeNumberOfBytes); + remainder = CalculateRemainderBits(lastByte, remainingNumberOfBits, parameters, remainder); + } + + // No need to mask the remainder here; the mask will be applied in the Finalize() function. + + return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); +} + +/** + @brief Computes a CRC via a lookup table. + @param[in] data Data over which CRC will be computed + @param[in] size Size of the data, in bits + @param[in] lookupTable CRC lookup table + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC +*/ +template +inline CRCType CRC::CalculateBits(const void * data, crcpp_size size, const Table & lookupTable) +{ + const Parameters & parameters = lookupTable.GetParameters(); + + CRCType remainder = parameters.initialValue; + + // Calculate the remainder on a whole number of bytes first, then call + // a special-case function for the remaining bits. + crcpp_size wholeNumberOfBytes = size / CHAR_BIT; + if (wholeNumberOfBytes > 0) + { + remainder = CalculateRemainder(data, wholeNumberOfBytes, lookupTable, remainder); + } + + crcpp_size remainingNumberOfBits = size % CHAR_BIT; + if (remainingNumberOfBits != 0) + { + unsigned char lastByte = *(reinterpret_cast(data) + wholeNumberOfBytes); + remainder = CalculateRemainderBits(lastByte, remainingNumberOfBits, parameters, remainder); + } + + // No need to mask the remainder here; the mask will be applied in the Finalize() function. + + return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); +} + +/** + @brief Appends additional data to a previous CRC calculation using a lookup table. + @note This function can be used to compute multi-part CRCs. + @param[in] data Data over which CRC will be computed + @param[in] size Size of the data, in bits + @param[in] lookupTable CRC lookup table + @param[in] crc CRC from a previous calculation + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC +*/ +template +inline CRCType CRC::CalculateBits(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc) +{ + const Parameters & parameters = lookupTable.GetParameters(); + + CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + + // Calculate the remainder on a whole number of bytes first, then call + // a special-case function for the remaining bits. + crcpp_size wholeNumberOfBytes = size / CHAR_BIT; + if (wholeNumberOfBytes > 0) + { + remainder = CalculateRemainder(data, wholeNumberOfBytes, lookupTable, parameters.initialValue); + } + + crcpp_size remainingNumberOfBits = size % CHAR_BIT; + if (remainingNumberOfBits > 0) + { + unsigned char lastByte = *(reinterpret_cast(data) + wholeNumberOfBytes); + remainder = CalculateRemainderBits(lastByte, remainingNumberOfBits, parameters, remainder); + } + + // No need to mask the remainder here; the mask will be applied in the Finalize() function. + + return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); +} + +/** + @brief Reflects (i.e. reverses the bits within) an integer value. + @param[in] value Value to reflect + @param[in] numBits Number of bits in the integer which will be reflected + @tparam IntegerType Integer type of the value being reflected + @return Reflected value +*/ +template +inline IntegerType CRC::Reflect(IntegerType value, crcpp_uint16 numBits) +{ + IntegerType reversedValue(0); + + for (crcpp_uint16 i = 0; i < numBits; ++i) + { + reversedValue = static_cast((reversedValue << 1) | (value & 1)); + value = static_cast(value >> 1); + } + + return reversedValue; +} + +/** + @brief Computes the final reflection and XOR of a CRC remainder. + @param[in] remainder CRC remainder to reflect and XOR + @param[in] finalXOR Final value to XOR with the remainder + @param[in] reflectOutput true to reflect each byte of the remainder before the XOR + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return Final CRC +*/ +template +inline CRCType CRC::Finalize(CRCType remainder, CRCType finalXOR, bool reflectOutput) +{ + // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) + static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | + ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); + + if (reflectOutput) + { + remainder = Reflect(remainder, CRCWidth); + } + + return (remainder ^ finalXOR) & BIT_MASK; +} + +/** + @brief Undoes the process of computing the final reflection and XOR of a CRC remainder. + @note This function allows for computation of multi-part CRCs + @note Calling UndoFinalize() followed by Finalize() (or vice versa) will always return the original remainder value: + + CRCType x = ...; + CRCType y = Finalize(x, finalXOR, reflectOutput); + CRCType z = UndoFinalize(y, finalXOR, reflectOutput); + assert(x == z); + + @param[in] crc Reflected and XORed CRC + @param[in] finalXOR Final value XORed with the remainder + @param[in] reflectOutput true if the remainder is to be reflected + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return Un-finalized CRC remainder +*/ +template +inline CRCType CRC::UndoFinalize(CRCType crc, CRCType finalXOR, bool reflectOutput) +{ + // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) + static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | + ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); + + crc = (crc & BIT_MASK) ^ finalXOR; + + if (reflectOutput) + { + crc = Reflect(crc, CRCWidth); + } + + return crc; +} + +/** + @brief Computes a CRC remainder. + @param[in] data Data over which the remainder will be computed + @param[in] size Size of the data, in bytes + @param[in] parameters CRC parameters + @param[in] remainder Running CRC remainder. Can be an initial value or the result of a previous CRC remainder calculation. + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC remainder +*/ +template +inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder) +{ +#ifdef CRCPP_USE_CPP11 + // This static_assert is put here because this function will always be compiled in no matter what + // the template parameters are and whether or not a table lookup or bit-by-bit algorithm is used. + static_assert(::std::numeric_limits::digits >= CRCWidth, "CRCType is too small to contain a CRC of width CRCWidth."); +#else + // Catching this compile-time error is very important. Sadly, the compiler error will be very cryptic, but it's + // better than nothing. + enum { static_assert_failed_CRCType_is_too_small_to_contain_a_CRC_of_width_CRCWidth = 1 / (::std::numeric_limits::digits >= CRCWidth ? 1 : 0) }; +#endif + + const unsigned char * current = reinterpret_cast(data); + + // Slightly different implementations based on the parameters. The current implementations try to eliminate as much + // computation from the inner loop (looping over each bit) as possible. + if (parameters.reflectInput) + { + CRCType polynomial = CRC::Reflect(parameters.polynomial, CRCWidth); + while (size--) + { + remainder = static_cast(remainder ^ *current++); + + // An optimizing compiler might choose to unroll this loop. + for (crcpp_size i = 0; i < CHAR_BIT; ++i) + { +#ifdef CRCPP_BRANCHLESS + // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: + // if (remainder & 1) + // remainder = (remainder >> 1) ^ polynomial; + // else + // remainder >>= 1; + remainder = static_cast((remainder >> 1) ^ ((remainder & 1) * polynomial)); +#else + remainder = static_cast((remainder & 1) ? ((remainder >> 1) ^ polynomial) : (remainder >> 1)); +#endif + } + } + } + else if (CRCWidth >= CHAR_BIT) + { + static crcpp_constexpr CRCType CRC_WIDTH_MINUS_ONE(CRCWidth - CRCType(1)); +#ifndef CRCPP_BRANCHLESS + static crcpp_constexpr CRCType CRC_HIGHEST_BIT_MASK(CRCType(1) << CRC_WIDTH_MINUS_ONE); +#endif + // The conditional expression is used to avoid a -Wshift-count-overflow warning. + static crcpp_constexpr CRCType SHIFT((CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); + + while (size--) + { + remainder = static_cast(remainder ^ (static_cast(*current++) << SHIFT)); + + // An optimizing compiler might choose to unroll this loop. + for (crcpp_size i = 0; i < CHAR_BIT; ++i) + { +#ifdef CRCPP_BRANCHLESS + // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: + // if (remainder & CRC_HIGHEST_BIT_MASK) + // remainder = (remainder << 1) ^ parameters.polynomial; + // else + // remainder <<= 1; + remainder = static_cast((remainder << 1) ^ (((remainder >> CRC_WIDTH_MINUS_ONE) & 1) * parameters.polynomial)); +#else + remainder = static_cast((remainder & CRC_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ parameters.polynomial) : (remainder << 1)); +#endif + } + } + } + else + { + static crcpp_constexpr CRCType CHAR_BIT_MINUS_ONE(CHAR_BIT - 1); +#ifndef CRCPP_BRANCHLESS + static crcpp_constexpr CRCType CHAR_BIT_HIGHEST_BIT_MASK(CRCType(1) << CHAR_BIT_MINUS_ONE); +#endif + // The conditional expression is used to avoid a -Wshift-count-overflow warning. + static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); + + CRCType polynomial = static_cast(parameters.polynomial << SHIFT); + remainder = static_cast(remainder << SHIFT); + + while (size--) + { + remainder = static_cast(remainder ^ *current++); + + // An optimizing compiler might choose to unroll this loop. + for (crcpp_size i = 0; i < CHAR_BIT; ++i) + { +#ifdef CRCPP_BRANCHLESS + // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: + // if (remainder & CHAR_BIT_HIGHEST_BIT_MASK) + // remainder = (remainder << 1) ^ polynomial; + // else + // remainder <<= 1; + remainder = static_cast((remainder << 1) ^ (((remainder >> CHAR_BIT_MINUS_ONE) & 1) * polynomial)); +#else + remainder = static_cast((remainder & CHAR_BIT_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ polynomial) : (remainder << 1)); +#endif + } + } + + remainder = static_cast(remainder >> SHIFT); + } + + return remainder; +} + +/** + @brief Computes a CRC remainder using lookup table. + @param[in] data Data over which the remainder will be computed + @param[in] size Size of the data, in bytes + @param[in] lookupTable CRC lookup table + @param[in] remainder Running CRC remainder. Can be an initial value or the result of a previous CRC remainder calculation. + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC remainder +*/ +template +inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder) +{ + const unsigned char * current = reinterpret_cast(data); + + if (lookupTable.GetParameters().reflectInput) + { + while (size--) + { +#if defined(WIN32) || defined(_WIN32) || defined(WINCE) + // Disable warning about data loss when doing (remainder >> CHAR_BIT) when + // remainder is one byte long. The algorithm is still correct in this case, + // though it's possible that one additional machine instruction will be executed. +# pragma warning (push) +# pragma warning (disable : 4333) +#endif + remainder = static_cast((remainder >> CHAR_BIT) ^ lookupTable[static_cast(remainder ^ *current++)]); +#if defined(WIN32) || defined(_WIN32) || defined(WINCE) +# pragma warning (pop) +#endif + } + } + else if (CRCWidth >= CHAR_BIT) + { + // The conditional expression is used to avoid a -Wshift-count-overflow warning. + static crcpp_constexpr CRCType SHIFT((CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); + + while (size--) + { + remainder = static_cast((remainder << CHAR_BIT) ^ lookupTable[static_cast((remainder >> SHIFT) ^ *current++)]); + } + } + else + { + // The conditional expression is used to avoid a -Wshift-count-overflow warning. + static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); + + remainder = static_cast(remainder << SHIFT); + + while (size--) + { + // Note: no need to mask here since remainder is guaranteed to fit in a single byte. + remainder = lookupTable[static_cast(remainder ^ *current++)]; + } + + remainder = static_cast(remainder >> SHIFT); + } + + return remainder; +} + +template +inline CRCType CRC::CalculateRemainderBits(unsigned char byte, crcpp_size numBits, const Parameters & parameters, CRCType remainder) +{ + // Slightly different implementations based on the parameters. The current implementations try to eliminate as much + // computation from the inner loop (looping over each bit) as possible. + if (parameters.reflectInput) + { + CRCType polynomial = CRC::Reflect(parameters.polynomial, CRCWidth); + remainder = static_cast(remainder ^ byte); + + // An optimizing compiler might choose to unroll this loop. + for (crcpp_size i = 0; i < numBits; ++i) + { +#ifdef CRCPP_BRANCHLESS + // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: + // if (remainder & 1) + // remainder = (remainder >> 1) ^ polynomial; + // else + // remainder >>= 1; + remainder = static_cast((remainder >> 1) ^ ((remainder & 1) * polynomial)); +#else + remainder = static_cast((remainder & 1) ? ((remainder >> 1) ^ polynomial) : (remainder >> 1)); +#endif + } + } + else if (CRCWidth >= CHAR_BIT) + { + static crcpp_constexpr CRCType CRC_WIDTH_MINUS_ONE(CRCWidth - CRCType(1)); +#ifndef CRCPP_BRANCHLESS + static crcpp_constexpr CRCType CRC_HIGHEST_BIT_MASK(CRCType(1) << CRC_WIDTH_MINUS_ONE); +#endif + // The conditional expression is used to avoid a -Wshift-count-overflow warning. + static crcpp_constexpr CRCType SHIFT((CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); + + remainder = static_cast(remainder ^ (static_cast(byte) << SHIFT)); + + // An optimizing compiler might choose to unroll this loop. + for (crcpp_size i = 0; i < numBits; ++i) + { +#ifdef CRCPP_BRANCHLESS + // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: + // if (remainder & CRC_HIGHEST_BIT_MASK) + // remainder = (remainder << 1) ^ parameters.polynomial; + // else + // remainder <<= 1; + remainder = static_cast((remainder << 1) ^ (((remainder >> CRC_WIDTH_MINUS_ONE) & 1) * parameters.polynomial)); +#else + remainder = static_cast((remainder & CRC_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ parameters.polynomial) : (remainder << 1)); +#endif + } + } + else + { + static crcpp_constexpr CRCType CHAR_BIT_MINUS_ONE(CHAR_BIT - 1); +#ifndef CRCPP_BRANCHLESS + static crcpp_constexpr CRCType CHAR_BIT_HIGHEST_BIT_MASK(CRCType(1) << CHAR_BIT_MINUS_ONE); +#endif + // The conditional expression is used to avoid a -Wshift-count-overflow warning. + static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); + + CRCType polynomial = static_cast(parameters.polynomial << SHIFT); + remainder = static_cast((remainder << SHIFT) ^ byte); + + // An optimizing compiler might choose to unroll this loop. + for (crcpp_size i = 0; i < numBits; ++i) + { +#ifdef CRCPP_BRANCHLESS + // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: + // if (remainder & CHAR_BIT_HIGHEST_BIT_MASK) + // remainder = (remainder << 1) ^ polynomial; + // else + // remainder <<= 1; + remainder = static_cast((remainder << 1) ^ (((remainder >> CHAR_BIT_MINUS_ONE) & 1) * polynomial)); +#else + remainder = static_cast((remainder & CHAR_BIT_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ polynomial) : (remainder << 1)); +#endif + } + + remainder = static_cast(remainder >> SHIFT); + } + + return remainder; +} + +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS +/** + @brief Returns a set of parameters for CRC-4 ITU. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-4 ITU has the following parameters and check value: + - polynomial = 0x3 + - initial value = 0x0 + - final XOR = 0x0 + - reflect input = true + - reflect output = true + - check value = 0x7 + @return CRC-4 ITU parameters +*/ +inline const CRC::Parameters & CRC::CRC_4_ITU() +{ + static const Parameters parameters = { 0x3, 0x0, 0x0, true, true }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-5 EPC. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-5 EPC has the following parameters and check value: + - polynomial = 0x09 + - initial value = 0x09 + - final XOR = 0x00 + - reflect input = false + - reflect output = false + - check value = 0x00 + @return CRC-5 EPC parameters +*/ +inline const CRC::Parameters & CRC::CRC_5_EPC() +{ + static const Parameters parameters = { 0x09, 0x09, 0x00, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-5 ITU. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-5 ITU has the following parameters and check value: + - polynomial = 0x15 + - initial value = 0x00 + - final XOR = 0x00 + - reflect input = true + - reflect output = true + - check value = 0x07 + @return CRC-5 ITU parameters +*/ +inline const CRC::Parameters & CRC::CRC_5_ITU() +{ + static const Parameters parameters = { 0x15, 0x00, 0x00, true, true }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-5 USB. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-5 USB has the following parameters and check value: + - polynomial = 0x05 + - initial value = 0x1F + - final XOR = 0x1F + - reflect input = true + - reflect output = true + - check value = 0x19 + @return CRC-5 USB parameters +*/ +inline const CRC::Parameters & CRC::CRC_5_USB() +{ + static const Parameters parameters = { 0x05, 0x1F, 0x1F, true, true }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-6 CDMA2000-A. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-6 CDMA2000-A has the following parameters and check value: + - polynomial = 0x27 + - initial value = 0x3F + - final XOR = 0x00 + - reflect input = false + - reflect output = false + - check value = 0x0D + @return CRC-6 CDMA2000-A parameters +*/ +inline const CRC::Parameters & CRC::CRC_6_CDMA2000A() +{ + static const Parameters parameters = { 0x27, 0x3F, 0x00, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-6 CDMA2000-B. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-6 CDMA2000-A has the following parameters and check value: + - polynomial = 0x07 + - initial value = 0x3F + - final XOR = 0x00 + - reflect input = false + - reflect output = false + - check value = 0x3B + @return CRC-6 CDMA2000-B parameters +*/ +inline const CRC::Parameters & CRC::CRC_6_CDMA2000B() +{ + static const Parameters parameters = { 0x07, 0x3F, 0x00, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-6 ITU. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-6 ITU has the following parameters and check value: + - polynomial = 0x03 + - initial value = 0x00 + - final XOR = 0x00 + - reflect input = true + - reflect output = true + - check value = 0x06 + @return CRC-6 ITU parameters +*/ +inline const CRC::Parameters & CRC::CRC_6_ITU() +{ + static const Parameters parameters = { 0x03, 0x00, 0x00, true, true }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-6 NR. + @note The parameters are static and are delayed-constructed to reduce memory + footprint. + @note CRC-6 NR has the following parameters and check value: + - polynomial = 0x21 + - initial value = 0x00 + - final XOR = 0x00 + - reflect input = false + - reflect output = false + - check value = 0x15 + @return CRC-6 NR parameters +*/ +inline const CRC::Parameters & CRC::CRC_6_NR() +{ + static const Parameters parameters = { 0x21, 0x00, 0x00, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-7 JEDEC. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-7 JEDEC has the following parameters and check value: + - polynomial = 0x09 + - initial value = 0x00 + - final XOR = 0x00 + - reflect input = false + - reflect output = false + - check value = 0x75 + @return CRC-7 JEDEC parameters +*/ +inline const CRC::Parameters & CRC::CRC_7() +{ + static const Parameters parameters = { 0x09, 0x00, 0x00, false, false }; + return parameters; +} +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + +/** + @brief Returns a set of parameters for CRC-8 SMBus. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-8 SMBus has the following parameters and check value: + - polynomial = 0x07 + - initial value = 0x00 + - final XOR = 0x00 + - reflect input = false + - reflect output = false + - check value = 0xF4 + @return CRC-8 SMBus parameters +*/ +inline const CRC::Parameters & CRC::CRC_8() +{ + static const Parameters parameters = { 0x07, 0x00, 0x00, false, false }; + return parameters; +} + +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS +/** + @brief Returns a set of parameters for CRC-8 EBU (aka CRC-8 AES). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-8 EBU has the following parameters and check value: + - polynomial = 0x1D + - initial value = 0xFF + - final XOR = 0x00 + - reflect input = true + - reflect output = true + - check value = 0x97 + @return CRC-8 EBU parameters +*/ +inline const CRC::Parameters & CRC::CRC_8_EBU() +{ + static const Parameters parameters = { 0x1D, 0xFF, 0x00, true, true }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-8 MAXIM (aka CRC-8 DOW-CRC). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-8 MAXIM has the following parameters and check value: + - polynomial = 0x31 + - initial value = 0x00 + - final XOR = 0x00 + - reflect input = true + - reflect output = true + - check value = 0xA1 + @return CRC-8 MAXIM parameters +*/ +inline const CRC::Parameters & CRC::CRC_8_MAXIM() +{ + static const Parameters parameters = { 0x31, 0x00, 0x00, true, true }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-8 WCDMA. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-8 WCDMA has the following parameters and check value: + - polynomial = 0x9B + - initial value = 0x00 + - final XOR = 0x00 + - reflect input = true + - reflect output = true + - check value = 0x25 + @return CRC-8 WCDMA parameters +*/ +inline const CRC::Parameters & CRC::CRC_8_WCDMA() +{ + static const Parameters parameters = { 0x9B, 0x00, 0x00, true, true }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-8 LTE. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-8 LTE has the following parameters and check value: + - polynomial = 0x9B + - initial value = 0x00 + - final XOR = 0x00 + - reflect input = false + - reflect output = false + - check value = 0xEA + @return CRC-8 LTE parameters +*/ +inline const CRC::Parameters & CRC::CRC_8_LTE() +{ + static const Parameters parameters = { 0x9B, 0x00, 0x00, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-10 ITU. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-10 ITU has the following parameters and check value: + - polynomial = 0x233 + - initial value = 0x000 + - final XOR = 0x000 + - reflect input = false + - reflect output = false + - check value = 0x199 + @return CRC-10 ITU parameters +*/ +inline const CRC::Parameters & CRC::CRC_10() +{ + static const Parameters parameters = { 0x233, 0x000, 0x000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-10 CDMA2000. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-10 CDMA2000 has the following parameters and check value: + - polynomial = 0x3D9 + - initial value = 0x3FF + - final XOR = 0x000 + - reflect input = false + - reflect output = false + - check value = 0x233 + @return CRC-10 CDMA2000 parameters +*/ +inline const CRC::Parameters & CRC::CRC_10_CDMA2000() +{ + static const Parameters parameters = { 0x3D9, 0x3FF, 0x000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-11 FlexRay. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-11 FlexRay has the following parameters and check value: + - polynomial = 0x385 + - initial value = 0x01A + - final XOR = 0x000 + - reflect input = false + - reflect output = false + - check value = 0x5A3 + @return CRC-11 FlexRay parameters +*/ +inline const CRC::Parameters & CRC::CRC_11() +{ + static const Parameters parameters = { 0x385, 0x01A, 0x000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-11 NR. + @note The parameters are static and are delayed-constructed to reduce memory + footprint. + @note CRC-11 NR has the following parameters and check value: + - polynomial = 0x621 + - initial value = 0x000 + - final XOR = 0x000 + - reflect input = false + - reflect output = false + - check value = 0x5CA + @return CRC-11 NR parameters +*/ +inline const CRC::Parameters & CRC::CRC_11_NR() +{ + static const Parameters parameters = { 0x621, 0x000, 0x000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-12 CDMA2000. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-12 CDMA2000 has the following parameters and check value: + - polynomial = 0xF13 + - initial value = 0xFFF + - final XOR = 0x000 + - reflect input = false + - reflect output = false + - check value = 0xD4D + @return CRC-12 CDMA2000 parameters +*/ +inline const CRC::Parameters & CRC::CRC_12_CDMA2000() +{ + static const Parameters parameters = { 0xF13, 0xFFF, 0x000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-12 DECT (aka CRC-12 X-CRC). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-12 DECT has the following parameters and check value: + - polynomial = 0x80F + - initial value = 0x000 + - final XOR = 0x000 + - reflect input = false + - reflect output = false + - check value = 0xF5B + @return CRC-12 DECT parameters +*/ +inline const CRC::Parameters & CRC::CRC_12_DECT() +{ + static const Parameters parameters = { 0x80F, 0x000, 0x000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-12 UMTS (aka CRC-12 3GPP). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-12 UMTS has the following parameters and check value: + - polynomial = 0x80F + - initial value = 0x000 + - final XOR = 0x000 + - reflect input = false + - reflect output = true + - check value = 0xDAF + @return CRC-12 UMTS parameters +*/ +inline const CRC::Parameters & CRC::CRC_12_UMTS() +{ + static const Parameters parameters = { 0x80F, 0x000, 0x000, false, true }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-13 BBC. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-13 BBC has the following parameters and check value: + - polynomial = 0x1CF5 + - initial value = 0x0000 + - final XOR = 0x0000 + - reflect input = false + - reflect output = false + - check value = 0x04FA + @return CRC-13 BBC parameters +*/ +inline const CRC::Parameters & CRC::CRC_13_BBC() +{ + static const Parameters parameters = { 0x1CF5, 0x0000, 0x0000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-15 CAN. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-15 CAN has the following parameters and check value: + - polynomial = 0x4599 + - initial value = 0x0000 + - final XOR = 0x0000 + - reflect input = false + - reflect output = false + - check value = 0x059E + @return CRC-15 CAN parameters +*/ +inline const CRC::Parameters & CRC::CRC_15() +{ + static const Parameters parameters = { 0x4599, 0x0000, 0x0000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-15 MPT1327. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-15 MPT1327 has the following parameters and check value: + - polynomial = 0x6815 + - initial value = 0x0000 + - final XOR = 0x0001 + - reflect input = false + - reflect output = false + - check value = 0x2566 + @return CRC-15 MPT1327 parameters +*/ +inline const CRC::Parameters & CRC::CRC_15_MPT1327() +{ + static const Parameters parameters = { 0x6815, 0x0000, 0x0001, false, false }; + return parameters; +} +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + +/** + @brief Returns a set of parameters for CRC-16 ARC (aka CRC-16 IBM, CRC-16 LHA). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 ARC has the following parameters and check value: + - polynomial = 0x8005 + - initial value = 0x0000 + - final XOR = 0x0000 + - reflect input = true + - reflect output = true + - check value = 0xBB3D + @return CRC-16 ARC parameters +*/ +inline const CRC::Parameters & CRC::CRC_16_ARC() +{ + static const Parameters parameters = { 0x8005, 0x0000, 0x0000, true, true }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-16 BUYPASS (aka CRC-16 VERIFONE, CRC-16 UMTS). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 BUYPASS has the following parameters and check value: + - polynomial = 0x8005 + - initial value = 0x0000 + - final XOR = 0x0000 + - reflect input = false + - reflect output = false + - check value = 0xFEE8 + @return CRC-16 BUYPASS parameters +*/ +inline const CRC::Parameters & CRC::CRC_16_BUYPASS() +{ + static const Parameters parameters = { 0x8005, 0x0000, 0x0000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-16 CCITT FALSE. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 CCITT FALSE has the following parameters and check value: + - polynomial = 0x1021 + - initial value = 0xFFFF + - final XOR = 0x0000 + - reflect input = false + - reflect output = false + - check value = 0x29B1 + @return CRC-16 CCITT FALSE parameters +*/ +inline const CRC::Parameters & CRC::CRC_16_CCITTFALSE() +{ + static const Parameters parameters = { 0x1021, 0xFFFF, 0x0000, false, false }; + return parameters; +} + +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS +/** + @brief Returns a set of parameters for CRC-16 CDMA2000. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 CDMA2000 has the following parameters and check value: + - polynomial = 0xC867 + - initial value = 0xFFFF + - final XOR = 0x0000 + - reflect input = false + - reflect output = false + - check value = 0x4C06 + @return CRC-16 CDMA2000 parameters +*/ +inline const CRC::Parameters & CRC::CRC_16_CDMA2000() +{ + static const Parameters parameters = { 0xC867, 0xFFFF, 0x0000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-16 CMS. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 CMS has the following parameters and check value: + - polynomial = 0x8005 + - initial value = 0xFFFF + - final XOR = 0x0000 + - reflect input = false + - reflect output = false + - check value = 0xAEE7 + @return CRC-16 CMS parameters +*/ +inline const CRC::Parameters & CRC::CRC_16_CMS() +{ + static const Parameters parameters = { 0x8005, 0xFFFF, 0x0000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-16 DECT-R (aka CRC-16 R-CRC). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 DECT-R has the following parameters and check value: + - polynomial = 0x0589 + - initial value = 0x0000 + - final XOR = 0x0001 + - reflect input = false + - reflect output = false + - check value = 0x007E + @return CRC-16 DECT-R parameters +*/ +inline const CRC::Parameters & CRC::CRC_16_DECTR() +{ + static const Parameters parameters = { 0x0589, 0x0000, 0x0001, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-16 DECT-X (aka CRC-16 X-CRC). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 DECT-X has the following parameters and check value: + - polynomial = 0x0589 + - initial value = 0x0000 + - final XOR = 0x0000 + - reflect input = false + - reflect output = false + - check value = 0x007F + @return CRC-16 DECT-X parameters +*/ +inline const CRC::Parameters & CRC::CRC_16_DECTX() +{ + static const Parameters parameters = { 0x0589, 0x0000, 0x0000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-16 DNP. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 DNP has the following parameters and check value: + - polynomial = 0x3D65 + - initial value = 0x0000 + - final XOR = 0xFFFF + - reflect input = true + - reflect output = true + - check value = 0xEA82 + @return CRC-16 DNP parameters +*/ +inline const CRC::Parameters & CRC::CRC_16_DNP() +{ + static const Parameters parameters = { 0x3D65, 0x0000, 0xFFFF, true, true }; + return parameters; +} +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + +/** + @brief Returns a set of parameters for CRC-16 GENIBUS (aka CRC-16 EPC, CRC-16 I-CODE, CRC-16 DARC). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 GENIBUS has the following parameters and check value: + - polynomial = 0x1021 + - initial value = 0xFFFF + - final XOR = 0xFFFF + - reflect input = false + - reflect output = false + - check value = 0xD64E + @return CRC-16 GENIBUS parameters +*/ +inline const CRC::Parameters & CRC::CRC_16_GENIBUS() +{ + static const Parameters parameters = { 0x1021, 0xFFFF, 0xFFFF, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-16 KERMIT (aka CRC-16 CCITT, CRC-16 CCITT-TRUE). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 KERMIT has the following parameters and check value: + - polynomial = 0x1021 + - initial value = 0x0000 + - final XOR = 0x0000 + - reflect input = true + - reflect output = true + - check value = 0x2189 + @return CRC-16 KERMIT parameters +*/ +inline const CRC::Parameters & CRC::CRC_16_KERMIT() +{ + static const Parameters parameters = { 0x1021, 0x0000, 0x0000, true, true }; + return parameters; +} + +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS +/** + @brief Returns a set of parameters for CRC-16 MAXIM. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 MAXIM has the following parameters and check value: + - polynomial = 0x8005 + - initial value = 0x0000 + - final XOR = 0xFFFF + - reflect input = true + - reflect output = true + - check value = 0x44C2 + @return CRC-16 MAXIM parameters +*/ +inline const CRC::Parameters & CRC::CRC_16_MAXIM() +{ + static const Parameters parameters = { 0x8005, 0x0000, 0xFFFF, true, true }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-16 MODBUS. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 MODBUS has the following parameters and check value: + - polynomial = 0x8005 + - initial value = 0xFFFF + - final XOR = 0x0000 + - reflect input = true + - reflect output = true + - check value = 0x4B37 + @return CRC-16 MODBUS parameters +*/ +inline const CRC::Parameters & CRC::CRC_16_MODBUS() +{ + static const Parameters parameters = { 0x8005, 0xFFFF, 0x0000, true, true }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-16 T10-DIF. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 T10-DIF has the following parameters and check value: + - polynomial = 0x8BB7 + - initial value = 0x0000 + - final XOR = 0x0000 + - reflect input = false + - reflect output = false + - check value = 0xD0DB + @return CRC-16 T10-DIF parameters +*/ +inline const CRC::Parameters & CRC::CRC_16_T10DIF() +{ + static const Parameters parameters = { 0x8BB7, 0x0000, 0x0000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-16 USB. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 USB has the following parameters and check value: + - polynomial = 0x8005 + - initial value = 0xFFFF + - final XOR = 0xFFFF + - reflect input = true + - reflect output = true + - check value = 0xB4C8 + @return CRC-16 USB parameters +*/ +inline const CRC::Parameters & CRC::CRC_16_USB() +{ + static const Parameters parameters = { 0x8005, 0xFFFF, 0xFFFF, true, true }; + return parameters; +} + +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + +/** + @brief Returns a set of parameters for CRC-16 X-25 (aka CRC-16 IBM-SDLC, CRC-16 ISO-HDLC, CRC-16 B). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 X-25 has the following parameters and check value: + - polynomial = 0x1021 + - initial value = 0xFFFF + - final XOR = 0xFFFF + - reflect input = true + - reflect output = true + - check value = 0x906E + @return CRC-16 X-25 parameters +*/ +inline const CRC::Parameters & CRC::CRC_16_X25() +{ + static const Parameters parameters = { 0x1021, 0xFFFF, 0xFFFF, true, true }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-16 XMODEM (aka CRC-16 ZMODEM, CRC-16 ACORN, CRC-16 LTE). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 XMODEM has the following parameters and check value: + - polynomial = 0x1021 + - initial value = 0x0000 + - final XOR = 0x0000 + - reflect input = false + - reflect output = false + - check value = 0x31C3 + @return CRC-16 XMODEM parameters +*/ +inline const CRC::Parameters & CRC::CRC_16_XMODEM() +{ + static const Parameters parameters = { 0x1021, 0x0000, 0x0000, false, false }; + return parameters; +} + +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS +/** + @brief Returns a set of parameters for CRC-17 CAN. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-17 CAN has the following parameters and check value: + - polynomial = 0x1685B + - initial value = 0x00000 + - final XOR = 0x00000 + - reflect input = false + - reflect output = false + - check value = 0x04F03 + @return CRC-17 CAN parameters +*/ +inline const CRC::Parameters & CRC::CRC_17_CAN() +{ + static const Parameters parameters = { 0x1685B, 0x00000, 0x00000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-21 CAN. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-21 CAN has the following parameters and check value: + - polynomial = 0x102899 + - initial value = 0x000000 + - final XOR = 0x000000 + - reflect input = false + - reflect output = false + - check value = 0x0ED841 + @return CRC-21 CAN parameters +*/ +inline const CRC::Parameters & CRC::CRC_21_CAN() +{ + static const Parameters parameters = { 0x102899, 0x000000, 0x000000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-24 OPENPGP. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-24 OPENPGP has the following parameters and check value: + - polynomial = 0x864CFB + - initial value = 0xB704CE + - final XOR = 0x000000 + - reflect input = false + - reflect output = false + - check value = 0x21CF02 + @return CRC-24 OPENPGP parameters +*/ +inline const CRC::Parameters & CRC::CRC_24() +{ + static const Parameters parameters = { 0x864CFB, 0xB704CE, 0x000000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-24 FlexRay-A. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-24 FlexRay-A has the following parameters and check value: + - polynomial = 0x5D6DCB + - initial value = 0xFEDCBA + - final XOR = 0x000000 + - reflect input = false + - reflect output = false + - check value = 0x7979BD + @return CRC-24 FlexRay-A parameters +*/ +inline const CRC::Parameters & CRC::CRC_24_FLEXRAYA() +{ + static const Parameters parameters = { 0x5D6DCB, 0xFEDCBA, 0x000000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-24 FlexRay-B. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-24 FlexRay-B has the following parameters and check value: + - polynomial = 0x5D6DCB + - initial value = 0xABCDEF + - final XOR = 0x000000 + - reflect input = false + - reflect output = false + - check value = 0x1F23B8 + @return CRC-24 FlexRay-B parameters +*/ +inline const CRC::Parameters & CRC::CRC_24_FLEXRAYB() +{ + static const Parameters parameters = { 0x5D6DCB, 0xABCDEF, 0x000000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-24 LTE-A/NR-A. + @note The parameters are static and are delayed-constructed to reduce memory + footprint. + @note CRC-24 LTE-A has the following parameters and check value: + - polynomial = 0x864CFB + - initial value = 0x000000 + - final XOR = 0x000000 + - reflect input = false + - reflect output = false + - check value = 0xCDE703 + @return CRC-24 LTE-A parameters +*/ +inline const CRC::Parameters & CRC::CRC_24_LTEA() +{ + static const Parameters parameters = { 0x864CFB, 0x000000, 0x000000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-24 LTE-B/NR-B. + @note The parameters are static and are delayed-constructed to reduce memory + footprint. + @note CRC-24 LTE-B has the following parameters and check value: + - polynomial = 0x800063 + - initial value = 0x000000 + - final XOR = 0x000000 + - reflect input = false + - reflect output = false + - check value = 0x23EF52 + @return CRC-24 LTE-B parameters +*/ +inline const CRC::Parameters & CRC::CRC_24_LTEB() +{ + static const Parameters parameters = { 0x800063, 0x000000, 0x000000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-24 NR-C. + @note The parameters are static and are delayed-constructed to reduce memory + footprint. + @note CRC-24 NR-C has the following parameters and check value: + - polynomial = 0xB2B117 + - initial value = 0x000000 + - final XOR = 0x000000 + - reflect input = false + - reflect output = false + - check value = 0xF48279 + @return CRC-24 NR-C parameters +*/ +inline const CRC::Parameters & CRC::CRC_24_NRC() +{ + static const Parameters parameters = { 0xB2B117, 0x000000, 0x000000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-30 CDMA. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-30 CDMA has the following parameters and check value: + - polynomial = 0x2030B9C7 + - initial value = 0x3FFFFFFF + - final XOR = 0x00000000 + - reflect input = false + - reflect output = false + - check value = 0x3B3CB540 + @return CRC-30 CDMA parameters +*/ +inline const CRC::Parameters & CRC::CRC_30() +{ + static const Parameters parameters = { 0x2030B9C7, 0x3FFFFFFF, 0x00000000, false, false }; + return parameters; +} +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + +/** + @brief Returns a set of parameters for CRC-32 (aka CRC-32 ADCCP, CRC-32 PKZip). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-32 has the following parameters and check value: + - polynomial = 0x04C11DB7 + - initial value = 0xFFFFFFFF + - final XOR = 0xFFFFFFFF + - reflect input = true + - reflect output = true + - check value = 0xCBF43926 + @return CRC-32 parameters +*/ +inline const CRC::Parameters & CRC::CRC_32() +{ + static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, true, true }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-32 BZIP2 (aka CRC-32 AAL5, CRC-32 DECT-B, CRC-32 B-CRC). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-32 BZIP2 has the following parameters and check value: + - polynomial = 0x04C11DB7 + - initial value = 0xFFFFFFFF + - final XOR = 0xFFFFFFFF + - reflect input = false + - reflect output = false + - check value = 0xFC891918 + @return CRC-32 BZIP2 parameters +*/ +inline const CRC::Parameters & CRC::CRC_32_BZIP2() +{ + static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, false, false }; + return parameters; +} + +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS +/** + @brief Returns a set of parameters for CRC-32 C (aka CRC-32 ISCSI, CRC-32 Castagnoli, CRC-32 Interlaken). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-32 C has the following parameters and check value: + - polynomial = 0x1EDC6F41 + - initial value = 0xFFFFFFFF + - final XOR = 0xFFFFFFFF + - reflect input = true + - reflect output = true + - check value = 0xE3069283 + @return CRC-32 C parameters +*/ +inline const CRC::Parameters & CRC::CRC_32_C() +{ + static const Parameters parameters = { 0x1EDC6F41, 0xFFFFFFFF, 0xFFFFFFFF, true, true }; + return parameters; +} +#endif + +/** + @brief Returns a set of parameters for CRC-32 MPEG-2. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-32 MPEG-2 has the following parameters and check value: + - polynomial = 0x04C11DB7 + - initial value = 0xFFFFFFFF + - final XOR = 0x00000000 + - reflect input = false + - reflect output = false + - check value = 0x0376E6E7 + @return CRC-32 MPEG-2 parameters +*/ +inline const CRC::Parameters & CRC::CRC_32_MPEG2() +{ + static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0x00000000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-32 POSIX. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-32 POSIX has the following parameters and check value: + - polynomial = 0x04C11DB7 + - initial value = 0x00000000 + - final XOR = 0xFFFFFFFF + - reflect input = false + - reflect output = false + - check value = 0x765E7680 + @return CRC-32 POSIX parameters +*/ +inline const CRC::Parameters & CRC::CRC_32_POSIX() +{ + static const Parameters parameters = { 0x04C11DB7, 0x00000000, 0xFFFFFFFF, false, false }; + return parameters; +} + +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS +/** + @brief Returns a set of parameters for CRC-32 Q. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-32 Q has the following parameters and check value: + - polynomial = 0x814141AB + - initial value = 0x00000000 + - final XOR = 0x00000000 + - reflect input = false + - reflect output = false + - check value = 0x3010BF7F + @return CRC-32 Q parameters +*/ +inline const CRC::Parameters & CRC::CRC_32_Q() +{ + static const Parameters parameters = { 0x814141AB, 0x00000000, 0x00000000, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-40 GSM. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-40 GSM has the following parameters and check value: + - polynomial = 0x0004820009 + - initial value = 0x0000000000 + - final XOR = 0xFFFFFFFFFF + - reflect input = false + - reflect output = false + - check value = 0xD4164FC646 + @return CRC-40 GSM parameters +*/ +inline const CRC::Parameters & CRC::CRC_40_GSM() +{ + static const Parameters parameters = { 0x0004820009, 0x0000000000, 0xFFFFFFFFFF, false, false }; + return parameters; +} + +/** + @brief Returns a set of parameters for CRC-64 ECMA. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-64 ECMA has the following parameters and check value: + - polynomial = 0x42F0E1EBA9EA3693 + - initial value = 0x0000000000000000 + - final XOR = 0x0000000000000000 + - reflect input = false + - reflect output = false + - check value = 0x6C40DF5F0B497347 + @return CRC-64 ECMA parameters +*/ +inline const CRC::Parameters & CRC::CRC_64() +{ + static const Parameters parameters = { 0x42F0E1EBA9EA3693, 0x0000000000000000, 0x0000000000000000, false, false }; + return parameters; +} +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + +#ifdef CRCPP_USE_NAMESPACE +} +#endif + +#endif // CRCPP_CRC_H_ diff --git a/src/Prometheus/Modules/communication/shard/include/Struct.hpp b/src/Prometheus/Modules/communication/shard/include/Struct.hpp new file mode 100644 index 00000000..806d761d --- /dev/null +++ b/src/Prometheus/Modules/communication/shard/include/Struct.hpp @@ -0,0 +1,900 @@ +#ifndef STRUCT_HPP +#define STRUCT_HPP + +#include +#include // NULL +#include +#include +#include +#include + +#include +#include +#include + +//uav control +#define OPENUAVBASIC "gnome-terminal -- roslaunch prometheus_uav_control uav_control_main_outdoor.launch" +// #define CLOSEUAVBASIC "gnome-terminal -- rosnode kill /joy_node | gnome-terminal -- rosnode kill /uav_control_main_1" +//rhea control +#define OPENUGVBASIC "" +#define CLOSEUGVBASIC "" +//集群 +#define OPENSWARMCONTROL "" +#define CLOSESWARMCONTROL "" +//自主降落 +#define OPENAUTONOMOUSLANDING "" +#define CLOSEAUTONOMOUSLANDING "" +//目标识别与追踪 +#define OPENOBJECTTRACKING "" +#define CLOSEOBJECTTRACKING "" +//杀掉除了通信节点和主节点的其他节点 +//分为两种情况 +//1:杀掉子模块，这种情况不会杀掉uav control节点和通信节点以及master节点。 +//2:杀掉uav control节点，这种情况下只会保留通信节点以及master节点。 +#define CLOSEUAVBASIC "gnome-terminal -- rosnode kill `rosnode list | grep -v /ground_station_bridge | grep -v /rosout`" +#define CLOSEOTHERMODE "gnome-terminal -- rosnode kill `rosnode list | grep -v /ground_station_bridge | grep -v /rosout | grep -v /uav_control_main_1 | grep -v /joy_node`" + +//重启 +#define REBOOTNXCMD "shutdown -r now" +//关机 +#define EXITNXCMD "shutdown -h now" + + +enum MsgId +{ + UAVSTATE = 1, + TEXTINFO = 3, + GIMBALSTATE = 4, + VISIONDIFF = 5, + HEARTBEAT = 6, + RHEASTATE = 7, + MULTIDETECTIONINFO = 8, + UAVCONTROLSTATE = 9, + + SWARMCOMMAND = 101, + GIMBALCONTROL = 102, + GIMBALSERVICE = 103, + WINDOWPOSITION = 104, + RHEACONTROL = 105, + GIMBALPARAMSET = 106, + IMAGEDATA = 107, + UAVCOMMAND = 108, + UAVSETUP = 109, + + CONNECTSTATE = 201, + MODESELECTION = 202, + + //rviz数据 + UGVLASERSCAN = 230, + UGVPOINTCLOUND2 = 231, + + UGVTFMESSAGE = 232, + UGVTFSTATIC = 233, + + UGVMARKERARRAY = 234, + UGVMARKERARRAYLANDMARK = 235, + UGVMARKERARRAYTRAJECTORY = 236 +}; + +//参考文件： UAVState.msg +//订阅话题： /uav*/prometheus/state +struct Quaternion +{ + double x; + double y; + double z; + double w; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &x; + ar &y; + ar &z; + ar &w; + } +}; +//MSG 1 +struct UAVState +{ + //无人机编号 + uint8_t uav_id; + + // //无人机状态 + // uint8_t state; + // //enum agent状态枚举 + // enum State + // { + // unknown = 0, + // ready = 1, + // dead = 2, + // lost = 3 + // }; + + //无人机定位来源 + uint8_t location_source; + + //enum agent定位来源枚举 + enum LocationSource + { + MOCAP = 0, + T265 = 1, + GAZEBO = 2, + FAKE_ODOM = 3, + GPS = 4, + RTK = 5, + UWB = 6 + }; + + // PX4飞控当前飞行模式 int8 + std::string mode; + // 机载电脑是否连接上飞控，true已连接，false则不是 + bool connected; + // 是否解锁，true为已解锁，false则不是 + bool armed; + // odom失效 + bool odom_valid; + // GPS状态,变量对应状态可参考mavros_msgs/GPSRAW中的fix_type + uint8_t gps_status; + // GPS状态枚举 + enum GPSStatus + { + GPS_FIX_TYPE_NO_GPS = 0, + GPS_FIX_TYPE_NO_FIX = 1, + GPS_FIX_TYPE_2D_FIX = 2, + GPS_FIX_TYPE_3D_FIX = 3, + GPS_FIX_TYPE_DGPS = 4, + GPS_FIX_TYPE_RTK_FLOATR = 5, + GPS_FIX_TYPE_RTK_FIXEDR = 6, + GPS_FIX_TYPE_STATIC = 7, + GPS_FIX_TYPE_PPP = 8 + }; + + //无人机经度、纬度、高度 + float latitude; + float longitude; + float altitude; + + // 无人机状态量：位置、速度、姿态 + float position[3]; // [m] + float velocity[3]; // [m/s] + float attitude[3]; // [rad] + Quaternion attitude_q; // 四元数 + float attitude_rate[3]; // [rad/s] + float battery_state; // 电池状态[v] + float battery_percetage; // [0-1] + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &uav_id; + // ar &state; + ar &location_source; + ar &connected; + ar &mode; + ar &armed; + ar &odom_valid; + ar &gps_status; + ar &latitude; + ar &longitude; + ar &altitude; + ar &position; + ar &velocity; + ar &attitude; + ar &attitude_q; + ar &attitude_rate; + ar &battery_state; + ar &battery_percetage; + } +}; + +struct Heartbeat +{ + uint32_t count; + std::string message; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &count; + ar &message; + } +}; + +struct RheaState +{ + uint8_t rhea_id; + double linear; + double angular; + double yaw; + float latitude; + float longitude; + float altitude; + float position[3]; + float battery_voltage; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &rhea_id; + ar &linear; + ar &angular; + ar &yaw; + ar &latitude; + ar &longitude; + ar &altitude; + ar &position; + ar &battery_voltage; + } +}; + +struct ModeSelection +{ + uint8_t mode; + enum Mode + { + UAVBASIC = 1, + UGVBASIC = 2, + SWARMCONTROL = 3, + //GIMBAL? + AUTONOMOUSLANDING = 4, + OBJECTTRACKING = 5, + CUSTOMMODE = 6, + REBOOTNX = 7, + EXITNX = 8 + }; +// bool is_simulation; + std::vector selectId; + // uint8_t swarm_num; + + uint8_t use_mode; + enum UseMode + { + CREATE = 0, + DELETE = 1 + }; + + bool is_simulation; + + int swarm_num; + + std::string cmd; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &selectId; + ar &mode; + ar &use_mode; + ar &is_simulation; + ar &swarm_num; + ar &cmd; + } +}; + +struct Point +{ + double x; + double y; + double z; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &x; + ar &y; + ar &z; + } +}; + +// SwarmCommand.msg +// /prometheus/swarm_command +struct SwarmCommand +{ + // 消息来源 + std::string source; + + //编队套件数量 + uint8_t swarm_num; + + //定位来源 + uint8_t swarm_location_source; + enum SwarmLocationSource + { + mocap = 0, + gps = 4, + rtk = 5, + uwb = 6 + }; + + // 命令 + uint8_t Swarm_CMD; + // enum CMD 控制模式枚举 + // enum CMD 控制模式枚举 + enum SwarmCMD + { + Ready = 0, + Init = 1, + Start = 2, + Hold = 3, + Stop = 4, + Formation = 5, + Follow = 11, + Search = 12, + Attack = 13 + }; + + // 编队控制参考量 + float leader_pos[3]; + float leader_vel[2]; + float swarm_size; + uint8_t swarm_shape; + enum SwarmShape + { + One_column = 0, + Triangle = 1, + Square = 2, + Circular = 3 + }; + + // 搜索控制参考量 + float target_area_x_min; // [m] + float target_area_y_min; // [m] + float target_area_x_max; // [m] + float target_area_y_max; // [m] + + // 攻击控制参考量 + float attack_target_pos[3]; // [m] + + std::vector formation_poses; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &source; + ar &swarm_num; + ar &swarm_location_source; + ar &Swarm_CMD; + ar &leader_pos; + ar &leader_vel; + ar &swarm_size; + ar &swarm_shape; + ar &target_area_x_min; + ar &target_area_y_min; + ar &target_area_x_max; + ar &target_area_y_max; + ar &attack_target_pos; + ar &formation_poses; + } +}; + +// StationFeedback.msg +// /uav*/prometheus/station_feedback +struct TextInfo +{ + enum MessageTypeGrade + { + //INFO:正常运行状况下反馈给地面站的信息,例如程序正常启动,状态切换的提示信息等. + INFO = 0, + //WARN:无人机或软件程序出现意外情况,依然能正常启动或继续执行任务,小概率会出现危险状况,例如无人机RTK无法维持退出到GPS,视觉跟踪目标突然丢失重新搜寻目标等. + WARN = 1, + //ERROR:无人机或软件程序出现重大意外情况,无法正常启动或继续执行任务,极有可能会出现危险状况,需要中断任务以及人为接管控制无人机,例如通信中断,无人机定位发散,ROS节点无法正常启动等. + ERROR = 2, + //FATAL:任务执行过程中,软件崩溃或无人机飞控崩溃导致无人机完全失控,需要迅速人为接管控制无人机降落减少炸机损失. + FATAL = 3 + }; + + int sec; + uint8_t MessageType; + std::string Message; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &sec; + ar &MessageType; + ar &Message; + } +}; + +//参考文件： GimbalState.msg +//订阅话题： ~/gimbal/state +struct GimbalState +{ + uint8_t Id; + //# 0: 发一句回一句 `# 1: 发一句，一直回复, 摄像头倍数返回将失效`` + uint8_t feedbackMode; + //mode 0:手动控制 1:home 2:tracking 3:yaw follow 4:hold吊舱跟随无人机 5:search + uint8_t mode; + //是否视频录制 + bool isRecording; + //# 是否开启自动缩放(VisionDiff需要指定面积才能生效) 0:保持 1:放大 2:缩小 3:自动 + uint8_t zoomState; + // 当前所处倍数 + float zoomVal; + //roll,pitch,yaw + float imuAngle[3]; + //Current gimbal joint angles(roll,pitch,yaw), published at 30 Hz. + float rotorAngle[3]; + //rpy_vel 角速度 + float imuAngleVel[3]; + //rpy_tgt 目标角度 + float rotorAngleTarget[3]; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &Id; + ar &feedbackMode; + ar &mode; + ar &isRecording; + ar &zoomState; + ar &zoomVal; + ar &imuAngle; + ar &rotorAngle; + ar &imuAngleVel; + ar &rotorAngleTarget; + } +}; +//参考文件： VisionDiff.msg +//订阅话题： ~/gimbal/track +struct VisionDiff +{ + uint8_t id; + uint8_t detect; + + uint16_t objectX; + uint16_t objectY; + uint16_t objectWidth; + uint16_t objectHeight; + + uint16_t frameWidth; + uint16_t frameHeight; + + // Gimbal 跟踪pid + float kp; + float ki; + float kd; + + int8_t ctlMode; // 0: yaw+pitch, 1: roll+pitch 3:混合(未实现) + enum CtlMode + { + yawPitch = 0, + rollPitch = 1, + mix = 3 + }; + + float currSize; //框选近大远小 + float maxSize; + float minSize; //框选大小 + + float trackIgnoreError; + bool autoZoom; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &id; + ar &objectX; + ar &objectY; + ar &objectWidth; + ar &objectHeight; + ar &frameWidth; + ar &frameHeight; + ar &kp; + ar &ki; + ar &kd; + ar &ctlMode; + ar &currSize; + ar &maxSize; + ar &minSize; + ar &trackIgnoreError; + ar &autoZoom; + } +}; + +//参考文件： GimbalControl.msg +//订阅话题： ~/gimbal/control +struct GimbalControl +{ + uint8_t Id; + //control mode 0:nothong 1:angle 2:speed 3:home postion + uint8_t rpyMode; + enum RPYMode + { + manual = 1, + home = 2, + hold = 3, + fellow = 4 + }; + + uint8_t roll; + uint8_t yaw; + uint8_t pitch; + + enum ControlMode + { + noCtl = 0, + velocityCtl = 1, + angleCtl = 2 + }; + + float rValue; // deg 单位 + float yValue; // deg + float pValue; // deg + + // focus + uint8_t focusMode; // 默认值 + enum FocusMode + { + focusStop = 1, + focusOut = 2, + focusIn = 3 + }; + + // zoom + uint8_t zoomMode; // 默认值 + enum ZoomMode + { + zoomStop = 1, + zoomOut = 2, + zoomIn = 3 + }; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &Id; + ar &rpyMode; + ar &roll; + ar &yaw; + ar &pitch; + ar &rValue; + ar &yValue; + ar &pValue; + ar &focusMode; + ar &zoomMode; + } +}; + +struct GimbalService +{ + uint8_t service; + enum Service + { + search = 1, + record_video = 2, + track_mode = 3 + }; + + bool data; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &service; + ar &data; + } +}; + +struct GimbalParamSet +{ + std::string param_id; + double real; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar ¶m_id; + ar ℜ + } +}; + +struct WindowPosition +{ + //模式：... + uint8_t mode; + enum Mode + { + IDLE = 0, + RECTANGLE = 1, + POINT = 2, + TRACK_ID = 3, + FRAME_ID_AND_POINT = 4 + }; + + //波门位置X,//波门位置Y(kcf,点击画面的功能的时候使用),左上角为（0,0） + int16_t origin_x; + int16_t origin_y; + int16_t width; + int16_t height; + + //波门位置X,//波门位置Y + //int16 window_position_x = origin_x + width/2 + //int16 window_position_y = origin_y + height/2 + int16_t window_position_x; + int16_t window_position_y; + + //算法检测结果的ID + int32_t track_id; + + // //被点击或框选帧（画面暂停的ID） + // int32_t frame_id; + + std::string udp_msg; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &mode; + ar &origin_x; + ar &origin_y; + ar &width; + ar &height; + ar &window_position_x; + ar &window_position_y; + ar &track_id; + // ar &frame_id; + ar &udp_msg; + } +}; + +//ROS话题: "/deepsort_ros/object_detection_result" +struct DetectionInfo +{ + //目标框的位置（主要斜对角两个点） + float left; + float top; + float bot; + float right; + + //TRACK TARGET + int32_t trackIds; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &left; + ar ⊤ + ar ⊥ + ar &right; + ar &trackIds; + } +}; +struct MultiDetectionInfo +{ + //模式：0：空闲 2.simaRPN 3.deepsort/sort + uint8_t mode; + + //检测到的目标数量 + int32_t num_objs; + + //每个目标的检测结果 + //DetectionInfo[] detection_infos; + std::vector detection_infos; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &mode; + ar &num_objs; + ar &detection_infos; + } +}; + +struct RheaGPS +{ + double latitude; + double longitude; + double altitude; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &latitude; + ar &longitude; + ar &altitude; + } +}; + +struct RheaControl +{ + + uint8_t Mode; + + //控制模式类型枚举 + enum Mode + { + Stop = 0, + Forward = 1, + Left = 2, + Right = 3, + Back = 4, + CMD = 5, + Waypoint = 6 + }; + + double linear; + double angular; + + std::vector waypoint; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &Mode; + ar &linear; + ar &angular; + ar &waypoint; + } +}; + +struct ImageData +{ + std::string name; + std::string data; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &name; + ar &data; + } +}; + +struct UAVCommand +{ + //控制命令的模式 + uint8_t Agent_CMD; + //Agent_CMD 枚举 + enum AgentCMD + { + Init_Pos_Hover = 1, + Current_Pos_Hover = 2, + Land = 3, + Move = 4, + User_Mode1 = 5 + }; + + //移动命令下的子模式 + uint8_t Move_mode; + // 移动命令下的子模式枚举 + enum MoveMode + { + XYZ_POS = 0, // 惯性系定点控制 + XY_VEL_Z_POS = 1, // 惯性系定高速度控制 + XYZ_VEL = 2, // 惯性系速度控制 + XYZ_POS_BODY = 3, // 机体系位置控制 + XYZ_VEL_BODY = 4, // 机体系速度控制 + XY_VEL_Z_POS_BODY = 5, // 机体系定高速度控制 + TRAJECTORY = 6, // 轨迹追踪控制 + XYZ_ATT = 7, // 姿态控制（来自外部控制器） + LAT_LON_ALT = 8 // 绝对坐标系下的经纬度 + }; + // 控制参考量 + float position_ref[3]; // [m] + float velocity_ref[3]; // [m/s] + float acceleration_ref[3]; // [m/s^2] + float yaw_ref; // [rad] + bool Yaw_Rate_Mode; // True 代表控制偏航角速率 + float yaw_rate_ref; // [rad/s] + float att_ref[4]; // [rad] + [0-1] + double latitude; // 无人机经度、纬度、高度 + double longitude; // 无人机经度、纬度、高度 + double altitude; // 无人机经度、纬度、高度 + + // 控制命令的编号 防止接收到错误命令， 编号应该逐次递加 + uint Command_ID; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &Agent_CMD; + ar &Move_mode; + ar &position_ref; + ar &velocity_ref; + ar &acceleration_ref; + ar &yaw_ref; + ar &Yaw_Rate_Mode; + ar &yaw_rate_ref; + ar &att_ref; + ar &Command_ID; + ar &latitude; + ar &longitude; + ar &altitude; + } +}; + +struct UAVSetup +{ + //Setup类型(模拟遥控器) + uint8_t cmd; + enum CMD + { + ARMING = 0, + SET_PX4_MODE = 1, + REBOOT_PX4 = 2, + SET_CONTROL_MODE = 3, + }; + + bool arming; + // http://wiki.ros.org/mavros/CustomModes ,可参考该网址设置模式名,常用模式名:OFFBOARD,AUTO.LAND,AUTO.RTL,POSCTL + std::string px4_mode; + std::string control_state; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &cmd; + ar &arming; + ar &px4_mode; + ar &control_state; + } +}; + +struct UAVControlState +{ + // 无人机编号 + uint8_t uav_id; + + // 无人机控制状态 + uint8_t control_state; + // 状态枚举 + enum ControlState + { + INIT = 0, + MANUAL_CONTROL = 1, + HOVER_CONTROL = 2, + COMMAND_CONTROL = 3, + LAND_CONTROL = 4 + }; + + // 无人机控制器标志量 + uint8_t pos_controller; + // 状态枚举 + enum ControllerFlag + { + PX4_ORIGIN = 0, + PID = 1, + UDE = 2, + NE = 3 + }; + + // 无人机安全保护触发标志量 + bool failsafe; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar &uav_id; + ar &control_state; + ar &pos_controller; + ar &failsafe; + } +}; + +struct ConnectState +{ + uint8_t num; + bool state; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /* file_version */) + { + ar # + ar &state; + } +}; + +#endif \ No newline at end of file diff --git a/src/Prometheus/Modules/communication/shard/include/communication.hpp b/src/Prometheus/Modules/communication/shard/include/communication.hpp new file mode 100644 index 00000000..150355fa --- /dev/null +++ b/src/Prometheus/Modules/communication/shard/include/communication.hpp @@ -0,0 +1,98 @@ +#ifndef COMUNICATION_HPP +#define COMUNICATION_HPP + +#include +#include +#include +#include +#include +#include + +#include "CRC.h" +#include "Struct.hpp" + + +#define BUF_LEN 1024 * 10 // 1024*10 bytes +#define SERV_PORT 20168 + +typedef unsigned char byte; + +enum Send_Mode +{ + TCP = 1, + UDP = 2 +}; + +class Communication +{ +public: + Communication(); + ~Communication(); + + void init(int id, int udp_port, int tcp_port, int tcp_heart_port); + + //编码 + template + int encodeMsg(int8_t send_mode, T msg); + + //解码 + int decodeMsg(char *buff); + + //根据传入的struct返回对应的MSG_ID + template + uint8_t getMsgId(T msg); + + template + T add(T num1,T num2); + + // UDP client + int connectToUdpMulticast(const char *ip, const int port); + + // TCP client 返回套接字 + int connectToDrone(const char *ip, const int port); + + void sendMsgByUdp(int msg_len, std::string target_ip); + + void sendMsgByUdp(int msg_len, const char* udp_msg ,std::string target_ip, int port); + + void sendMsgByTcp(int msg_len, std::string target_ip); + + // TCP server + int waitConnectionFromGroundStation(const int port); + + // UDP server + int waitConnectionFromMulticast(const int port); + + unsigned short checksum(char *buff, int len); + +protected: + int ROBOT_ID = 0; + + // tcp/udp + struct sockaddr_in tcp_addr, udp_addr; + int tcp_send_sock, udp_send_sock, server_fd, udp_fd, recv_sock, udp_socket, rviz_socket, UDP_PORT, TCP_PORT, TCP_HEARTBEAT_PORT; + char udp_send_buf[BUF_LEN], udp_recv_buf[BUF_LEN], tcp_send_buf[BUF_LEN], tcp_recv_buf[BUF_LEN]; + std::string udp_ip, multicast_udp_ip; + + int try_connect_num = 0, disconnect_num = 0; + +public: + struct SwarmCommand recv_swarm_command_; + struct UAVState recv_uav_state_; + struct ConnectState recv_connect_state_; + struct GimbalControl recv_gimbal_control_; + struct ModeSelection recv_mode_selection_; + struct GimbalService recv_gimbal_service_; + struct WindowPosition recv_window_position_; + struct RheaControl recv_rhea_control_; + struct GimbalParamSet recv_param_set_; + struct RheaState recv_rhea_state_; + struct ImageData recv_image_data_; + struct UAVCommand recv_uav_cmd_; + struct UAVSetup recv_uav_setup_; + struct TextInfo recv_text_info_; + struct GimbalState recv_gimbal_state_; + struct VisionDiff recv_vision_diff_; +}; + +#endif \ No newline at end of file diff --git a/src/Prometheus/Modules/communication/shard/libs/libcommunication.so b/src/Prometheus/Modules/communication/shard/libs/libcommunication.so new file mode 100755 index 00000000..35766967 Binary files /dev/null and b/src/Prometheus/Modules/communication/shard/libs/libcommunication.so differ diff --git a/src/Prometheus/Modules/communication/src/autonomous_landing_topic.cpp b/src/Prometheus/Modules/communication/src/autonomous_landing_topic.cpp new file mode 100644 index 00000000..ff6b308d --- /dev/null +++ b/src/Prometheus/Modules/communication/src/autonomous_landing_topic.cpp @@ -0,0 +1,76 @@ +#include "autonomous_landing_topic.hpp" + +AutonomousLanding::AutonomousLanding(ros::NodeHandle &nh,Communication *communication) +{ + nh.param("ROBOT_ID", robot_id, 0); + nh.param("multicast_udp_ip", multicast_udp_ip, "224.0.0.88"); + + this->communication_ = communication; + + //prefix.c_str() + std::to_string(robot_id) + + //【服务】是否开启搜索 + this->gimbal_search_client_ = nh.serviceClient("/gimbal/search"); + //【服务】是否开启视频录制 + this->gimbal_record_video_client_ = nh.serviceClient("/gimbal/record_video"); + //【服务】是否自动反馈(真实IMU速度) + this->gimbal_track_mode_client_ = nh.serviceClient("/gimbal/track_mode"); + //【服务】自主降落参数配置 + this->param_set_client_ = nh.serviceClient("/autonomous_landing/ParamSet"); + //【发布】无人车数据 + this->ugv_state_pub_ = nh.advertise("/ugv1/prometheus/state", 1000); + + +}; + +AutonomousLanding::~AutonomousLanding() +{ + // delete this->communication_; +}; + +void AutonomousLanding::gimbalSearchServer(bool is) +{ + std_srvs::SetBool set_bool; + set_bool.request.data = is; + + this->gimbal_search_client_.call(set_bool); +} + +void AutonomousLanding::gimbalRecordVideoServer(bool is) +{ + std_srvs::SetBool set_bool; + set_bool.request.data = is; + this->gimbal_record_video_client_.call(set_bool); +} + +void AutonomousLanding::gimbalTrackModeServer(bool is) +{ + std_srvs::SetBool set_bool; + set_bool.request.data = is; + this->gimbal_track_mode_client_.call(set_bool); +} + +void AutonomousLanding::gimbalParamSetServer(struct GimbalParamSet param_set) +{ + mavros_msgs::ParamSet srv; + srv.request.param_id = param_set.param_id; + srv.request.value.real = param_set.real; + this->param_set_client_.call(srv); +} + +void AutonomousLanding::rheaStatePub(struct RheaState rhea_state) +{ + prometheus_msgs::RheaState rhea_state_; + rhea_state_.rhea_id = rhea_state.rhea_id; + rhea_state_.angular = rhea_state.angular; + rhea_state_.linear = rhea_state.linear; + rhea_state_.yaw = rhea_state.yaw; + rhea_state_.latitude = rhea_state.latitude; + rhea_state_.longitude = rhea_state.longitude; + rhea_state_.battery_voltage = rhea_state.battery_voltage; + rhea_state_.altitude = rhea_state.altitude; + for(int i = 0; i < 3; i++) + { + rhea_state_.position[i] = rhea_state.position[i]; + } + this->ugv_state_pub_.publish(rhea_state_); +} \ No newline at end of file diff --git a/src/Prometheus/Modules/communication/src/communication_bridge.cpp b/src/Prometheus/Modules/communication/src/communication_bridge.cpp new file mode 100644 index 00000000..6ebed95e --- /dev/null +++ b/src/Prometheus/Modules/communication/src/communication_bridge.cpp @@ -0,0 +1,843 @@ +#include "communication_bridge.hpp" +#include +#include +std::mutex g_m; +std::mutex g_uav_basic; +// boost::shared_mutex g_m; + +#define DEMO1 "gnome-terminal -- roslaunch prometheus_demo takeoff_land.launch" + +CommunicationBridge::CommunicationBridge(ros::NodeHandle &nh) : Communication() +{ + //是否仿真 1 为是 0为否 + nh.param("is_simulation", this->is_simulation_, 1); + //集群数量 非集群模式值为0 + nh.param("swarm_num", this->swarm_num_, 0); + //载体类型 + nh.param("user_type", this->user_type_, 1); + //集群模式下数据更新超时多久进行反馈 + nh.param("swarm_data_update_timeout", this->swarm_data_update_timeout_, 5); + + nh.param("udp_port", UDP_PORT, 8889); + nh.param("tcp_port", TCP_PORT, 55555); + nh.param("tcp_heartbeat_port", TCP_HEARTBEAT_PORT, 55556); + // nh.param("rviz_port", RVIZ_PORT, 8890); + nh.param("ROBOT_ID", ROBOT_ID, 1); + nh.param("ground_stationt_ip", udp_ip, "127.0.0.1"); + nh.param("multicast_udp_ip", multicast_udp_ip, "224.0.0.88"); + nh.param("try_connect_num", try_connect_num, 3); + + this->nh_ = nh; + + Communication::init(ROBOT_ID,UDP_PORT,TCP_PORT,TCP_HEARTBEAT_PORT); + + bool auto_start = false; + this->nh_.param("auto_start", auto_start, false); + //自动启动话题 + if (auto_start == true) + init(); + + // thread_recCommunicationBridgeiver + boost::thread recv_thd(&CommunicationBridge::serverFun, this); + recv_thd.detach(); //后台 + ros::Duration(1).sleep(); // wait + + // thread_receiver + boost::thread recv_multicast_thd(&CommunicationBridge::multicastUdpFun, this); + recv_multicast_thd.detach(); + ros::Duration(1).sleep(); // wait + + boost::thread to_ground_station_thd(&CommunicationBridge::toGroundStationFun, this); + to_ground_station_thd.detach(); + ros::Duration(1).sleep(); // wait + + // system(OPENUAVBASIC); +} + +CommunicationBridge::~CommunicationBridge() +{ + if (this->uav_basic_ != NULL) + delete this->uav_basic_; + if (this->ugv_basic_ != NULL) + delete this->ugv_basic_; + if (this->autonomous_landing_ != NULL) + delete this->autonomous_landing_; + if (this->object_tracking_ != NULL) + delete this->object_tracking_; + if (this->swarm_control_ != NULL) + delete this->swarm_control_; +} + +void CommunicationBridge::init() +{ + // if (this->user_type_ == 1) + // { + // this->uav_basic_ = new UAVBasic(this->nh_, ROBOT_ID); + // if (this->is_simulation_ == 1) + // { + // if (this->swarm_num_ != 0) + // { + // for (int i = 1; i <= this->swarm_num_; i++) + // { + // if (i == ROBOT_ID) + // { + // this->swarm_control_simulation_[i] = this->uav_basic_; + // continue; + // } + // this->swarm_control_simulation_[i] = new UAVBasic(this->nh_, i); + // } + // this->swarm_control_ = new SwarmControl(this->nh_,this->swarm_num_); + // } + // }else + // { + // if(this->swarm_num_ != 0) + // this->swarm_control_ = new SwarmControl(this->nh_,ROBOT_ID,this->swarm_num_); + // } + // this->gimbal_basic_ = new GimbalBasic(this->nh_); + // this->object_tracking_ = new ObjectTracking(this->nh_); + // this->autonomous_landing_ = new AutonomousLanding(this->nh_); + // } + //根据载体进行初始化 + if (this->user_type_ == UserType::UAV) + { + this->uav_basic_ = new UAVBasic(this->nh_, ROBOT_ID,(Communication*)this); + } + else if (this->user_type_ == UserType::UGV) + { + this->ugv_basic_ = new UGVBasic(this->nh_,(Communication*)this); + } +} + +//TCP服务端 +void CommunicationBridge::serverFun() +{ + int valread; + if (waitConnectionFromGroundStation(TCP_PORT) < 0) + { + ROS_ERROR("[bridge_node]Socket recever creation error!"); + exit(EXIT_FAILURE); + } + + while (true) + { + //等待连接队列中抽取第一个连接，创建一个与s同类的新的套接口并返回句柄。 + if ((recv_sock = accept(server_fd, (struct sockaddr *)NULL, NULL)) < 0) + { + perror("accept"); + exit(EXIT_FAILURE); + } + + //recv函数从TCP连接的另一端接收数据 + valread = recv(recv_sock, tcp_recv_buf, BUF_LEN, 0); + usleep(200000); + + if (valread <= 0) + { + ROS_ERROR("Received message length <= 0, maybe connection has lost"); + close(recv_sock); + continue; + } + + // std::lock_guard lg(g_m); + + std::cout << "tcp valread: " << valread << std::endl; + //char *ptr = tcp_recv_buf; + //目前只有地面站发送TCP消息、所以TCP服务端接收到数据后开始心跳包的发送 + this->is_heartbeat_ready_ = true; + + pubMsg(decodeMsg(tcp_recv_buf)); + close(recv_sock); + } +} + +void CommunicationBridge::recvData(struct UAVState uav_state) +{ + + if (this->swarm_control_ != NULL) + { + //融合到所有无人机状态然后发布话题 + this->swarm_control_->updateAllUAVState(uav_state); + //发布话题 + this->swarm_control_->allUAVStatePub(this->swarm_control_->getMultiUAVState()); + } +} +void CommunicationBridge::recvData(struct UAVCommand uav_cmd) +{ + if (this->uav_basic_ == NULL) + { + return; + } + this->uav_basic_->uavCmdPub(uav_cmd); +} +void CommunicationBridge::recvData(struct SwarmCommand swarm_command) +{ + if (this->swarm_control_ == NULL) + { + return; + } + if (swarm_command.swarm_num != this->swarm_num_) + { + struct TextInfo text_info; + text_info.MessageType = text_info.WARN; + text_info.Message = "ground station swarm num ！= communication module swarm num"; + text_info.sec = ros::Time::now().sec; + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); + } + //发布话题 + this->swarm_control_->swarmCommandPub(swarm_command); +} +void CommunicationBridge::recvData(struct ConnectState connect_state) +{ + if (this->is_simulation_ == 0) + return; + if (!connect_state.state || connect_state.num < this->swarm_num_) + //this->swarm_control_->closeUAVState(connect_state.num); + //触发降落信号 + this->swarm_control_->communicationStatePub(connect_state.state, connect_state.num); +} +void CommunicationBridge::recvData(struct GimbalControl gimbal_control) +{ + if (this->gimbal_basic_ == NULL) + { + return; + } + this->gimbal_basic_->gimbalControlPub(gimbal_control); +} +void CommunicationBridge::recvData(struct GimbalService gimbal_service) +{ + if (this->autonomous_landing_ == NULL) + { + return; + } + if (gimbal_service.service == gimbal_service.search) + this->autonomous_landing_->gimbalSearchServer(gimbal_service.data); + else if (gimbal_service.service == gimbal_service.record_video) + this->autonomous_landing_->gimbalRecordVideoServer(gimbal_service.data); + else if (gimbal_service.service == gimbal_service.track_mode) + this->autonomous_landing_->gimbalTrackModeServer(gimbal_service.data); +} +void CommunicationBridge::recvData(struct GimbalParamSet param_set) +{ + if (this->autonomous_landing_ == NULL) + { + return; + } + this->autonomous_landing_->gimbalParamSetServer(param_set); +} +void CommunicationBridge::recvData(struct WindowPosition window_position) +{ + if (this->gimbal_basic_ == NULL) + { + return; + } + //如果udp_msg数据不为空 则向udo端口发送数据。否则发布ros话题 + if (!window_position.udp_msg.empty()) + { + std::cout << "udp_msg size :" << window_position.udp_msg.size() << std::endl; + sendMsgByUdp(window_position.udp_msg.size(), window_position.udp_msg.c_str(), "127.0.0.1", SERV_PORT); + } + else + { + this->gimbal_basic_->gimbalWindowPositionPub(window_position); + } +} +void CommunicationBridge::recvData(struct RheaControl rhea_control) +{ + if (this->ugv_basic_ == NULL) + { + return; + } + this->ugv_basic_->rheaControlPub(rhea_control); +} +void CommunicationBridge::recvData(struct RheaState rhea_state) +{ + if (this->autonomous_landing_ == NULL) + { + return; + } + this->autonomous_landing_->rheaStatePub(rhea_state); +} +void CommunicationBridge::recvData(struct ImageData image_data) +{ + createImage(image_data); +} +void CommunicationBridge::recvData(struct UAVSetup uav_setup) +{ + if (this->uav_basic_ == NULL) + { + return; + } + this->uav_basic_->uavSetupPub(uav_setup); +} +void CommunicationBridge::recvData(struct ModeSelection mode_selection) +{ + modeSwitch(mode_selection); +} + +//根据协议中MSG_ID的值，将数据段数据转化为正确的结构体 +void CommunicationBridge::pubMsg(int msg_id) +{ + switch (msg_id) + { + case MsgId::UAVSTATE: + recvData(recv_uav_state_); + break; + case MsgId::SWARMCOMMAND: + recvData(recv_swarm_command_); + break; + case MsgId::CONNECTSTATE: + //集群仿真下有效 + recvData(recv_connect_state_); + break; + case MsgId::GIMBALCONTROL: + recvData(recv_gimbal_control_); + break; + case MsgId::GIMBALSERVICE: + recvData(recv_gimbal_service_); + break; + case MsgId::GIMBALPARAMSET: + recvData(recv_param_set_); + break; + case MsgId::WINDOWPOSITION: + recvData(recv_window_position_); + break; + case MsgId::RHEACONTROL: + recvData(recv_rhea_control_); + break; + case MsgId::RHEASTATE: + recvData(recv_rhea_state_); + break; + case MsgId::IMAGEDATA: + recvData(recv_image_data_); + break; + case MsgId::UAVCOMMAND: + recvData(recv_uav_cmd_); + break; + case MsgId::UAVSETUP: + recvData(recv_uav_setup_); + break; + case MsgId::MODESELECTION: + recvData(recv_mode_selection_); + break; + default: + break; + } +} + +void CommunicationBridge::createImage(struct ImageData image_data) +{ + std::ofstream os(image_data.name); + os << image_data.data; + os.close(); + std::cout << "image_data" << std::endl; +} + +void CommunicationBridge::modeSwitch(struct ModeSelection mode_selection) +{ + //test + // if ((int)mode_selection.mode == 6) + // { + // system(DEMO1); + // return; + // } + if (mode_selection.mode == ModeSelection::Mode::REBOOTNX) + { + system(REBOOTNXCMD); + } + else if (mode_selection.mode == ModeSelection::Mode::EXITNX) + { + system(EXITNXCMD); + } + + struct TextInfo text_info; + + text_info.sec = ros::Time::now().sec; + if (mode_selection.use_mode == ModeSelection::UseMode::CREATE) + { + if (createMode(mode_selection)) + { + text_info.MessageType = TextInfo::MessageTypeGrade::INFO; + text_info.Message = "open mode success!"; + } + else + { + text_info.MessageType = TextInfo::MessageTypeGrade::WARN; + text_info.Message = "open mode fail!"; + } + } + else if (mode_selection.use_mode == ModeSelection::UseMode::DELETE) + { + if (deleteMode(mode_selection)) + { + text_info.MessageType = TextInfo::MessageTypeGrade::INFO; + text_info.Message = "close mode success!"; + } + else + { + text_info.MessageType = TextInfo::MessageTypeGrade::WARN; + text_info.Message = "close mode fail!"; + } + } + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); +} + +bool CommunicationBridge::createMode(struct ModeSelection mode_selection) +{ + struct TextInfo text_info; + text_info.MessageType = TextInfo::MessageTypeGrade::INFO; + text_info.sec = ros::Time::now().sec; + bool is = true; + if (mode_selection.mode == ModeSelection::Mode::UAVBASIC) + { + //仿真模式 允许同一通信节点创建多个飞机的话题 + if (this->is_simulation_ == 1) + { + for (int i = 0; i < mode_selection.selectId.size(); i++) + { + //判断是否已经存在 + if (!this->swarm_control_simulation_.empty()) + { + if (this->swarm_control_simulation_.find(mode_selection.selectId[i]) != this->swarm_control_simulation_.end()) + { + text_info.Message = "UAVBasic simulation id :" + to_string(mode_selection.selectId[i]) + " already exists"; + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); + return false; + } + } + + //创建并存入 + this->swarm_control_simulation_[mode_selection.selectId[i]] = new UAVBasic(this->nh_, mode_selection.selectId[i],(Communication*)this); + text_info.Message = "create UAVBasic simulation id :" + to_string(mode_selection.selectId[i]) + "..."; + //如果id与通信节点相同则存入uav_basic_ + if (ROBOT_ID == mode_selection.selectId[i]) + { + if (this->uav_basic_ != NULL) + { + return false; + } + + this->uav_basic_ = this->swarm_control_simulation_[mode_selection.selectId[i]]; + + //打开 + // system(OPENUAVBASIC); + } + text_info.Message = "create UAVBasic simulation id :" + to_string(mode_selection.selectId[0]) + "..."; + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); + } + } + //真机模式 同一通信节点只能创建一个飞机的话题 + else + { + for (int i = 0; i < mode_selection.selectId.size(); i++) + { + //如果id与通信节点相同则存入uav_basic_ + if (mode_selection.selectId[i] == ROBOT_ID) + { + if (this->uav_basic_ == NULL) + { + this->uav_basic_ = new UAVBasic(this->nh_, ROBOT_ID,(Communication*)this); + text_info.Message = "create UAVBasic :" + to_string(ROBOT_ID) + "..."; + + //启动 uav_control节点 + //先关闭防止重复打开 + // system(CLOSEUAVBASIC); + //打开 + // system(OPENUAVBASIC); + } + } + else + { + text_info.MessageType = TextInfo::MessageTypeGrade::WARN; + text_info.Message = "id inconformity"; + is = false; + } + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); + } + } + } + else if (mode_selection.mode == ModeSelection::Mode::UGVBASIC) + { + if (this->ugv_basic_ == NULL) + { + this->ugv_basic_ = new UGVBasic(this->nh_,(Communication*)this); + text_info.Message = "UGVBasic"; + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); + system(CLOSEUGVBASIC); + system(OPENUGVBASIC); + } + } + //集群模式 + else if (mode_selection.mode == ModeSelection::Mode::SWARMCONTROL) + { + if (this->swarm_num_ != mode_selection.selectId.size()) + { + text_info.MessageType = TextInfo::MessageTypeGrade::WARN; + text_info.Message = "mode switch fail，because swarm num inconsistent."; + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); + return false; + } + if (this->is_simulation_ != mode_selection.is_simulation) + { + text_info.MessageType = TextInfo::MessageTypeGrade::WARN; + text_info.Message = "mode switch fail，because mode inconsistent."; + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); + return false; + } + //仿真模式 + if (this->is_simulation_ == 1) + { + for (int i = 0; i < mode_selection.selectId.size(); i++) + { + if (this->swarm_control_simulation_.count(mode_selection.selectId[i]) == 0) + { + text_info.MessageType = TextInfo::MessageTypeGrade::WARN; + text_info.Message = "mode switch fail，id " + to_string(mode_selection.selectId[i]) + " non-existent"; + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); + return false; + } + } + if (this->swarm_control_ == NULL) + { + this->swarm_control_ = new SwarmControl(this->nh_, this->swarm_num_,(Communication*)this); + //this->swarm_control_ = std::make_shared(this->nh_, this->swarm_num); + text_info.Message = "simulation SwarmControl: swarm_num:" + std::to_string(this->swarm_num_); + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); + system(OPENSWARMCONTROL); + } + } + else //真机 + { + for (int i = 0; i < mode_selection.selectId.size(); i++) + { + if (mode_selection.selectId[i] == ROBOT_ID) + { + this->swarm_control_ = new SwarmControl(this->nh_, ROBOT_ID, this->swarm_num_,(Communication*)this); + text_info.Message = "SwarmControl: swarm_num:" + std::to_string(this->swarm_num_); + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); + break; + } + if (i == mode_selection.selectId.size() - 1) + { + text_info.MessageType = TextInfo::MessageTypeGrade::WARN; + text_info.Message = "mode switch fail，id " + to_string(ROBOT_ID) + " non-existent"; + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); + return false; + } + } + } + + //启动子模块指令 + //system() + } + + else if (mode_selection.mode == ModeSelection::Mode::AUTONOMOUSLANDING) + { + if (this->ugv_basic_ != NULL) + { + text_info.MessageType = TextInfo::MessageTypeGrade::WARN; + text_info.Message = "mode switch fail，because user type ugv."; + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); + return false; + } + if (this->uav_basic_ != NULL) + { + if (this->gimbal_basic_ == NULL) + { + this->gimbal_basic_ = new GimbalBasic(this->nh_,(Communication*)this); + } + //自主降落 + if (this->autonomous_landing_ == NULL) + { + this->autonomous_landing_ = new AutonomousLanding(this->nh_,(Communication*)this); + } + text_info.Message = "AutonomousLanding"; + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); + system(OPENAUTONOMOUSLANDING); + } + } + //目标识别与跟踪模式 + else if (mode_selection.mode == ModeSelection::Mode::OBJECTTRACKING) + { + if (this->uav_basic_ != NULL) + { + if (this->gimbal_basic_ == NULL) + { + this->gimbal_basic_ = new GimbalBasic(this->nh_,(Communication*)this); + } + if (this->object_tracking_ == NULL) + { + this->object_tracking_ = new ObjectTracking(this->nh_,(Communication*)this); + } + text_info.Message = "ObjectTracking"; + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); + system(OPENOBJECTTRACKING); + } + } + else if (mode_selection.mode == ModeSelection::Mode::CUSTOMMODE) + { + system(mode_selection.cmd.c_str()); + } + this->current_mode_ = mode_selection.mode; + return is; +} + +bool CommunicationBridge::deleteMode(struct ModeSelection mode_selection) +{ + struct TextInfo text_info; + text_info.MessageType = TextInfo::MessageTypeGrade::INFO; + text_info.sec = ros::Time::now().sec; + + if (mode_selection.mode == ModeSelection::Mode::UAVBASIC) + { + if (this->is_simulation_ == 1) + { + for (int i = 0; i < mode_selection.selectId.size(); i++) + { + std::lock_guard lg_uav_basic(g_uav_basic); + if (this->swarm_control_simulation_.find(mode_selection.selectId[i]) != this->swarm_control_simulation_.end()) + { + delete this->swarm_control_simulation_[mode_selection.selectId[i]]; + this->swarm_control_simulation_.erase(this->swarm_control_simulation_.find(mode_selection.selectId[i])); + + if (ROBOT_ID == mode_selection.selectId[i]) + { + // delete this->uav_basic_; + this->uav_basic_ = NULL; + // system(CLOSEUAVBASIC); + } + } + text_info.Message = "delete UAVBasic simulation id :" + to_string(mode_selection.selectId[i]) + "..."; + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); + } + } + else + { + for (int i = 0; i < mode_selection.selectId.size(); i++) + { + if (ROBOT_ID == mode_selection.selectId[i]) + { + if (this->uav_basic_ != NULL) + { + delete this->uav_basic_; + this->uav_basic_ = NULL; + system(CLOSEUAVBASIC); + text_info.Message = "delete UAVBasic id :" + to_string(mode_selection.selectId[i]) + "..."; + } + } + else + { + text_info.MessageType = TextInfo::MessageTypeGrade::WARN; + text_info.Message = "id inconformity"; + return false; + } + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); + } + } + } + else if (mode_selection.mode == ModeSelection::Mode::UGVBASIC) + { + if (this->ugv_basic_ != NULL) + { + delete this->ugv_basic_; + this->ugv_basic_ = NULL; + system(CLOSEUGVBASIC); + } + } + else if (mode_selection.mode == ModeSelection::Mode::SWARMCONTROL) + { + // std::lock_guard lg(g_m); + if (this->swarm_control_ != NULL) + { + //开启互斥锁 + //boost::unique_lock lockImageStatus(g_m); + std::lock_guard lg(g_m); + delete this->swarm_control_; + this->swarm_control_ = NULL; + system(CLOSEOTHERMODE); + } + } + else if (mode_selection.mode == ModeSelection::Mode::AUTONOMOUSLANDING) + { + if (this->autonomous_landing_ != NULL) + { + delete this->autonomous_landing_; + this->autonomous_landing_ = NULL; + system(CLOSEOTHERMODE); + } + } + else if (mode_selection.mode == ModeSelection::Mode::OBJECTTRACKING) + { + if (this->object_tracking_ != NULL) + { + delete this->object_tracking_; + this->object_tracking_ = NULL; + system(CLOSEOTHERMODE); + } + } + return true; +} + +//接收组播地址的数据 +void CommunicationBridge::multicastUdpFun() +{ + if (this->swarm_num_ == 0) + { + return; + } + while (this->is_simulation_ == 1) + { + std::lock_guard lg_uav_basic(g_uav_basic); + for (auto it = this->swarm_control_simulation_.begin(); it != this->swarm_control_simulation_.end(); it++) + { + //开启互斥锁 + //boost::shared_lock lock(g_m); + std::lock_guard lg(g_m); + if (this->swarm_control_ != NULL) + { + this->swarm_control_->updateAllUAVState(it->second->getUAVState()); + this->swarm_control_->allUAVStatePub(this->swarm_control_->getMultiUAVState()); + } + } + } + int valread; + if (waitConnectionFromMulticast(UDP_PORT) < 0) + { + ROS_ERROR("[bridge_node]Socket recever creation error!"); + exit(EXIT_FAILURE); + } + + struct ip_mreq mreq; + mreq.imr_multiaddr.s_addr = inet_addr(multicast_udp_ip.c_str()); + + setsockopt(udp_socket, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)); + sockaddr_in srv_Addr; //用于存储发送方信息 + socklen_t addr_len = sizeof(srv_Addr); + + while (true) + { + std::lock_guard lg(g_m); + if (this->swarm_control_ == NULL) + { + continue; + } + + valread = recvfrom(udp_socket, udp_recv_buf, BUF_LEN, 0, (struct sockaddr *)&srv_Addr, &addr_len); + + if (valread <= 0) + { + ROS_ERROR("Received message length <= 0, maybe connection has lost"); + continue; + } + + //std::lock_guard lg(g_m); + + std::cout << "udp valread: " << valread << std::endl; + pubMsg(decodeMsg(udp_recv_buf)); + } +} + +//给地面站发送心跳包, 超时检测 +void CommunicationBridge::toGroundStationFun() +{ + struct Heartbeat heartbeat; + heartbeat.message = "OK"; + heartbeat.count = 0; + + //记录 集群数据刷新的时间戳 + uint swarm_control_time[this->swarm_num_] = {0}; + //记录 未刷新的次数 + uint swarm_control_timeout_count[this->swarm_num_] = {0}; + //记录 无人机或无人车的时间戳 + uint time = 0; + uint time_count = 0; + while (true) + { + if (!this->is_heartbeat_ready_) + { + continue; + } + + sendMsgByTcp(encodeMsg(Send_Mode::TCP, heartbeat), udp_ip); + heartbeat.count++; + if (disconnect_num > try_connect_num) //跟地面站断联后的措施 + { + std::cout << "conenect ground station failed！" << std::endl; + //如果是集群模式 由集群模块触发降落 + if (this->swarm_num_ != 0 && this->swarm_control_ != NULL) + { + if (this->is_simulation_ == 0) + this->swarm_control_->communicationStatePub(false); + else + { + for (int i = 0; i < this->swarm_num_; i++) + { + this->swarm_control_->communicationStatePub(false, i); + } + } + } + //无人机 触发降落或者返航 + else if (this->uav_basic_ != NULL) + { + //触发降落 暂定 + struct UAVSetup uav_setup; + uav_setup.cmd = UAVSetup::CMD::SET_PX4_MODE; + uav_setup.px4_mode = "AUTO.LAND"; + //uav_setup.px4_mode = "AUTO.RTL"; //返航 + uav_setup.arming = false; + uav_setup.control_state = ""; + this->uav_basic_->uavSetupPub(uav_setup); + } + //无人车 停止小车 + else if (this->ugv_basic_ != NULL) + { + //停止小车 + struct RheaControl rhea_control; + rhea_control.Mode = RheaControl::Mode::Stop; + rhea_control.linear = 0; + rhea_control.angular = 0; + this->ugv_basic_->rheaControlPub(rhea_control); + } + //触发机制后 心跳准备标志置为false，停止心跳包的发送 再次接收到地面站指令激活 + this->is_heartbeat_ready_ = false; + } + + //无人机数据或者无人车数据是否超时 + if (this->uav_basic_ != NULL || this->ugv_basic_ != NULL) + { + uint time_stamp = 0; + if (this->uav_basic_ != NULL) + { + time_stamp = this->uav_basic_->getTimeStamp(); + } + else if (this->ugv_basic_ != NULL) + { + time_stamp = this->ugv_basic_->getTimeStamp(); + } + //拿单机状态时间戳进行比较 如果不相等说明数据在更新 + if (time != time_stamp) + { + time = time_stamp; + time_count = 0; + } + else //相等 数据未更新 + { + time_count++; + if (time_count > this->swarm_data_update_timeout_) + { + //反馈地面站 + struct TextInfo text_info; + text_info.MessageType = TextInfo::MessageTypeGrade::ERROR; + if (this->uav_basic_ != NULL) + text_info.Message = "UAV" + to_string(ROBOT_ID) + " data update timeout"; + else + text_info.Message = "UGV" + to_string(ROBOT_ID) + " data update timeout"; + text_info.sec = ros::Time::now().sec; + sendMsgByUdp(encodeMsg(Send_Mode::UDP, text_info), multicast_udp_ip); + usleep(10); + } + } + } + + sleep(1); + } +} diff --git a/src/Prometheus/Modules/communication/src/gimbal_basic_topic.cpp b/src/Prometheus/Modules/communication/src/gimbal_basic_topic.cpp new file mode 100644 index 00000000..a3da8bde --- /dev/null +++ b/src/Prometheus/Modules/communication/src/gimbal_basic_topic.cpp @@ -0,0 +1,93 @@ +#include "gimbal_basic_topic.hpp" + +GimbalBasic::GimbalBasic(ros::NodeHandle &nh,Communication *communication) +{ + nh.param("multicast_udp_ip", multicast_udp_ip, "224.0.0.88"); + this->communication_ = communication; + //【订阅】吊舱状态数据 + this->gimbal_state_sub_ = nh.subscribe("/gimbal/state", 10, &GimbalBasic::stateCb, this); + //【订阅】跟踪误差 + this->vision_diff_sub_ = nh.subscribe("/gimbal/track", 10, &GimbalBasic::trackCb, this); + //【发布】框选 点击 目标 + this->window_position_pub_ = nh.advertise("/detection/bbox_draw", 1000); + //【发布】吊舱控制 + this->gimbal_control_pub_ = nh.advertise("/gimbal/control", 1000); +} + +GimbalBasic::~GimbalBasic() +{ + // delete this->communication_; +} + +void GimbalBasic::stateCb(const prometheus_msgs::GimbalState::ConstPtr &msg) +{ + this->gimbal_state_.Id = msg->Id; + this->gimbal_state_.feedbackMode = msg->feedbackMode; + this->gimbal_state_.mode = msg->mode; + this->gimbal_state_.isRecording = msg->isRecording; + this->gimbal_state_.zoomState = msg->zoomState; + this->gimbal_state_.zoomVal = msg->zoomVal; + for(int i = 0; i < 3; i++) + { + this->gimbal_state_.imuAngle[i] = msg->imuAngle[i]; + this->gimbal_state_.rotorAngle[i] = msg->rotorAngle[i]; + this->gimbal_state_.imuAngleVel[i] = msg->imuAngleVel[i]; + this->gimbal_state_.rotorAngleTarget[i] = msg->rotorAngleTarget[i]; + } + //发送到组播地址 + this->communication_->sendMsgByUdp(this->communication_->encodeMsg(Send_Mode::UDP,this->gimbal_state_),multicast_udp_ip); +} + +void GimbalBasic::trackCb(const prometheus_msgs::VisionDiff::ConstPtr &msg) +{ + this->vision_diff_.id = msg->Id; + this->vision_diff_.detect = msg->detect; + this->vision_diff_.objectX = msg->objectX; + this->vision_diff_.objectY = msg->objectY; + this->vision_diff_.objectWidth = msg->objectWidth; + this->vision_diff_.objectHeight = msg->objectHeight; + this->vision_diff_.frameWidth = msg->frameWidth; + this->vision_diff_.frameHeight = msg->frameHeight; + this->vision_diff_.kp = msg->kp; + this->vision_diff_.ki = msg->ki; + this->vision_diff_.kd = msg->kd; + this->vision_diff_.ctlMode = msg->ctlMode; + this->vision_diff_.currSize = msg->currSize; + this->vision_diff_.maxSize = msg->maxSize; + this->vision_diff_.minSize = msg->minSize; + this->vision_diff_.trackIgnoreError = msg->trackIgnoreError; + this->vision_diff_.autoZoom = msg->autoZoom; + //发送到组播地址 + this->communication_->sendMsgByUdp(this->communication_->encodeMsg(Send_Mode::UDP,this->vision_diff_),multicast_udp_ip); +} + +void GimbalBasic::gimbalWindowPositionPub(struct WindowPosition window_position) +{ + prometheus_msgs::WindowPosition window_position_; + window_position_.mode = window_position.mode; + window_position_.track_id = window_position.track_id; + window_position_.origin_x = window_position.origin_x; + window_position_.origin_y = window_position.origin_y; + window_position_.height = window_position.height; + window_position_.width = window_position.width; + window_position_.window_position_x = window_position.window_position_x; + window_position_.window_position_y = window_position.window_position_y; + this->window_position_pub_.publish(window_position_); +} + +void GimbalBasic::gimbalControlPub(struct GimbalControl gimbal_control) +{ + prometheus_msgs::GimbalControl gimbal_control_; + gimbal_control_.Id = gimbal_control.Id; + gimbal_control_.rpyMode = gimbal_control.rpyMode; + gimbal_control_.roll = gimbal_control.roll; + gimbal_control_.yaw = gimbal_control.yaw; + gimbal_control_.pitch = gimbal_control.pitch; + gimbal_control_.rValue = gimbal_control.rValue; + gimbal_control_.yValue = gimbal_control.yValue; + gimbal_control_.pValue = gimbal_control.pValue; + gimbal_control_.focusMode = gimbal_control.focusMode; + gimbal_control_.zoomMode = gimbal_control.zoomMode; + //发布话题 + this->gimbal_control_pub_.publish(gimbal_control_); +} diff --git a/src/Prometheus/Modules/communication/src/main.cpp b/src/Prometheus/Modules/communication/src/main.cpp new file mode 100644 index 00000000..6126f805 --- /dev/null +++ b/src/Prometheus/Modules/communication/src/main.cpp @@ -0,0 +1,15 @@ +#include "communication_bridge.hpp" + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "ground_station_bridge"); + ros::NodeHandle nh("~"); + + printf("\033[1;32m---->[ground_station_bridge] start running\n\033[0m"); + + CommunicationBridge communication_bridge_(nh); + + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/src/Prometheus/Modules/communication/src/object_tracking_topic.cpp b/src/Prometheus/Modules/communication/src/object_tracking_topic.cpp new file mode 100644 index 00000000..8f434bf9 --- /dev/null +++ b/src/Prometheus/Modules/communication/src/object_tracking_topic.cpp @@ -0,0 +1,30 @@ +#include "object_tracking_topic.hpp" + +ObjectTracking::ObjectTracking(ros::NodeHandle &nh,Communication *communication) +{ + nh.param("multicast_udp_ip", this->multicast_udp_ip, "224.0.0.88"); + this->communication_ = communication; + this->multi_detection_info_sub_ = nh.subscribe("/deepsort_ros/object_detection_result", 10, &ObjectTracking::multiDetectionInfoCb, this); +} +ObjectTracking::~ObjectTracking() +{ + delete this->communication_; +} + +void ObjectTracking::multiDetectionInfoCb(const prometheus_msgs::MultiDetectionInfoSub::ConstPtr &msg) +{ + struct MultiDetectionInfo multi_detection_info; + multi_detection_info.mode = msg->mode; + multi_detection_info.num_objs = msg->num_objs; + for(int i = 0; i < msg->num_objs; i++) + { + struct DetectionInfo detection_info; + detection_info.left = msg->detection_infos[i].left; + detection_info.top = msg->detection_infos[i].top; + detection_info.bot = msg->detection_infos[i].bot; + detection_info.right = msg->detection_infos[i].right; + detection_info.trackIds = msg->detection_infos[i].trackIds; + multi_detection_info.detection_infos.push_back(detection_info); + } + this->communication_->sendMsgByUdp(this->communication_->encodeMsg(Send_Mode::UDP,multi_detection_info),this->multicast_udp_ip); +} \ No newline at end of file diff --git a/src/Prometheus/Modules/communication/src/swarm_control_topic.cpp b/src/Prometheus/Modules/communication/src/swarm_control_topic.cpp new file mode 100644 index 00000000..966f042a --- /dev/null +++ b/src/Prometheus/Modules/communication/src/swarm_control_topic.cpp @@ -0,0 +1,214 @@ +#include "swarm_control_topic.hpp" + +//真机构造 +SwarmControl::SwarmControl(ros::NodeHandle &nh, int id, int swarm_num,Communication *communication) // : UAVBasic(nh, Mode::SWARMCONTROL) +{ + std::cout << "集群 真机模式" << std::endl; + this->communication_ = communication; + is_simulation_ = false; + init(nh, swarm_num, id); + + //【发布】集群控制指令 + this->swarm_command_pub_ = nh.advertise("/prometheus/swarm_command", 1000); + //【发布】连接是否失效 + this->communication_state_pub_ = nh.advertise("/uav" + std::to_string(id) + "/prometheus/communication_state", 10); + //【发布】所有无人机状态 + this->all_uav_state_pub_ = nh.advertise("/prometheus/all_uav_state", 1000); +} + +//仿真构造 +SwarmControl::SwarmControl(ros::NodeHandle &nh, int swarm_num,Communication *communication) +{ + std::cout << "集群 仿真模式" << std::endl; + this->communication_ = communication; + is_simulation_ = true; + init(nh, swarm_num); + + for (int i = 1; i <= swarm_num; i++) + { + //连接是否失效 + ros::Publisher simulation_communication_state = nh.advertise("/uav" + std::to_string(i) + "/prometheus/communication_state", 10); + simulation_communication_state_pub.push_back(simulation_communication_state); + } + + //【发布】所有无人机状态 + this->all_uav_state_pub_ = nh.advertise("/prometheus/all_uav_state", 1000); + //【发布】集群控制指令 + this->swarm_command_pub_ = nh.advertise("/prometheus/swarm_command", 1000); +} + +SwarmControl::~SwarmControl() +{ + // delete this->communication_; + // this->communication_ = nullptr; +} + +void SwarmControl::init(ros::NodeHandle &nh, int swarm_num, int id) +{ + nh.param("ground_stationt_ip", udp_ip, "127.0.0.1"); + nh.param("multicast_udp_ip", multicast_udp_ip, "224.0.0.88"); + for (int i = 1; i <= swarm_num; i++) + { + struct UAVState uav_state; + uav_state.uav_id = i; + // uav_state.state = UAVState::State::unknown; + uav_state.location_source = UAVState::LocationSource::MOCAP; + uav_state.gps_status = 0; + uav_state.mode = ""; + uav_state.connected = false; + uav_state.armed = false; + uav_state.odom_valid = false; + for (int j = 0; j < 3; j++) + { + uav_state.position[j] = 0; + uav_state.velocity[j] = 0; + uav_state.attitude[j] = 0; + uav_state.attitude_rate[j] = 0; + } + uav_state.latitude = 0; + uav_state.longitude = 0; + uav_state.altitude = 0; + + uav_state.attitude_q.x = 0; + uav_state.attitude_q.y = 0; + uav_state.attitude_q.z = 0; + uav_state.attitude_q.w = 0; + uav_state.battery_state = 0; + uav_state.battery_percetage = 0; + this->multi_uav_state_.uav_state_all.push_back(uav_state); + } +} + + +//更新全部飞机数据 +void SwarmControl::updateAllUAVState(struct UAVState uav_state) +{ + //更新数据 + for(int i = 0; i < this->multi_uav_state_.uav_state_all.size(); i++) + { + if(this->multi_uav_state_.uav_state_all[i].uav_id == uav_state.uav_id) + { + this->multi_uav_state_.uav_state_all[i] = uav_state; + break; + } + } + +} + +//【发布】集群控制指令 +void SwarmControl::swarmCommandPub(struct SwarmCommand swarm_command) +{ + //进行发布 + prometheus_msgs::SwarmCommand m_swarm_command; + m_swarm_command.source = swarm_command.source; + m_swarm_command.Swarm_CMD = swarm_command.Swarm_CMD; + m_swarm_command.swarm_location_source = swarm_command.swarm_location_source; + m_swarm_command.swarm_num = swarm_command.swarm_num; + for (int i = 0; i < 2; i++) + { + m_swarm_command.leader_pos[i] = swarm_command.leader_pos[i]; + m_swarm_command.leader_vel[i] = swarm_command.leader_vel[i]; + } + m_swarm_command.leader_pos[2] = swarm_command.leader_pos[2]; + + m_swarm_command.swarm_size = swarm_command.swarm_size; + m_swarm_command.swarm_shape = swarm_command.swarm_shape; + m_swarm_command.target_area_x_min = swarm_command.target_area_x_min; + m_swarm_command.target_area_y_min = swarm_command.target_area_y_min; + m_swarm_command.target_area_x_max = swarm_command.target_area_x_max; + m_swarm_command.target_area_y_max = swarm_command.target_area_y_max; + for (int i = 0; i < 3; i++) + { + m_swarm_command.attack_target_pos[i] = swarm_command.attack_target_pos[i]; + }; + for(int i = 0; i < swarm_command.formation_poses.size() ; i++) + { + geometry_msgs::Point point; + point.x = swarm_command.formation_poses[i].x; + point.y = swarm_command.formation_poses[i].y; + point.z = swarm_command.formation_poses[i].z; + m_swarm_command.formation_poses.push_back(point); + } + this->swarm_command_pub_.publish(m_swarm_command); +} + +//【发布】连接是否失效 +void SwarmControl::communicationStatePub(bool communication) +{ + std_msgs::Bool communication_state; + communication_state.data = communication; + this->communication_state_pub_.publish(communication_state); +} + +void SwarmControl::communicationStatePub(bool communication, int id) +{ + std_msgs::Bool communication_state; + communication_state.data = communication; + //this->communication_state_pub_.publish(communication_state); + this->simulation_communication_state_pub[id].publish(communication_state); +} + +//【发布】所有无人机状态 +void SwarmControl::allUAVStatePub(struct MultiUAVState m_multi_uav_state) +{ + prometheus_msgs::MultiUAVState multi_uav_state; + multi_uav_state.uav_num = 0; + + for (auto it = m_multi_uav_state.uav_state_all.begin(); it != m_multi_uav_state.uav_state_all.end(); it++) + { + prometheus_msgs::UAVState uav_state; + uav_state.uav_id = (*it).uav_id; + // uav_state.state = (*it).state; + uav_state.mode = (*it).mode; + uav_state.connected = (*it).connected; + uav_state.armed = (*it).armed; + uav_state.odom_valid = (*it).odom_valid; + uav_state.location_source = (*it).location_source; + uav_state.gps_status = (*it).gps_status; + for (int i = 0; i < 3; i++) + { + uav_state.position[i] = (*it).position[i]; + uav_state.velocity[i] = (*it).velocity[i]; + uav_state.attitude[i] = (*it).attitude[i]; + uav_state.attitude_rate[i] = (*it).attitude_rate[i]; + }; + uav_state.latitude = (*it).latitude; + uav_state.longitude = (*it).longitude; + uav_state.altitude = (*it).altitude; + + uav_state.attitude_q.x = (*it).attitude_q.x; + uav_state.attitude_q.y = (*it).attitude_q.y; + uav_state.attitude_q.z = (*it).attitude_q.z; + uav_state.attitude_q.w = (*it).attitude_q.w; + uav_state.battery_state = (*it).battery_state; + uav_state.battery_percetage = (*it).battery_percetage; + + multi_uav_state.uav_num++; + multi_uav_state.uav_state_all.push_back(uav_state); + } + + //发布话题 + this->all_uav_state_pub_.publish(multi_uav_state); +} + +void SwarmControl::closeTopic() +{ + // if(is_simulation_) + // { + //auto it = simulation_communication_state_pub.begin(); + //std::cout << "size():"<communication_state_pub_.shutdown(); + // } + + // this->all_uav_state_pub_.shutdown(); + // this->swarm_command_pub_.shutdown(); +} diff --git a/src/Prometheus/Modules/communication/src/uav_basic_topic.cpp b/src/Prometheus/Modules/communication/src/uav_basic_topic.cpp new file mode 100644 index 00000000..e5d533e5 --- /dev/null +++ b/src/Prometheus/Modules/communication/src/uav_basic_topic.cpp @@ -0,0 +1,147 @@ +#include "uav_basic_topic.hpp" + +UAVBasic::UAVBasic() +{ + +} + +UAVBasic::UAVBasic(ros::NodeHandle &nh,int id,Communication *communication) +{ + nh.param("multicast_udp_ip", multicast_udp_ip, "224.0.0.88"); + this->robot_id = id; + this->offset_pose_.x = 0; + this->offset_pose_.y = 0; + this->communication_ = communication; + + //【订阅】uav状态信息 + this->uav_state_sub_ = nh.subscribe("/uav" + std::to_string(this->robot_id) + "/prometheus/state", 10, &UAVBasic::stateCb, this); + //【订阅】uav反馈信息 + this->text_info_sub_ = nh.subscribe("/uav" + std::to_string(id) + "/prometheus/text_info", 10, &UAVBasic::textInfoCb, this); + //【订阅】uav控制状态信息 + this->uav_control_state_sub_ = nh.subscribe("/uav" + std::to_string(id) + "/prometheus/control_state", 10, &UAVBasic::controlStateCb, this); + //【订阅】偏移量 + this->offset_pose_sub_ = nh.subscribe("/uav" + std::to_string(id) + "/prometheus/offset_pose", 10, &UAVBasic::offsetPoseCb, this); + //【发布】底层控制指令(-> uav_control.cpp) + this->uav_cmd_pub_ = nh.advertise("/uav" + std::to_string(id) + "/prometheus/command", 1); + //【发布】mavros接口调用指令(-> uav_control.cpp) + this->uav_setup_pub_ = nh.advertise("/uav" + std::to_string(this->robot_id) + "/prometheus/setup", 1); +} + +UAVBasic::~UAVBasic() +{ + // delete this->communication_; +} + +void UAVBasic::stateCb(const prometheus_msgs::UAVState::ConstPtr &msg) +{ + this->uav_state_.uav_id = msg->uav_id; + // this->uav_state_.state = msg->state; + this->uav_state_.location_source = msg->location_source; + this->uav_state_.gps_status = msg->gps_status; + this->uav_state_.mode = msg->mode; + this->uav_state_.connected = msg->connected; + this->uav_state_.armed = msg->armed; + this->uav_state_.odom_valid = msg->odom_valid; + + for (int i = 0; i < 3; i++) + { + this->uav_state_.velocity[i] = msg->velocity[i]; + this->uav_state_.attitude[i] = msg->attitude[i]; + this->uav_state_.attitude_rate[i] = msg->attitude_rate[i]; + }; + this->uav_state_.position[0] = msg->position[0] + offset_pose_.x; + this->uav_state_.position[1] = msg->position[1] + offset_pose_.y; + this->uav_state_.position[2] = msg->position[2]; + + this->uav_state_.latitude = msg->latitude; + this->uav_state_.longitude = msg->longitude; + this->uav_state_.altitude = msg->altitude; + + this->uav_state_.attitude_q.x = msg->attitude_q.x; + this->uav_state_.attitude_q.y = msg->attitude_q.y; + this->uav_state_.attitude_q.z = msg->attitude_q.z; + this->uav_state_.attitude_q.w = msg->attitude_q.w; + this->uav_state_.battery_state = msg->battery_state; + this->uav_state_.battery_percetage = msg->battery_percetage; + + //发送到组播地址 + this->communication_->sendMsgByUdp(this->communication_->encodeMsg(Send_Mode::UDP, this->uav_state_), multicast_udp_ip); + setTimeStamp(msg->header.stamp.sec); +} + +//【回调】uav反馈信息 +void UAVBasic::textInfoCb(const prometheus_msgs::TextInfo::ConstPtr &msg) +{ + this->text_info_.sec = msg->header.stamp.sec; + this->text_info_.MessageType = msg->MessageType; + this->text_info_.Message = msg->Message; + + //发送到组播地址 + this->communication_->sendMsgByUdp(this->communication_->encodeMsg(Send_Mode::UDP, this->text_info_), multicast_udp_ip); +} + +//【订阅】偏移量 +void UAVBasic::offsetPoseCb(const prometheus_msgs::OffsetPose::ConstPtr &msg) +{ + this->offset_pose_.x = msg->x; + this->offset_pose_.y = msg->y; +} + +void UAVBasic::controlStateCb(const prometheus_msgs::UAVControlState::ConstPtr &msg) +{ + this->uav_control_state_.uav_id = msg->uav_id; + this->uav_control_state_.control_state = msg->control_state; + this->uav_control_state_.pos_controller = msg->pos_controller; + this->uav_control_state_.failsafe = msg->failsafe; + + //发送到组播地址 + this->communication_->sendMsgByUdp(this->communication_->encodeMsg(Send_Mode::UDP, this->uav_control_state_), multicast_udp_ip); +} + +struct UAVState UAVBasic::getUAVState() +{ + return this->uav_state_; +} + +void UAVBasic::uavCmdPub(struct UAVCommand uav_cmd) +{ + prometheus_msgs::UAVCommand uav_cmd_; + uav_cmd_.Agent_CMD = uav_cmd.Agent_CMD; + uav_cmd_.Move_mode = uav_cmd.Move_mode; + for(int i = 0; i < 3; i++) + { + uav_cmd_.position_ref[i] = uav_cmd.position_ref[i]; + uav_cmd_.velocity_ref[i] = uav_cmd.velocity_ref[i]; + uav_cmd_.acceleration_ref[i] = uav_cmd.acceleration_ref[i]; + uav_cmd_.att_ref[i] = uav_cmd.att_ref[i]; + } + uav_cmd_.att_ref[3] = uav_cmd.att_ref[3]; + uav_cmd_.yaw_ref = uav_cmd.yaw_ref; + uav_cmd_.Yaw_Rate_Mode = uav_cmd.Yaw_Rate_Mode; + uav_cmd_.yaw_rate_ref = uav_cmd.yaw_rate_ref; + uav_cmd_.Command_ID = uav_cmd.Command_ID; + uav_cmd_.latitude = uav_cmd.latitude; + uav_cmd_.longitude = uav_cmd.longitude; + uav_cmd_.altitude = uav_cmd.altitude; + this->uav_cmd_pub_.publish(uav_cmd_); +} + +void UAVBasic::uavSetupPub(struct UAVSetup uav_setup) +{ + prometheus_msgs::UAVSetup uav_setup_; + uav_setup_.cmd = uav_setup.cmd; + uav_setup_.arming = uav_setup.arming; + uav_setup_.control_state = uav_setup.control_state; + uav_setup_.px4_mode = uav_setup.px4_mode; + this->uav_setup_pub_.publish(uav_setup_); +} + +void UAVBasic::setTimeStamp(uint time) +{ + this->time_stamp_ = time; +} + +uint UAVBasic::getTimeStamp() +{ + return this->time_stamp_; +} diff --git a/src/Prometheus/Modules/communication/src/ugv_basic_topic.cpp b/src/Prometheus/Modules/communication/src/ugv_basic_topic.cpp new file mode 100644 index 00000000..1dc22d23 --- /dev/null +++ b/src/Prometheus/Modules/communication/src/ugv_basic_topic.cpp @@ -0,0 +1,118 @@ +#include "ugv_basic_topic.hpp" + +UGVBasic::UGVBasic(ros::NodeHandle &nh,Communication *communication) +{ + this->communication_ = communication; + nh.param("ground_stationt_ip", udp_ip, "127.0.0.1"); + nh.param("multicast_udp_ip", multicast_udp_ip, "224.0.0.88"); + + //【订阅】rviz 点云 + // this->scan_matched_points2_sub_ = nh.subscribe("/prometheus/pcl_groundtruth", 10, &UGVBasic::scanMatchedPoints2Cb, this); + // //【订阅】rviz 激光雷达 + // this->scan_sub_ = nh.subscribe("/scan", 10, &UGVBasic::scanCb, this); + // //【订阅】rviz tf_static + // this->tf_static_sub_ = nh.subscribe("/tf_static", 10, &UGVBasic::tfCb, this); + // //【订阅】rviz tf + // this->tf_sub_ = nh.subscribe("/tf", 10, &UGVBasic::tfCb, this); + //【订阅】rviz constraint_list + //this->constraint_list_sub_ + //【订阅】rviz landmark_poses_list + //this->landmark_poses_list_sub_ + //【订阅】rviz trajectory_node_list + //this->trajectory_node_list_sub_ + this->rhea_control_pub_ = nh.advertise("/rhea_command/control", 1000); + this->rhea_state_sub_ = nh.subscribe("/rhea_command/state", 10, &UGVBasic::rheaStateCb, this); +} + +UGVBasic::~UGVBasic() +{ + // delete this->communication_; +} + +// void UGVBasic::scanCb(const sensor_msgs::LaserScan::ConstPtr &msg) +// { +// sensor_msgs::LaserScan laser_scan = *msg; +// this->communication_->sendRvizByUdp(this->communication_->encodeRvizMsg(laser_scan), udp_ip); +// } + +// void UGVBasic::scanMatchedPoints2Cb(const sensor_msgs::PointCloud2::ConstPtr &msg) +// { +// sensor_msgs::PointCloud2 scan_matched_points2 = *msg; +// this->communication_->sendRvizByUdp(this->communication_->encodeRvizMsg(scan_matched_points2), udp_ip); +// } + +// void UGVBasic::tfCb(const tf2_msgs::TFMessage::ConstPtr &msg) +// { +// tf2_msgs::TFMessage tf = *msg; +// this->communication_->sendRvizByUdp(this->communication_->encodeRvizMsg(tf), udp_ip); +// } + +// void UGVBasic::tfStaticCb(const tf2_msgs::TFMessage::ConstPtr &msg) +// { +// tf2_msgs::TFMessage tf_static = *msg; +// this->communication_->sendRvizByUdp(this->communication_->encodeRvizMsg(tf_static, MsgId::UGVTFSTATIC), udp_ip); +// } + +// void UGVBasic::constraintListCb(const visualization_msgs::MarkerArray::ConstPtr &msg) +// { +// visualization_msgs::MarkerArray constraint_list = *msg; +// this->communication_->sendRvizByUdp(this->communication_->encodeRvizMsg(constraint_list), udp_ip); +// } + +// void UGVBasic::landmarkPosesListCb(const visualization_msgs::MarkerArray::ConstPtr &msg) +// { +// visualization_msgs::MarkerArray landmark_poses_list = *msg; +// this->communication_->sendRvizByUdp(this->communication_->encodeRvizMsg(landmark_poses_list, MsgId::UGVMARKERARRAYLANDMARK), udp_ip); +// } + +// void UGVBasic::trajectoryNodeListCb(const visualization_msgs::MarkerArray::ConstPtr &msg) +// { +// visualization_msgs::MarkerArray trajectory_node_list = *msg; +// this->communication_->sendRvizByUdp(this->communication_->encodeRvizMsg(trajectory_node_list, MsgId::UGVMARKERARRAYTRAJECTORY), udp_ip); +// } + +void UGVBasic::rheaControlPub(struct RheaControl rhea_control) +{ + prometheus_msgs::RheaCommunication rhea_control_; + rhea_control_.Mode = rhea_control.Mode; + rhea_control_.linear = rhea_control.linear; + rhea_control_.angular = rhea_control.angular; + for(int i = 0; i < rhea_control.waypoint.size(); i++) + { + prometheus_msgs::RheaGPS rhea_gps; + rhea_gps.altitude = rhea_control.waypoint[i].altitude; + rhea_gps.latitude = rhea_control.waypoint[i].latitude; + rhea_gps.longitude = rhea_control.waypoint[i].longitude; + rhea_control_.waypoint.push_back(rhea_gps); + } + this->rhea_control_pub_.publish(rhea_control_); +} + +void UGVBasic::rheaStateCb(const prometheus_msgs::RheaState::ConstPtr &msg) +{ + struct RheaState rhea_state; + rhea_state.rhea_id = msg->rhea_id; + rhea_state.linear = msg->linear; + rhea_state.angular = msg->angular; + rhea_state.yaw = msg->yaw; + rhea_state.latitude = msg->latitude; + rhea_state.longitude = msg->longitude; + rhea_state.battery_voltage = msg->battery_voltage; + rhea_state.altitude = msg->altitude; + for(int i = 0; i < 3; i++) + { + rhea_state.position[i] = msg->position[i]; + } + this->communication_->sendMsgByUdp(this->communication_->encodeMsg(Send_Mode::UDP,rhea_state),multicast_udp_ip); + setTimeStamp(msg->header.stamp.sec); +} + +void UGVBasic::setTimeStamp(uint time) +{ + this->time_stamp_ = time; +} + +uint UGVBasic::getTimeStamp() +{ + return this->time_stamp_; +} \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/CMakeLists.txt b/src/Prometheus/Modules/ego_planner_swarm/CMakeLists.txt new file mode 120000 index 00000000..66dd650a --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/CMakeLists.txt @@ -0,0 +1 @@ +/opt/ros/melodic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/CMakeLists.txt b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/CMakeLists.txt new file mode 100755 index 00000000..e61c145f --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 2.8.3) +project(bspline_opt) + +set(CMAKE_BUILD_TYPE "Release") +ADD_COMPILE_OPTIONS(-std=c++11 ) +ADD_COMPILE_OPTIONS(-std=c++14 ) +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") + +find_package(Eigen3 REQUIRED) +find_package(PCL 1.7 REQUIRED) + +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + visualization_msgs + plan_env + cv_bridge + traj_utils + path_searching +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES bspline_opt + CATKIN_DEPENDS plan_env +# DEPENDS system_lib +) + +include_directories( + SYSTEM + include + ${catkin_INCLUDE_DIRS} + ${Eigen3_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} +) + +add_library( bspline_opt + src/uniform_bspline.cpp + src/bspline_optimizer.cpp + src/gradient_descent_optimizer.cpp + ) +target_link_libraries( bspline_opt + ${catkin_LIBRARIES} + ) diff --git a/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/bspline_optimizer.h b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/bspline_optimizer.h new file mode 100644 index 00000000..838d62ec --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/bspline_optimizer.h @@ -0,0 +1,213 @@ +#ifndef _BSPLINE_OPTIMIZER_H_ +#define _BSPLINE_OPTIMIZER_H_ + +#include +#include +#include +#include +#include +#include +#include "bspline_opt/lbfgs.hpp" +#include + +// Gradient and elasitc band optimization + +// Input: a signed distance field and a sequence of points +// Output: the optimized sequence of points +// The format of points: N x 3 matrix, each row is a point +namespace ego_planner +{ + + class ControlPoints + { + public: + double clearance; + int size; + Eigen::MatrixXd points; + std::vector> base_point; // The point at the statrt of the direction vector (collision point) + std::vector> direction; // Direction vector, must be normalized. + std::vector flag_temp; // A flag that used in many places. Initialize it everytime before using it. + // std::vector occupancy; + + void resize(const int size_set) + { + size = size_set; + + base_point.clear(); + direction.clear(); + flag_temp.clear(); + // occupancy.clear(); + + points.resize(3, size_set); + base_point.resize(size); + direction.resize(size); + flag_temp.resize(size); + // occupancy.resize(size); + } + + void segment(ControlPoints &buf, const int start, const int end) + { + + if (start < 0 || end >= size || points.rows() != 3) + { + ROS_ERROR("Wrong segment index! start=%d, end=%d", start, end); + return; + } + + buf.resize(end - start + 1); + buf.points = points.block(0, start, 3, end - start + 1); + buf.clearance = clearance; + buf.size = end - start + 1; + for (int i = start; i <= end; i++) + { + buf.base_point[i - start] = base_point[i]; + buf.direction[i - start] = direction[i]; + + // if ( buf.base_point[i - start].size() > 1 ) + // { + // ROS_ERROR("buf.base_point[i - start].size()=%d, base_point[i].size()=%d", buf.base_point[i - start].size(), base_point[i].size()); + // } + } + + // cout << "RichInfoOneSeg_temp, insede" << endl; + // for ( int k=0; k 0 ) + // { + // cout << "###" << buf.points.col(k).transpose() << endl; + // for (int k2 = 0; k2 < buf.base_point[k].size(); k2++) + // { + // cout << " " << buf.base_point[k][k2].transpose() << " @ " << buf.direction[k][k2].transpose() << endl; + // } + // } + } + }; + + class BsplineOptimizer + { + + public: + BsplineOptimizer() {} + ~BsplineOptimizer() {} + + /* main API */ + void setEnvironment(const GridMap::Ptr &map); + void setEnvironment(const GridMap::Ptr &map, const fast_planner::ObjPredictor::Ptr mov_obj); + void setParam(ros::NodeHandle &nh); + Eigen::MatrixXd BsplineOptimizeTraj(const Eigen::MatrixXd &points, const double &ts, + const int &cost_function, int max_num_id, int max_time_id); + + /* helper function */ + + // required inputs + void setControlPoints(const Eigen::MatrixXd &points); + void setBsplineInterval(const double &ts); + void setSwarmTrajs(SwarmTrajData *swarm_trajs_ptr); + void setDroneId(const int drone_id); + + // optional inputs + void setGuidePath(const vector &guide_pt); + void setWaypoints(const vector &waypts, + const vector &waypt_idx); // N-2 constraints at most + void setLocalTargetPt(const Eigen::Vector3d local_target_pt) { local_target_pt_ = local_target_pt; }; + + void optimize(); + + ControlPoints getControlPoints() { return cps_; }; + + AStar::Ptr a_star_; + std::vector ref_pts_; + + std::vector distinctiveTrajs(vector> segments); + std::vector> initControlPoints(Eigen::MatrixXd &init_points, bool flag_first_init = true); + bool BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double ts); // must be called after initControlPoints() + bool BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double &final_cost, const ControlPoints &control_points, double ts); + bool BsplineOptimizeTrajRefine(const Eigen::MatrixXd &init_points, const double ts, Eigen::MatrixXd &optimal_points); + + inline int getOrder(void) { return order_; } + inline double getSwarmClearance(void) { return swarm_clearance_; } + + private: + GridMap::Ptr grid_map_; + fast_planner::ObjPredictor::Ptr moving_objs_; + SwarmTrajData *swarm_trajs_{NULL}; // Can not use shared_ptr and no need to free + int drone_id_; + + enum FORCE_STOP_OPTIMIZE_TYPE + { + DONT_STOP, + STOP_FOR_REBOUND, + STOP_FOR_ERROR + } force_stop_type_; + + // main input + // Eigen::MatrixXd control_points_; // B-spline control points, N x dim + double bspline_interval_; // B-spline knot span + Eigen::Vector3d end_pt_; // end of the trajectory + // int dim_; // dimension of the B-spline + // + vector guide_pts_; // geometric guiding path points, N-6 + vector waypoints_; // waypts constraints + vector waypt_idx_; // waypts constraints index + // + int max_num_id_, max_time_id_; // stopping criteria + int cost_function_; // used to determine objective function + double start_time_; // global time for moving obstacles + + /* optimization parameters */ + int order_; // bspline degree + double lambda1_; // jerk smoothness weight + double lambda2_, new_lambda2_; // distance weight + double lambda3_; // feasibility weight + double lambda4_; // curve fitting + + int a; + // + double dist0_, swarm_clearance_; // safe distance + double max_vel_, max_acc_; // dynamic limits + + int variable_num_; // optimization variables + int iter_num_; // iteration of the solver + Eigen::VectorXd best_variable_; // + double min_cost_; // + + Eigen::Vector3d local_target_pt_; + +#define INIT_min_ellip_dist_ 123456789.0123456789 + double min_ellip_dist_; + + ControlPoints cps_; + + /* cost function */ + /* calculate each part of cost function with control points q as input */ + + static double costFunction(const std::vector &x, std::vector &grad, void *func_data); + void combineCost(const std::vector &x, vector &grad, double &cost); + + // q contains all control points + void calcSmoothnessCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient, bool falg_use_jerk = true); + void calcFeasibilityCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient); + void calcTerminalCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient); + void calcDistanceCostRebound(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient, int iter_num, double smoothness_cost); + void calcMovingObjCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient); + void calcSwarmCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient); + void calcFitnessCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient); + bool check_collision_and_rebound(void); + + static int earlyExit(void *func_data, const double *x, const double *g, const double fx, const double xnorm, const double gnorm, const double step, int n, int k, int ls); + static double costFunctionRebound(void *func_data, const double *x, double *grad, const int n); + static double costFunctionRefine(void *func_data, const double *x, double *grad, const int n); + + bool rebound_optimize(double &final_cost); + bool refine_optimize(); + void combineCostRebound(const double *x, double *grad, double &f_combine, const int n); + void combineCostRefine(const double *x, double *grad, double &f_combine, const int n); + + /* for benckmark evaluation only */ + public: + typedef unique_ptr Ptr; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + +} // namespace ego_planner +#endif \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/gradient_descent_optimizer.h b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/gradient_descent_optimizer.h new file mode 100755 index 00000000..01a5a716 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/gradient_descent_optimizer.h @@ -0,0 +1,52 @@ +#ifndef _GRADIENT_DESCENT_OPT_H_ +#define _GRADIENT_DESCENT_OPT_H_ + +#include +#include +#include + +using namespace std; + +class GradientDescentOptimizer +{ + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + typedef double (*objfunDef)(const Eigen::VectorXd &x, Eigen::VectorXd &grad, bool &force_return, void *data); + enum RESULT + { + FIND_MIN, + FAILED, + RETURN_BY_ORDER, + REACH_MAX_ITERATION + }; + + GradientDescentOptimizer(int v_num, objfunDef objf, void *f_data) + { + variable_num_ = v_num; + objfun_ = objf; + f_data_ = f_data; + }; + + void set_maxiter(int limit) { iter_limit_ = limit; } + void set_maxeval(int limit) { invoke_limit_ = limit; } + void set_xtol_rel(double xtol_rel) { xtol_rel_ = xtol_rel; } + void set_xtol_abs(double xtol_abs) { xtol_abs_ = xtol_abs; } + void set_min_grad(double min_grad) { min_grad_ = min_grad; } + + RESULT optimize(Eigen::VectorXd &x_init_optimal, double &opt_f); + +private: + int variable_num_{0}; + int iter_limit_{1e10}; + int invoke_limit_{1e10}; + double xtol_rel_; + double xtol_abs_; + double min_grad_; + double time_limit_; + void *f_data_; + objfunDef objfun_; +}; + +#endif diff --git a/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/lbfgs.hpp b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/lbfgs.hpp new file mode 100644 index 00000000..484c4b64 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/lbfgs.hpp @@ -0,0 +1,1451 @@ +#ifndef LBFGS_HPP +#define LBFGS_HPP + +#include +#include +#include +#include +#include + +namespace lbfgs +{ + // ----------------------- Data Type Part ----------------------- + + /** + * Return values of lbfgs_optimize(). + * + * Roughly speaking, a negative value indicates an error. + */ + + enum + { + /** L-BFGS reaches convergence. */ + LBFGS_CONVERGENCE = 0, + /** L-BFGS satisfies stopping criteria. */ + LBFGS_STOP, + /** The initial variables already minimize the objective function. */ + LBFGS_ALREADY_MINIMIZED, + + /** Unknown error. */ + LBFGSERR_UNKNOWNERROR = -1024, + /** Logic error. */ + LBFGSERR_LOGICERROR, + /** The minimization process has been canceled. */ + LBFGSERR_CANCELED, + /** Invalid number of variables specified. */ + LBFGSERR_INVALID_N, + /** Invalid parameter lbfgs_parameter_t::mem_size specified. */ + LBFGSERR_INVALID_MEMSIZE, + /** Invalid parameter lbfgs_parameter_t::g_epsilon specified. */ + LBFGSERR_INVALID_GEPSILON, + /** Invalid parameter lbfgs_parameter_t::past specified. */ + LBFGSERR_INVALID_TESTPERIOD, + /** Invalid parameter lbfgs_parameter_t::delta specified. */ + LBFGSERR_INVALID_DELTA, + /** Invalid parameter lbfgs_parameter_t::min_step specified. */ + LBFGSERR_INVALID_MINSTEP, + /** Invalid parameter lbfgs_parameter_t::max_step specified. */ + LBFGSERR_INVALID_MAXSTEP, + /** Invalid parameter lbfgs_parameter_t::f_dec_coeff specified. */ + LBFGSERR_INVALID_FDECCOEFF, + /** Invalid parameter lbfgs_parameter_t::wolfe specified. */ + LBFGSERR_INVALID_SCURVCOEFF, + /** Invalid parameter lbfgs_parameter_t::xtol specified. */ + LBFGSERR_INVALID_XTOL, + /** Invalid parameter lbfgs_parameter_t::max_linesearch specified. */ + LBFGSERR_INVALID_MAXLINESEARCH, + /** The line-search step went out of the interval of uncertainty. */ + LBFGSERR_OUTOFINTERVAL, + /** A logic error occurred; alternatively, the interval of uncertainty + became too small. */ + LBFGSERR_INCORRECT_TMINMAX, + /** A rounding error occurred; alternatively, no line-search step + satisfies the sufficient decrease and curvature conditions. */ + LBFGSERR_ROUNDING_ERROR, + /** The line-search step became smaller than lbfgs_parameter_t::min_step. */ + LBFGSERR_MINIMUMSTEP, + /** The line-search step became larger than lbfgs_parameter_t::max_step. */ + LBFGSERR_MAXIMUMSTEP, + /** The line-search routine reaches the maximum number of evaluations. */ + LBFGSERR_MAXIMUMLINESEARCH, + /** The algorithm routine reaches the maximum number of iterations. */ + LBFGSERR_MAXIMUMITERATION, + /** Relative width of the interval of uncertainty is at most + lbfgs_parameter_t::xtol. */ + LBFGSERR_WIDTHTOOSMALL, + /** A logic error (negative line-search step) occurred. */ + LBFGSERR_INVALIDPARAMETERS, + /** The current search direction increases the objective function value. */ + LBFGSERR_INCREASEGRADIENT, + }; + + /** + * L-BFGS optimization parameters. + * Call lbfgs_load_default_parameters() function to initialize parameters to the + * default values. + */ + struct lbfgs_parameter_t + { + /** + * The number of corrections to approximate the inverse hessian matrix. + * The L-BFGS routine stores the computation results of previous m + * iterations to approximate the inverse hessian matrix of the current + * iteration. This parameter controls the size of the limited memories + * (corrections). The default value is 6. Values less than 3 are + * not recommended. Large values will result in excessive computing time. + */ + int mem_size; + + /** + * Epsilon for grad norm convergence test. + * This parameter determines the accuracy with which the solution is to + * be found. A minimization terminates when + * ||g|| < g_epsilon * max(1, ||x||), + * where ||.|| denotes the Euclidean (L2) norm. The default value is 1e-5. + */ + double g_epsilon; + + /** + * Distance for delta-based convergence test. + * This parameter determines the distance, in iterations, to compute + * the rate of decrease of the objective function. If the value of this + * parameter is zero, the library does not perform the delta-based + * convergence test. The default value is 0. + */ + int past; + + /** + * Delta for convergence test. + * This parameter determines the minimum rate of decrease of the + * objective function. The library stops iterations when the + * following condition is met: + * (f' - f) / f < delta, + * where f' is the objective value of past iterations ago, and f is + * the objective value of the current iteration. + * The default value is 1e-5. + */ + double delta; + + /** + * The maximum number of iterations. + * The lbfgs_optimize() function terminates an optimization process with + * ::LBFGSERR_MAXIMUMITERATION status code when the iteration count + * exceedes this parameter. Setting this parameter to zero continues an + * optimization process until a convergence or error. The default value + * is 0. + */ + int max_iterations; + + /** + * The maximum number of trials for the line search. + * This parameter controls the number of function and gradients evaluations + * per iteration for the line search routine. The default value is 40. + */ + int max_linesearch; + + /** + * The minimum step of the line search routine. + * The default value is 1e-20. This value need not be modified unless + * the exponents are too large for the machine being used, or unless the + * problem is extremely badly scaled (in which case the exponents should + * be increased). + */ + double min_step; + + /** + * The maximum step of the line search. + * The default value is 1e+20. This value need not be modified unless + * the exponents are too large for the machine being used, or unless the + * problem is extremely badly scaled (in which case the exponents should + * be increased). + */ + double max_step; + + /** + * A parameter to control the accuracy of the line search routine. + * The default value is 1e-4. This parameter should be greater + * than zero and smaller than 0.5. + */ + double f_dec_coeff; + + /** + * A parameter to control the accuracy of the line search routine. + * The default value is 0.9. If the function and gradient + * evaluations are inexpensive with respect to the cost of the + * iteration (which is sometimes the case when solving very large + * problems) it may be advantageous to set this parameter to a small + * value. A typical small value is 0.1. This parameter shuold be + * greater than the f_dec_coeff parameter (1e-4) + * and smaller than 1.0. + */ + double s_curv_coeff; + + /** + * The machine precision for floating-point values. + * This parameter must be a positive value set by a client program to + * estimate the machine precision. The line search routine will terminate + * with the status code (::LBFGSERR_ROUNDING_ERROR) if the relative width + * of the interval of uncertainty is less than this parameter. + */ + double xtol; + }; + + /** + * Callback interface to provide objective function and gradient evaluations. + * + * The lbfgs_optimize() function call this function to obtain the values of objective + * function and its gradients when needed. A client program must implement + * this function to evaluate the values of the objective function and its + * gradients, given current values of variables. + * + * @param instance The user data sent for lbfgs_optimize() function by the client. + * @param x The current values of variables. + * @param g The gradient vector. The callback function must compute + * the gradient values for the current variables. + * @param n The number of variables. + * @retval double The value of the objective function for the current + * variables. + */ + typedef double (*lbfgs_evaluate_t)(void *instance, + const double *x, + double *g, + const int n); + + /** + * Callback interface to provide an upper bound at the beginning of current linear search. + * + * The lbfgs_optimize() function call this function to obtain the values of the + * upperbound of the stepsize to search in, provided with the beginning values of + * variables before the linear search, and the current step vector (can be descent direction). + * A client program can implement this function for more efficient linesearch. + * If it is not used, just set it NULL or nullptr. + * + * @param instance The user data sent for lbfgs_optimize() function by the client. + * @param xp The values of variables before current line search. + * @param d The step vector. It can be the descent direction. + * @param n The number of variables. + * @retval double The upperboud of the step in current line search routine, + * such that stpbound*d is the maximum reasonable step. + */ + typedef double (*lbfgs_stepbound_t)(void *instance, + const double *xp, + const double *d, + const int n); + + /** + * Callback interface to receive the progress of the optimization process. + * + * The lbfgs_optimize() function call this function for each iteration. Implementing + * this function, a client program can store or display the current progress + * of the optimization process. If it is not used, just set it NULL or nullptr. + * + * @param instance The user data sent for lbfgs_optimize() function by the client. + * @param x The current values of variables. + * @param g The current gradient values of variables. + * @param fx The current value of the objective function. + * @param xnorm The Euclidean norm of the variables. + * @param gnorm The Euclidean norm of the gradients. + * @param step The line-search step used for this iteration. + * @param n The number of variables. + * @param k The iteration count. + * @param ls The number of evaluations called for this iteration. + * @retval int Zero to continue the optimization process. Returning a + * non-zero value will cancel the optimization process. + */ + typedef int (*lbfgs_progress_t)(void *instance, + const double *x, + const double *g, + const double fx, + const double xnorm, + const double gnorm, + const double step, + int n, + int k, + int ls); + + /** + * Callback data struct + */ + + struct callback_data_t + { + int n; + void *instance; + lbfgs_evaluate_t proc_evaluate; + lbfgs_stepbound_t proc_stepbound; + lbfgs_progress_t proc_progress; + }; + + /** + * Iteration data struct + */ + struct iteration_data_t + { + double alpha; + double *s; /* [n] */ + double *y; /* [n] */ + double ys; /* vecdot(y, s) */ + }; + + // ----------------------- Arithmetic Part ----------------------- + +/** + * Define the local variables for computing minimizers. + */ +#define USES_MINIMIZER_LBFGS \ + double a, d, gamm, theta, p, q, r, s; + +/** + * Find a minimizer of an interpolated cubic function. + * @param cm The minimizer of the interpolated cubic. + * @param u The value of one point, u. + * @param fu The value of f(u). + * @param du The value of f'(u). + * @param v The value of another point, v. + * @param fv The value of f(v). + * @param du The value of f'(v). + */ +#define CUBIC_MINIMIZER_LBFGS(cm, u, fu, du, v, fv, dv) \ + d = (v) - (u); \ + theta = ((fu) - (fv)) * 3 / d + (du) + (dv); \ + p = fabs(theta); \ + q = fabs(du); \ + r = fabs(dv); \ + s = p >= q ? p : q; \ + s = s >= r ? s : r; \ + /* gamm = s*sqrt((theta/s)**2 - (du/s) * (dv/s)) */ \ + a = theta / s; \ + gamm = s * sqrt(a * a - ((du) / s) * ((dv) / s)); \ + if ((v) < (u)) \ + gamm = -gamm; \ + p = gamm - (du) + theta; \ + q = gamm - (du) + gamm + (dv); \ + r = p / q; \ + (cm) = (u) + r * d; + +/** + * Find a minimizer of an interpolated cubic function. + * @param cm The minimizer of the interpolated cubic. + * @param u The value of one point, u. + * @param fu The value of f(u). + * @param du The value of f'(u). + * @param v The value of another point, v. + * @param fv The value of f(v). + * @param du The value of f'(v). + * @param xmin The maximum value. + * @param xmin The minimum value. + */ +#define CUBIC_MINIMIZER2_LBFGS(cm, u, fu, du, v, fv, dv, xmin, xmax) \ + d = (v) - (u); \ + theta = ((fu) - (fv)) * 3 / d + (du) + (dv); \ + p = fabs(theta); \ + q = fabs(du); \ + r = fabs(dv); \ + s = p >= q ? p : q; \ + s = s >= r ? s : r; \ + /* gamm = s*sqrt((theta/s)**2 - (du/s) * (dv/s)) */ \ + a = theta / s; \ + gamm = a * a - ((du) / s) * ((dv) / s); \ + gamm = gamm > 0 ? s * sqrt(gamm) : 0; \ + if ((u) < (v)) \ + gamm = -gamm; \ + p = gamm - (dv) + theta; \ + q = gamm - (dv) + gamm + (du); \ + r = p / q; \ + if (r < 0. && gamm != 0.) \ + { \ + (cm) = (v)-r * d; \ + } \ + else if (a < 0) \ + { \ + (cm) = (xmax); \ + } \ + else \ + { \ + (cm) = (xmin); \ + } + +/** + * Find a minimizer of an interpolated quadratic function. + * @param qm The minimizer of the interpolated quadratic. + * @param u The value of one point, u. + * @param fu The value of f(u). + * @param du The value of f'(u). + * @param v The value of another point, v. + * @param fv The value of f(v). + */ +#define QUARD_MINIMIZER_LBFGS(qm, u, fu, du, v, fv) \ + a = (v) - (u); \ + (qm) = (u) + (du) / (((fu) - (fv)) / a + (du)) / 2 * a; + +/** + * Find a minimizer of an interpolated quadratic function. + * @param qm The minimizer of the interpolated quadratic. + * @param u The value of one point, u. + * @param du The value of f'(u). + * @param v The value of another point, v. + * @param dv The value of f'(v). + */ +#define QUARD_MINIMIZER2_LBFGS(qm, u, du, v, dv) \ + a = (u) - (v); \ + (qm) = (v) + (dv) / ((dv) - (du)) * a; + + inline void *vecalloc(size_t size) + { + void *memblock = malloc(size); + if (memblock) + { + memset(memblock, 0, size); + } + return memblock; + } + + inline void vecfree(void *memblock) + { + free(memblock); + } + + inline void veccpy(double *y, const double *x, const int n) + { + memcpy(y, x, sizeof(double) * n); + } + + inline void vecncpy(double *y, const double *x, const int n) + { + int i; + + for (i = 0; i < n; ++i) + { + y[i] = -x[i]; + } + } + + inline void vecadd(double *y, const double *x, const double c, const int n) + { + int i; + + for (i = 0; i < n; ++i) + { + y[i] += c * x[i]; + } + } + + inline void vecdiff(double *z, const double *x, const double *y, const int n) + { + int i; + + for (i = 0; i < n; ++i) + { + z[i] = x[i] - y[i]; + } + } + + inline void vecscale(double *y, const double c, const int n) + { + int i; + + for (i = 0; i < n; ++i) + { + y[i] *= c; + } + } + + inline void vecdot(double *s, const double *x, const double *y, const int n) + { + int i; + *s = 0.; + for (i = 0; i < n; ++i) + { + *s += x[i] * y[i]; + } + } + + inline void vec2norm(double *s, const double *x, const int n) + { + vecdot(s, x, x, n); + *s = (double)sqrt(*s); + } + + inline void vec2norminv(double *s, const double *x, const int n) + { + vec2norm(s, x, n); + *s = (double)(1.0 / *s); + } + + // ----------------------- L-BFGS Part ----------------------- + + /** + * Update a safeguarded trial value and interval for line search. + * + * The parameter x represents the step with the least function value. + * The parameter t represents the current step. This function assumes + * that the derivative at the point of x in the direction of the step. + * If the bracket is set to true, the minimizer has been bracketed in + * an interval of uncertainty with endpoints between x and y. + * + * @param x The pointer to the value of one endpoint. + * @param fx The pointer to the value of f(x). + * @param dx The pointer to the value of f'(x). + * @param y The pointer to the value of another endpoint. + * @param fy The pointer to the value of f(y). + * @param dy The pointer to the value of f'(y). + * @param t The pointer to the value of the trial value, t. + * @param ft The pointer to the value of f(t). + * @param dt The pointer to the value of f'(t). + * @param tmin The minimum value for the trial value, t. + * @param tmax The maximum value for the trial value, t. + * @param brackt The pointer to the predicate if the trial value is + * bracketed. + * @retval int Status value. Zero indicates a normal termination. + * + * @see + * Jorge J. More and David J. Thuente. Line search algorithm with + * guaranteed sufficient decrease. ACM Transactions on Mathematical + * Software (TOMS), Vol 20, No 3, pp. 286-307, 1994. + */ + inline int update_trial_interval(double *x, + double *fx, + double *dx, + double *y, + double *fy, + double *dy, + double *t, + double *ft, + double *dt, + const double tmin, + const double tmax, + int *brackt) + { + int bound; + int dsign = *dt * (*dx / fabs(*dx)) < 0.; + double mc; /* minimizer of an interpolated cubic. */ + double mq; /* minimizer of an interpolated quadratic. */ + double newt; /* new trial value. */ + USES_MINIMIZER_LBFGS; /* for CUBIC_MINIMIZER and QUARD_MINIMIZER. */ + + /* Check the input parameters for errors. */ + if (*brackt) + { + if (*t <= (*x <= *y ? *x : *y) || (*x >= *y ? *x : *y) <= *t) + { + /* The trival value t is out of the interval. */ + return LBFGSERR_OUTOFINTERVAL; + } + if (0. <= *dx * (*t - *x)) + { + /* The function must decrease from x. */ + return LBFGSERR_INCREASEGRADIENT; + } + if (tmax < tmin) + { + /* Incorrect tmin and tmax specified. */ + return LBFGSERR_INCORRECT_TMINMAX; + } + } + + /* + Trial value selection. + */ + if (*fx < *ft) + { + /* + Case 1: a higher function value. + The minimum is brackt. If the cubic minimizer is closer + to x than the quadratic one, the cubic one is taken, else + the average of the minimizers is taken. + */ + *brackt = 1; + bound = 1; + CUBIC_MINIMIZER_LBFGS(mc, *x, *fx, *dx, *t, *ft, *dt); + QUARD_MINIMIZER_LBFGS(mq, *x, *fx, *dx, *t, *ft); + if (fabs(mc - *x) < fabs(mq - *x)) + { + newt = mc; + } + else + { + newt = mc + 0.5 * (mq - mc); + } + } + else if (dsign) + { + /* + Case 2: a lower function value and derivatives of + opposite sign. The minimum is brackt. If the cubic + minimizer is closer to x than the quadratic (secant) one, + the cubic one is taken, else the quadratic one is taken. + */ + *brackt = 1; + bound = 0; + CUBIC_MINIMIZER_LBFGS(mc, *x, *fx, *dx, *t, *ft, *dt); + QUARD_MINIMIZER2_LBFGS(mq, *x, *dx, *t, *dt); + if (fabs(mc - *t) > fabs(mq - *t)) + { + newt = mc; + } + else + { + newt = mq; + } + } + else if (fabs(*dt) < fabs(*dx)) + { + /* + Case 3: a lower function value, derivatives of the + same sign, and the magnitude of the derivative decreases. + The cubic minimizer is only used if the cubic tends to + infinity in the direction of the minimizer or if the minimum + of the cubic is beyond t. Otherwise the cubic minimizer is + defined to be either tmin or tmax. The quadratic (secant) + minimizer is also computed and if the minimum is brackt + then the the minimizer closest to x is taken, else the one + farthest away is taken. + */ + bound = 1; + CUBIC_MINIMIZER2_LBFGS(mc, *x, *fx, *dx, *t, *ft, *dt, tmin, tmax); + QUARD_MINIMIZER2_LBFGS(mq, *x, *dx, *t, *dt); + if (*brackt) + { + if (fabs(*t - mc) < fabs(*t - mq)) + { + newt = mc; + } + else + { + newt = mq; + } + } + else + { + if (fabs(*t - mc) > fabs(*t - mq)) + { + newt = mc; + } + else + { + newt = mq; + } + } + } + else + { + /* + Case 4: a lower function value, derivatives of the + same sign, and the magnitude of the derivative does + not decrease. If the minimum is not brackt, the step + is either tmin or tmax, else the cubic minimizer is taken. + */ + bound = 0; + if (*brackt) + { + CUBIC_MINIMIZER_LBFGS(newt, *t, *ft, *dt, *y, *fy, *dy); + } + else if (*x < *t) + { + newt = tmax; + } + else + { + newt = tmin; + } + } + + /* + Update the interval of uncertainty. This update does not + depend on the new step or the case analysis above. + + - Case a: if f(x) < f(t), + x <- x, y <- t. + - Case b: if f(t) <= f(x) && f'(t)*f'(x) > 0, + x <- t, y <- y. + - Case c: if f(t) <= f(x) && f'(t)*f'(x) < 0, + x <- t, y <- x. + */ + if (*fx < *ft) + { + /* Case a */ + *y = *t; + *fy = *ft; + *dy = *dt; + } + else + { + /* Case c */ + if (dsign) + { + *y = *x; + *fy = *fx; + *dy = *dx; + } + /* Cases b and c */ + *x = *t; + *fx = *ft; + *dx = *dt; + } + + /* Clip the new trial value in [tmin, tmax]. */ + if (tmax < newt) + newt = tmax; + if (newt < tmin) + newt = tmin; + + /* + Redefine the new trial value if it is close to the upper bound + of the interval. + */ + if (*brackt && bound) + { + mq = *x + 0.66 * (*y - *x); + if (*x < *y) + { + if (mq < newt) + newt = mq; + } + else + { + if (newt < mq) + newt = mq; + } + } + + /* Return the new trial value. */ + *t = newt; + return 0; + } + + inline int line_search_morethuente(int n, + double *x, + double *f, + double *g, + double *s, + double *stp, + const double *xp, + const double *gp, + const double *stpmin, + const double *stpmax, + callback_data_t *cd, + const lbfgs_parameter_t *param) + { + int count = 0; + int brackt, stage1, uinfo = 0; + double dg; + double stx, fx, dgx; + double sty, fy, dgy; + double fxm, dgxm, fym, dgym, fm, dgm; + double finit, ftest1, dginit, dgtest; + double width, prev_width; + double stmin, stmax; + + /* Check the input parameters for errors. */ + if (*stp <= 0.) + { + return LBFGSERR_INVALIDPARAMETERS; + } + + /* Compute the initial gradient in the search direction. */ + vecdot(&dginit, g, s, n); + + /* Make sure that s points to a descent direction. */ + if (0 < dginit) + { + return LBFGSERR_INCREASEGRADIENT; + } + + /* Initialize local variables. */ + brackt = 0; + stage1 = 1; + finit = *f; + dgtest = param->f_dec_coeff * dginit; + width = *stpmax - *stpmin; + prev_width = 2.0 * width; + + /* + The variables stx, fx, dgx contain the values of the step, + function, and directional derivative at the best step. + The variables sty, fy, dgy contain the value of the step, + function, and derivative at the other endpoint of + the interval of uncertainty. + The variables stp, f, dg contain the values of the step, + function, and derivative at the current step. + */ + stx = sty = 0.; + fx = fy = finit; + dgx = dgy = dginit; + + for (;;) + { + /* Report the progress. */ + if (cd->proc_progress) + { + double xnorm; + double gnorm; + vec2norm(&xnorm, x, n); + vec2norm(&gnorm, g, n); + if (cd->proc_progress(cd->instance, x, g, fx, xnorm, gnorm, *stp, cd->n, 0, 0)) + { + return LBFGSERR_CANCELED; + } + } + + /* + Set the minimum and maximum steps to correspond to the + present interval of uncertainty. + */ + if (brackt) + { + stmin = stx <= sty ? stx : sty; + stmax = stx >= sty ? stx : sty; + } + else + { + stmin = stx; + stmax = *stp + 4.0 * (*stp - stx); + } + + /* Clip the step in the range of [stpmin, stpmax]. */ + if (*stp < *stpmin) + *stp = *stpmin; + if (*stpmax < *stp) + *stp = *stpmax; + + /* + If an unusual termination is to occur then let + stp be the lowest point obtained so far. + */ + if ((brackt && ((*stp <= stmin || stmax <= *stp) || param->max_linesearch <= count + 1 || uinfo != 0)) || (brackt && (stmax - stmin <= param->xtol * stmax))) + { + *stp = stx; + } + + /* + Compute the current value of x: + x <- x + (*stp) * s. + */ + veccpy(x, xp, n); + vecadd(x, s, *stp, n); + + /* Evaluate the function and gradient values. */ + *f = cd->proc_evaluate(cd->instance, x, g, cd->n); + vecdot(&dg, g, s, n); + + ftest1 = finit + *stp * dgtest; + ++count; + + /* Test for errors and convergence. */ + if (brackt && ((*stp <= stmin || stmax <= *stp) || uinfo != 0)) + { + /* Rounding errors prevent further progress. */ + return LBFGSERR_ROUNDING_ERROR; + } + if (*stp == *stpmax && *f <= ftest1 && dg <= dgtest) + { + /* The step is the maximum value. */ + return LBFGSERR_MAXIMUMSTEP; + } + if (*stp == *stpmin && (ftest1 < *f || dgtest <= dg)) + { + /* The step is the minimum value. */ + return LBFGSERR_MINIMUMSTEP; + } + if (brackt && (stmax - stmin) <= param->xtol * stmax) + { + /* Relative width of the interval of uncertainty is at most xtol. */ + return LBFGSERR_WIDTHTOOSMALL; + } + if (param->max_linesearch <= count) + { + /* Maximum number of iteration. */ + return LBFGSERR_MAXIMUMLINESEARCH; + } + if (*f <= ftest1 && fabs(dg) <= param->s_curv_coeff * (-dginit)) + { + /* The sufficient decrease condition and the strong curvature condition hold. */ + return count; + } + + /* + In the first stage we seek a step for which the modified + function has a nonpositive value and nonnegative derivative. + */ + if (stage1 && *f <= ftest1 && + (param->f_dec_coeff <= param->s_curv_coeff ? param->f_dec_coeff : param->s_curv_coeff) * dginit <= dg) + { + stage1 = 0; + } + + /* + A modified function is used to predict the step only if + we have not obtained a step for which the modified + function has a nonpositive function value and nonnegative + derivative, and if a lower function value has been + obtained but the decrease is not sufficient. + */ + if (stage1 && ftest1 < *f && *f <= fx) + { + /* Define the modified function and derivative values. */ + fm = *f - *stp * dgtest; + fxm = fx - stx * dgtest; + fym = fy - sty * dgtest; + dgm = dg - dgtest; + dgxm = dgx - dgtest; + dgym = dgy - dgtest; + + /* + Call update_trial_interval() to update the interval of + uncertainty and to compute the new step. + */ + uinfo = update_trial_interval( + &stx, &fxm, &dgxm, + &sty, &fym, &dgym, + stp, &fm, &dgm, + stmin, stmax, &brackt); + + /* Reset the function and gradient values for f. */ + fx = fxm + stx * dgtest; + fy = fym + sty * dgtest; + dgx = dgxm + dgtest; + dgy = dgym + dgtest; + } + else + { + /* + Call update_trial_interval() to update the interval of + uncertainty and to compute the new step. + */ + uinfo = update_trial_interval( + &stx, &fx, &dgx, + &sty, &fy, &dgy, + stp, f, &dg, + stmin, stmax, &brackt); + } + + /* + Force a sufficient decrease in the interval of uncertainty. + */ + if (brackt) + { + if (0.66 * prev_width <= fabs(sty - stx)) + { + *stp = stx + 0.5 * (sty - stx); + } + prev_width = width; + width = fabs(sty - stx); + } + } + + return LBFGSERR_LOGICERROR; + } + + /** + * Default L-BFGS parameters. + */ + static const lbfgs_parameter_t _default_param = { + 8, + 1e-5, + 0, + 1e-5, + 0, + 40, + 1e-20, + 1e20, + 1e-4, + 0.9, + 1.0e-16, + }; + + /** + * Initialize L-BFGS parameters to the default values. + * + * Call this function to fill a parameter structure with the default values + * and overwrite parameter values if necessary. + * + * @param param The pointer to the parameter structure. + */ + inline void lbfgs_load_default_parameters(lbfgs_parameter_t *param) + { + memcpy(param, &_default_param, sizeof(*param)); + } + + /** + * Start a L-BFGS optimization. + * A user must implement a function compatible with ::lbfgs_evaluate_t (evaluation + * callback) and pass the pointer to the callback function to lbfgs_optimize() + * arguments. Similarly, a user can implement a function compatible with + * ::lbfgs_stepbound_t to provide an external upper bound for stepsize, and + * ::lbfgs_progress_t (progress callback) to obtain the current progress + * (e.g., variables, function value, ||G||, etc) and to cancel the iteration + * process if necessary. Implementation of the stepbound and the progress callback + * is optional: a user can pass NULL if progress notification is not necessary. + * + * This algorithm terminates an optimization + * when: + * + * ||G|| < g_epsilon \cdot \max(1, ||x||) . + * + * In this formula, ||.|| denotes the Euclidean norm. + * + * @param n The number of variables. + * @param x The array of variables. A client program can set + * default values for the optimization and receive the + * optimization result through this array. + * @param ptr_fx The pointer to the variable that receives the final + * value of the objective function for the variables. + * This argument can be set to NULL if the final + * value of the objective function is unnecessary. + * @param proc_evaluate The callback function to provide function and + * gradient evaluations given a current values of + * variables. A client program must implement a + * callback function compatible with + * lbfgs_evaluate_t and pass the pointer to the + * callback function. + * @param proc_stepbound The callback function to provide values of the + * upperbound of the stepsize to search in, provided + * with the beginning values of variables before the + * linear search, and the current step vector (can + * be negative gradient). A client program can implement + * this function for more efficient linesearch. If it is + * not used, just set it NULL or nullptr. + * @param proc_progress The callback function to receive the progress + * (the number of iterations, the current value of + * the objective function) of the minimization + * process. This argument can be set to NULL if + * a progress report is unnecessary. + * @param instance A user data for the client program. The callback + * functions will receive the value of this argument. + * @param param The pointer to a structure representing parameters for + * L-BFGS optimization. A client program can set this + * parameter to NULL to use the default parameters. + * Call lbfgs_load_default_parameters() function to + * fill a structure with the default values. + * @retval int The status code. This function returns zero if the + * minimization process terminates without an error. A + * non-zero value indicates an error. + */ + inline int lbfgs_optimize(int n, + double *x, + double *ptr_fx, + lbfgs_evaluate_t proc_evaluate, + lbfgs_stepbound_t proc_stepbound, + lbfgs_progress_t proc_progress, + void *instance, + lbfgs_parameter_t *_param) + { + int ret; + int i, j, k, ls, end, bound; + double step; + int loop; + double step_min, step_max; + + /* Constant parameters and their default values. */ + lbfgs_parameter_t param = (_param != NULL) ? (*_param) : _default_param; + const int m = param.mem_size; + + double *xp = NULL; + double *g = NULL, *gp = NULL; + double *d = NULL, *pf = NULL; + iteration_data_t *lm = NULL, *it = NULL; + double ys, yy; + double xnorm, gnorm, beta; + double fx = 0.; + double rate = 0.; + + /* Construct a callback data. */ + callback_data_t cd; + cd.n = n; + cd.instance = instance; + cd.proc_evaluate = proc_evaluate; + cd.proc_stepbound = proc_stepbound; + cd.proc_progress = proc_progress; + + /* Check the input parameters for errors. */ + if (n <= 0) + { + return LBFGSERR_INVALID_N; + } + if (m <= 0) + { + return LBFGSERR_INVALID_MEMSIZE; + } + if (param.g_epsilon < 0.) + { + return LBFGSERR_INVALID_GEPSILON; + } + if (param.past < 0) + { + return LBFGSERR_INVALID_TESTPERIOD; + } + if (param.delta < 0.) + { + return LBFGSERR_INVALID_DELTA; + } + if (param.min_step < 0.) + { + return LBFGSERR_INVALID_MINSTEP; + } + if (param.max_step < param.min_step) + { + return LBFGSERR_INVALID_MAXSTEP; + } + if (param.f_dec_coeff < 0.) + { + return LBFGSERR_INVALID_FDECCOEFF; + } + if (param.s_curv_coeff <= param.f_dec_coeff || 1. <= param.s_curv_coeff) + { + return LBFGSERR_INVALID_SCURVCOEFF; + } + if (param.xtol < 0.) + { + return LBFGSERR_INVALID_XTOL; + } + if (param.max_linesearch <= 0) + { + return LBFGSERR_INVALID_MAXLINESEARCH; + } + + /* Allocate working space. */ + xp = (double *)vecalloc(n * sizeof(double)); + g = (double *)vecalloc(n * sizeof(double)); + gp = (double *)vecalloc(n * sizeof(double)); + d = (double *)vecalloc(n * sizeof(double)); + + /* Allocate limited memory storage. */ + lm = (iteration_data_t *)vecalloc(m * sizeof(iteration_data_t)); + + /* Initialize the limited memory. */ + for (i = 0; i < m; ++i) + { + it = &lm[i]; + it->alpha = 0; + it->ys = 0; + it->s = (double *)vecalloc(n * sizeof(double)); + it->y = (double *)vecalloc(n * sizeof(double)); + } + + /* Allocate an array for storing previous values of the objective function. */ + if (0 < param.past) + { + pf = (double *)vecalloc(param.past * sizeof(double)); + } + + /* Evaluate the function value and its gradient. */ + fx = cd.proc_evaluate(cd.instance, x, g, cd.n); + + /* Store the initial value of the objective function. */ + if (pf != NULL) + { + pf[0] = fx; + } + + /* + Compute the direction; + we assume the initial hessian matrix H_0 as the identity matrix. + */ + vecncpy(d, g, n); + + /* + Make sure that the initial variables are not a minimizer. + */ + vec2norm(&xnorm, x, n); + vec2norm(&gnorm, g, n); + + if (xnorm < 1.0) + xnorm = 1.0; + if (gnorm / xnorm <= param.g_epsilon) + { + ret = LBFGS_ALREADY_MINIMIZED; + } + else + { + /* Compute the initial step: + step = 1.0 / sqrt(vecdot(d, d, n)) + */ + vec2norminv(&step, d, n); + + k = 1; + end = 0; + loop = 1; + + while (loop == 1) + { + /* Store the current position and gradient vectors. */ + veccpy(xp, x, n); + veccpy(gp, g, n); + + // If the step bound can be provied dynamically, then apply it. + step_min = param.min_step; + step_max = param.max_step; + if (cd.proc_stepbound) + { + step_max = cd.proc_stepbound(cd.instance, xp, d, cd.n); + step_max = step_max < param.max_step ? step_max : param.max_step; + if (step >= step_max) + step = step_max / 2.0; + } + + /* Search for an optimal step. */ + ls = line_search_morethuente(n, x, &fx, g, d, &step, xp, gp, &step_min, &step_max, &cd, ¶m); + + if (ls < 0) + { + /* Revert to the previous point. */ + veccpy(x, xp, n); + veccpy(g, gp, n); + ret = ls; + loop = 0; + continue; + } + + /* Compute x and g norms. */ + vec2norm(&xnorm, x, n); + vec2norm(&gnorm, g, n); + + // /* Report the progress. */ + // if (cd.proc_progress) + // { + // if ((ret = cd.proc_progress(cd.instance, x, g, fx, xnorm, gnorm, step, cd.n, k, ls))) + // { + // loop = 0; + // continue; + // } + // } + + /* + Convergence test. + The criterion is given by the following formula: + |g(x)| / \max(1, |x|) < g_epsilon + */ + if (xnorm < 1.0) + xnorm = 1.0; + if (gnorm / xnorm <= param.g_epsilon) + { + /* Convergence. */ + ret = LBFGS_CONVERGENCE; + break; + } + + /* + Test for stopping criterion. + The criterion is given by the following formula: + |(f(past_x) - f(x))| / f(x) < \delta + */ + if (pf != NULL) + { + /* We don't test the stopping criterion while k < past. */ + if (param.past <= k) + { + /* Compute the relative improvement from the past. */ + rate = (pf[k % param.past] - fx) / fx; + + /* The stopping criterion. */ + if (fabs(rate) < param.delta) + { + ret = LBFGS_STOP; + break; + } + } + + /* Store the current value of the objective function. */ + pf[k % param.past] = fx; + } + + if (param.max_iterations != 0 && param.max_iterations < k + 1) + { + /* Maximum number of iterations. */ + ret = LBFGSERR_MAXIMUMITERATION; + break; + } + + /* + Update vectors s and y: + s_{k+1} = x_{k+1} - x_{k} = \step * d_{k}. + y_{k+1} = g_{k+1} - g_{k}. + */ + it = &lm[end]; + vecdiff(it->s, x, xp, n); + vecdiff(it->y, g, gp, n); + + /* + Compute scalars ys and yy: + ys = y^t \cdot s = 1 / \rho. + yy = y^t \cdot y. + Notice that yy is used for scaling the hessian matrix H_0 (Cholesky factor). + */ + vecdot(&ys, it->y, it->s, n); + vecdot(&yy, it->y, it->y, n); + it->ys = ys; + + /* + Recursive formula to compute dir = -(H \cdot g). + This is described in page 779 of: + Jorge Nocedal. + Updating Quasi-Newton Matrices with Limited Storage. + Mathematics of Computation, Vol. 35, No. 151, + pp. 773--782, 1980. + */ + bound = (m <= k) ? m : k; + ++k; + end = (end + 1) % m; + + /* Compute the negative of gradients. */ + vecncpy(d, g, n); + + j = end; + for (i = 0; i < bound; ++i) + { + j = (j + m - 1) % m; /* if (--j == -1) j = m-1; */ + it = &lm[j]; + /* \alpha_{j} = \rho_{j} s^{t}_{j} \cdot q_{k+1}. */ + vecdot(&it->alpha, it->s, d, n); + it->alpha /= it->ys; + /* q_{i} = q_{i+1} - \alpha_{i} y_{i}. */ + vecadd(d, it->y, -it->alpha, n); + } + + vecscale(d, ys / yy, n); + + for (i = 0; i < bound; ++i) + { + it = &lm[j]; + /* \beta_{j} = \rho_{j} y^t_{j} \cdot \gamm_{i}. */ + vecdot(&beta, it->y, d, n); + beta /= it->ys; + /* \gamm_{i+1} = \gamm_{i} + (\alpha_{j} - \beta_{j}) s_{j}. */ + vecadd(d, it->s, it->alpha - beta, n); + j = (j + 1) % m; /* if (++j == m) j = 0; */ + } + + /* + Now the search direction d is ready. We try step = 1 first. + */ + step = 1.0; + } + } + + /* Return the final value of the objective function. */ + if (ptr_fx != NULL) + { + *ptr_fx = fx; + } + + vecfree(pf); + + /* Free memory blocks used by this function. */ + if (lm != NULL) + { + for (i = 0; i < m; ++i) + { + vecfree(lm[i].s); + vecfree(lm[i].y); + } + vecfree(lm); + } + vecfree(d); + vecfree(gp); + vecfree(g); + vecfree(xp); + + return ret; + } + + /** + * Get string description of an lbfgs_optimize() return code. + * + * @param err A value returned by lbfgs_optimize(). + */ + inline const char *lbfgs_strerror(int err) + { + switch (err) + { + case LBFGS_CONVERGENCE: + return "Success: reached convergence (g_epsilon)."; + + case LBFGS_STOP: + return "Success: met stopping criteria (past f decrease less than delta)."; + + case LBFGS_ALREADY_MINIMIZED: + return "The initial variables already minimize the objective function."; + + case LBFGSERR_UNKNOWNERROR: + return "Unknown error."; + + case LBFGSERR_LOGICERROR: + return "Logic error."; + + case LBFGSERR_CANCELED: + return "The minimization process has been canceled."; + + case LBFGSERR_INVALID_N: + return "Invalid number of variables specified."; + + case LBFGSERR_INVALID_MEMSIZE: + return "Invalid parameter lbfgs_parameter_t::mem_size specified."; + + case LBFGSERR_INVALID_GEPSILON: + return "Invalid parameter lbfgs_parameter_t::g_epsilon specified."; + + case LBFGSERR_INVALID_TESTPERIOD: + return "Invalid parameter lbfgs_parameter_t::past specified."; + + case LBFGSERR_INVALID_DELTA: + return "Invalid parameter lbfgs_parameter_t::delta specified."; + + case LBFGSERR_INVALID_MINSTEP: + return "Invalid parameter lbfgs_parameter_t::min_step specified."; + + case LBFGSERR_INVALID_MAXSTEP: + return "Invalid parameter lbfgs_parameter_t::max_step specified."; + + case LBFGSERR_INVALID_FDECCOEFF: + return "Invalid parameter lbfgs_parameter_t::f_dec_coeff specified."; + + case LBFGSERR_INVALID_SCURVCOEFF: + return "Invalid parameter lbfgs_parameter_t::s_curv_coeff specified."; + + case LBFGSERR_INVALID_XTOL: + return "Invalid parameter lbfgs_parameter_t::xtol specified."; + + case LBFGSERR_INVALID_MAXLINESEARCH: + return "Invalid parameter lbfgs_parameter_t::max_linesearch specified."; + + case LBFGSERR_OUTOFINTERVAL: + return "The line-search step went out of the interval of uncertainty."; + + case LBFGSERR_INCORRECT_TMINMAX: + return "A logic error occurred; alternatively, the interval of uncertainty" + " became too small."; + + case LBFGSERR_ROUNDING_ERROR: + return "A rounding error occurred; alternatively, no line-search step" + " satisfies the sufficient decrease and curvature conditions."; + + case LBFGSERR_MINIMUMSTEP: + return "The line-search step became smaller than lbfgs_parameter_t::min_step."; + + case LBFGSERR_MAXIMUMSTEP: + return "The line-search step became larger than lbfgs_parameter_t::max_step."; + + case LBFGSERR_MAXIMUMLINESEARCH: + return "The line-search routine reaches the maximum number of evaluations."; + + case LBFGSERR_MAXIMUMITERATION: + return "The algorithm routine reaches the maximum number of iterations."; + + case LBFGSERR_WIDTHTOOSMALL: + return "Relative width of the interval of uncertainty is at most" + " lbfgs_parameter_t::xtol."; + + case LBFGSERR_INVALIDPARAMETERS: + return "A logic error (negative line-search step) occurred."; + + case LBFGSERR_INCREASEGRADIENT: + return "The current search direction increases the objective function value."; + + default: + return "(unknown)"; + } + } + +} // namespace lbfgs + +#endif \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/uniform_bspline.h b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/uniform_bspline.h new file mode 100644 index 00000000..d2507a27 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/include/bspline_opt/uniform_bspline.h @@ -0,0 +1,80 @@ +#ifndef _UNIFORM_BSPLINE_H_ +#define _UNIFORM_BSPLINE_H_ + +#include +#include +#include + +using namespace std; + +namespace ego_planner +{ + // An implementation of non-uniform B-spline with different dimensions + // It also represents uniform B-spline which is a special case of non-uniform + class UniformBspline + { + private: + // control points for B-spline with different dimensions. + // Each row represents one single control point + // The dimension is determined by column number + // e.g. B-spline with N points in 3D space -> Nx3 matrix + Eigen::MatrixXd control_points_; + + int p_, n_, m_; // p degree, n+1 control points, m = n+p+1 + Eigen::VectorXd u_; // knots vector + double interval_; // knot span \delta t + + Eigen::MatrixXd getDerivativeControlPoints(); + + double limit_vel_, limit_acc_, limit_ratio_, feasibility_tolerance_; // physical limits and time adjustment ratio + + public: + UniformBspline() {} + UniformBspline(const Eigen::MatrixXd &points, const int &order, const double &interval); + ~UniformBspline(); + + Eigen::MatrixXd get_control_points(void) { return control_points_; } + + // initialize as an uniform B-spline + void setUniformBspline(const Eigen::MatrixXd &points, const int &order, const double &interval); + + // get / set basic bspline info + + void setKnot(const Eigen::VectorXd &knot); + Eigen::VectorXd getKnot(); + Eigen::MatrixXd getControlPoint(); + double getInterval(); + bool getTimeSpan(double &um, double &um_p); + + // compute position / derivative + + Eigen::VectorXd evaluateDeBoor(const double &u); // use u \in [up, u_mp] + inline Eigen::VectorXd evaluateDeBoorT(const double &t) { return evaluateDeBoor(t + u_(p_)); } // use t \in [0, duration] + UniformBspline getDerivative(); + + // 3D B-spline interpolation of points in point_set, with boundary vel&acc + // constraints + // input : (K+2) points with boundary vel/acc; ts + // output: (K+6) control_pts + static void parameterizeToBspline(const double &ts, const vector &point_set, + const vector &start_end_derivative, + Eigen::MatrixXd &ctrl_pts); + + /* check feasibility, adjust time */ + + void setPhysicalLimits(const double &vel, const double &acc, const double &tolerance); + bool checkFeasibility(double &ratio, bool show = false); + void lengthenTime(const double &ratio); + + /* for performance evaluation */ + + double getTimeSum(); + double getLength(const double &res = 0.01); + double getJerk(); + void getMeanAndMaxVel(double &mean_v, double &max_v); + void getMeanAndMaxAcc(double &mean_a, double &max_a); + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} // namespace ego_planner +#endif \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/package.xml b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/package.xml new file mode 100755 index 00000000..02e098be --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/package.xml @@ -0,0 +1,77 @@ + + + bspline_opt + 0.0.0 + The bspline_opt package + + + + + iszhouxin + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + plan_env + path_searching + traj_utils + roscpp + rospy + std_msgs + plan_env + path_searching + traj_utils + roscpp + rospy + std_msgs + plan_env + path_searching + traj_utils + + + + + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/src/bspline_optimizer.cpp b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/src/bspline_optimizer.cpp new file mode 100644 index 00000000..4d113323 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/src/bspline_optimizer.cpp @@ -0,0 +1,1735 @@ +#include "bspline_opt/bspline_optimizer.h" +#include "bspline_opt/gradient_descent_optimizer.h" +// using namespace std; + +namespace ego_planner +{ + + void BsplineOptimizer::setParam(ros::NodeHandle &nh) + { + nh.param("optimization/lambda_smooth", lambda1_, -1.0); + nh.param("optimization/lambda_collision", lambda2_, -1.0); + nh.param("optimization/lambda_feasibility", lambda3_, -1.0); + nh.param("optimization/lambda_fitness", lambda4_, -1.0); + + nh.param("optimization/dist0", dist0_, -1.0); + nh.param("optimization/swarm_clearance", swarm_clearance_, -1.0); + nh.param("optimization/max_vel", max_vel_, -1.0); + nh.param("optimization/max_acc", max_acc_, -1.0); + // 3维 + nh.param("optimization/order", order_, 3); + } + + void BsplineOptimizer::setEnvironment(const GridMap::Ptr &map) + { + this->grid_map_ = map; + } + + void BsplineOptimizer::setEnvironment(const GridMap::Ptr &map, const fast_planner::ObjPredictor::Ptr mov_obj) + { + this->grid_map_ = map; + this->moving_objs_ = mov_obj; + } + + void BsplineOptimizer::setControlPoints(const Eigen::MatrixXd &points) + { + cps_.points = points; + } + + void BsplineOptimizer::setBsplineInterval(const double &ts) { bspline_interval_ = ts; } + + void BsplineOptimizer::setSwarmTrajs(SwarmTrajData *swarm_trajs_ptr) { swarm_trajs_ = swarm_trajs_ptr; } + + void BsplineOptimizer::setDroneId(const int drone_id) { drone_id_ = drone_id; } + + std::vector BsplineOptimizer::distinctiveTrajs(vector> segments) + { + if (segments.size() == 0) // will be invoked again later. + { + std::vector oneSeg; + oneSeg.push_back(cps_); + return oneSeg; + } + + constexpr int MAX_TRAJS = 8; + constexpr int VARIS = 2; + int seg_upbound = std::min((int)segments.size(), static_cast(floor(log(MAX_TRAJS) / log(VARIS)))); + std::vector control_pts_buf; + control_pts_buf.reserve(MAX_TRAJS); + const double RESOLUTION = grid_map_->getResolution(); + const double CTRL_PT_DIST = (cps_.points.col(0) - cps_.points.col(cps_.size - 1)).norm() / (cps_.size - 1); + + // Step 1. Find the opposite vectors and base points for every segment. + std::vector> RichInfoSegs; + for (int i = 0; i < seg_upbound; i++) + { + std::pair RichInfoOneSeg; + ControlPoints RichInfoOneSeg_temp; + cps_.segment(RichInfoOneSeg_temp, segments[i].first, segments[i].second); + RichInfoOneSeg.first = RichInfoOneSeg_temp; + RichInfoOneSeg.second = RichInfoOneSeg_temp; + RichInfoSegs.push_back(RichInfoOneSeg); + + // cout << "RichInfoOneSeg_temp, out" << endl; + // cout << "RichInfoSegs[" << i << "].first" << endl; + // for ( int k=0; k 0 ) + // { + // cout << "###" << RichInfoOneSeg_temp.points.col(k).transpose() << endl; + // for (int k2 = 0; k2 < RichInfoOneSeg_temp.base_point[k].size(); k2++) + // { + // cout << " " << RichInfoOneSeg_temp.base_point[k][k2].transpose() << " @ " << RichInfoOneSeg_temp.direction[k][k2].transpose() << endl; + // } + // } + } + + for (int i = 0; i < seg_upbound; i++) + { + + // 1.1 Find the start occupied point id and the last occupied point id + if (RichInfoSegs[i].first.size > 1) + { + int occ_start_id = -1, occ_end_id = -1; + Eigen::Vector3d occ_start_pt, occ_end_pt; + for (int j = 0; j < RichInfoSegs[i].first.size - 1; j++) + { + //cout << "A *" << j << "*" << endl; + double step_size = RESOLUTION / (RichInfoSegs[i].first.points.col(j) - RichInfoSegs[i].first.points.col(j + 1)).norm() / 2; + for (double a = 1; a > 0; a -= step_size) + { + Eigen::Vector3d pt(a * RichInfoSegs[i].first.points.col(j) + (1 - a) * RichInfoSegs[i].first.points.col(j + 1)); + //cout << " " << grid_map_->getInflateOccupancy(pt) << " pt=" << pt.transpose() << endl; + if (grid_map_->getInflateOccupancy(pt)) + { + occ_start_id = j; + occ_start_pt = pt; + goto exit_multi_loop1; + } + } + } + exit_multi_loop1:; + for (int j = RichInfoSegs[i].first.size - 1; j >= 1; j--) + { + //cout << "j=" << j << endl; + //cout << "B *" << j << "*" << endl; + ; + double step_size = RESOLUTION / (RichInfoSegs[i].first.points.col(j) - RichInfoSegs[i].first.points.col(j - 1)).norm(); + for (double a = 1; a > 0; a -= step_size) + { + Eigen::Vector3d pt(a * RichInfoSegs[i].first.points.col(j) + (1 - a) * RichInfoSegs[i].first.points.col(j - 1)); + //cout << " " << grid_map_->getInflateOccupancy(pt) << " pt=" << pt.transpose() << endl; + ; + if (grid_map_->getInflateOccupancy(pt)) + { + occ_end_id = j; + occ_end_pt = pt; + goto exit_multi_loop2; + } + } + } + exit_multi_loop2:; + + // double check + if (occ_start_id == -1 || occ_end_id == -1) + { + // It means that the first or the last control points of one segment are in obstacles, which is not allowed. + // ROS_WARN("What? occ_start_id=%d, occ_end_id=%d", occ_start_id, occ_end_id); + + segments.erase(segments.begin() + i); + RichInfoSegs.erase(RichInfoSegs.begin() + i); + seg_upbound--; + i--; + + continue; + + // cout << "RichInfoSegs[" << i << "].first" << endl; + // for (int k = 0; k < RichInfoSegs[i].first.size; k++) + // { + // if (RichInfoSegs[i].first.base_point.size() > 0) + // { + // cout << "###" << RichInfoSegs[i].first.points.col(k).transpose() << endl; + // for (int k2 = 0; k2 < RichInfoSegs[i].first.base_point[k].size(); k2++) + // { + // cout << " " << RichInfoSegs[i].first.base_point[k][k2].transpose() << " @ " << RichInfoSegs[i].first.direction[k][k2].transpose() << endl; + // } + // } + // } + } + + // 1.2 Reverse the vector and find new base points from occ_start_id to occ_end_id. + for (int j = occ_start_id; j <= occ_end_id; j++) + { + Eigen::Vector3d base_pt_reverse, base_vec_reverse; + if (RichInfoSegs[i].first.base_point[j].size() != 1) + { + cout << "RichInfoSegs[" << i << "].first.base_point[" << j << "].size()=" << RichInfoSegs[i].first.base_point[j].size() << endl; + ROS_ERROR("Wrong number of base_points!!! Should not be happen!."); + + cout << setprecision(5); + cout << "cps_" << endl; + cout << " clearance=" << cps_.clearance << " cps.size=" << cps_.size << endl; + for (int temp_i = 0; temp_i < cps_.size; temp_i++) + { + if (cps_.base_point[temp_i].size() > 1 && cps_.base_point[temp_i].size() < 1000) + { + ROS_ERROR("Should not happen!!!"); + cout << "######" << cps_.points.col(temp_i).transpose() << endl; + for (size_t temp_j = 0; temp_j < cps_.base_point[temp_i].size(); temp_j++) + cout << " " << cps_.base_point[temp_i][temp_j].transpose() << " @ " << cps_.direction[temp_i][temp_j].transpose() << endl; + } + } + + std::vector blank; + return blank; + } + + base_vec_reverse = -RichInfoSegs[i].first.direction[j][0]; + + // The start and the end case must get taken special care of. + if (j == occ_start_id) + { + base_pt_reverse = occ_start_pt; + } + else if (j == occ_end_id) + { + base_pt_reverse = occ_end_pt; + } + else + { + base_pt_reverse = RichInfoSegs[i].first.points.col(j) + base_vec_reverse * (RichInfoSegs[i].first.base_point[j][0] - RichInfoSegs[i].first.points.col(j)).norm(); + } + + if (grid_map_->getInflateOccupancy(base_pt_reverse)) // Search outward. + { + double l_upbound = 5 * CTRL_PT_DIST; // "5" is the threshold. + double l = RESOLUTION; + for (; l <= l_upbound; l += RESOLUTION) + { + Eigen::Vector3d base_pt_temp = base_pt_reverse + l * base_vec_reverse; + //cout << base_pt_temp.transpose() << endl; + if (!grid_map_->getInflateOccupancy(base_pt_temp)) + { + RichInfoSegs[i].second.base_point[j][0] = base_pt_temp; + RichInfoSegs[i].second.direction[j][0] = base_vec_reverse; + break; + } + } + if (l > l_upbound) + { + ROS_WARN("Can't find the new base points at the opposite within the threshold. i=%d, j=%d", i, j); + + segments.erase(segments.begin() + i); + RichInfoSegs.erase(RichInfoSegs.begin() + i); + seg_upbound--; + i--; + + goto exit_multi_loop3; // break "for (int j = 0; j < RichInfoSegs[i].first.size; j++)" + } + } + else if ((base_pt_reverse - RichInfoSegs[i].first.points.col(j)).norm() >= RESOLUTION) // Unnecessary to search. + { + RichInfoSegs[i].second.base_point[j][0] = base_pt_reverse; + RichInfoSegs[i].second.direction[j][0] = base_vec_reverse; + } + else + { + ROS_WARN("base_point and control point are too close!"); + cout << "base_point=" << RichInfoSegs[i].first.base_point[j][0].transpose() << " control point=" << RichInfoSegs[i].first.points.col(j).transpose() << endl; + + segments.erase(segments.begin() + i); + RichInfoSegs.erase(RichInfoSegs.begin() + i); + seg_upbound--; + i--; + + goto exit_multi_loop3; // break "for (int j = 0; j < RichInfoSegs[i].first.size; j++)" + } + } + + // 1.3 Assign the base points to control points within [0, occ_start_id) and (occ_end_id, RichInfoSegs[i].first.size()-1]. + if (RichInfoSegs[i].second.size) + { + for (int j = occ_start_id - 1; j >= 0; j--) + { + RichInfoSegs[i].second.base_point[j][0] = RichInfoSegs[i].second.base_point[occ_start_id][0]; + RichInfoSegs[i].second.direction[j][0] = RichInfoSegs[i].second.direction[occ_start_id][0]; + } + for (int j = occ_end_id + 1; j < RichInfoSegs[i].second.size; j++) + { + RichInfoSegs[i].second.base_point[j][0] = RichInfoSegs[i].second.base_point[occ_end_id][0]; + RichInfoSegs[i].second.direction[j][0] = RichInfoSegs[i].second.direction[occ_end_id][0]; + } + } + + exit_multi_loop3:; + } + else if (RichInfoSegs[i].first.size == 1) + { + cout << "i=" << i << " RichInfoSegs.size()=" << RichInfoSegs.size() << endl; + cout << "RichInfoSegs[i].first.size=" << RichInfoSegs[i].first.size << endl; + cout << "RichInfoSegs[i].first.direction.size()=" << RichInfoSegs[i].first.direction.size() << endl; + cout << "RichInfoSegs[i].first.direction[0].size()=" << RichInfoSegs[i].first.direction[0].size() << endl; + cout << "RichInfoSegs[i].first.points.cols()=" << RichInfoSegs[i].first.points.cols() << endl; + cout << "RichInfoSegs[i].first.base_point.size()=" << RichInfoSegs[i].first.base_point.size() << endl; + cout << "RichInfoSegs[i].first.base_point[0].size()=" << RichInfoSegs[i].first.base_point[0].size() << endl; + Eigen::Vector3d base_vec_reverse = -RichInfoSegs[i].first.direction[0][0]; + Eigen::Vector3d base_pt_reverse = RichInfoSegs[i].first.points.col(0) + base_vec_reverse * (RichInfoSegs[i].first.base_point[0][0] - RichInfoSegs[i].first.points.col(0)).norm(); + + if (grid_map_->getInflateOccupancy(base_pt_reverse)) // Search outward. + { + double l_upbound = 5 * CTRL_PT_DIST; // "5" is the threshold. + double l = RESOLUTION; + for (; l <= l_upbound; l += RESOLUTION) + { + Eigen::Vector3d base_pt_temp = base_pt_reverse + l * base_vec_reverse; + //cout << base_pt_temp.transpose() << endl; + if (!grid_map_->getInflateOccupancy(base_pt_temp)) + { + RichInfoSegs[i].second.base_point[0][0] = base_pt_temp; + RichInfoSegs[i].second.direction[0][0] = base_vec_reverse; + break; + } + } + if (l > l_upbound) + { + ROS_WARN("Can't find the new base points at the opposite within the threshold, 2. i=%d", i); + + segments.erase(segments.begin() + i); + RichInfoSegs.erase(RichInfoSegs.begin() + i); + seg_upbound--; + i--; + } + } + else if ((base_pt_reverse - RichInfoSegs[i].first.points.col(0)).norm() >= RESOLUTION) // Unnecessary to search. + { + RichInfoSegs[i].second.base_point[0][0] = base_pt_reverse; + RichInfoSegs[i].second.direction[0][0] = base_vec_reverse; + } + else + { + ROS_WARN("base_point and control point are too close!, 2"); + cout << "base_point=" << RichInfoSegs[i].first.base_point[0][0].transpose() << " control point=" << RichInfoSegs[i].first.points.col(0).transpose() << endl; + + segments.erase(segments.begin() + i); + RichInfoSegs.erase(RichInfoSegs.begin() + i); + seg_upbound--; + i--; + } + } + else + { + segments.erase(segments.begin() + i); + RichInfoSegs.erase(RichInfoSegs.begin() + i); + seg_upbound--; + i--; + } + } + // cout << "A3" << endl; + + // Step 2. Assemble each segment to make up the new control point sequence. + if (seg_upbound == 0) // After the erase operation above, segment legth will decrease to 0 again. + { + std::vector oneSeg; + oneSeg.push_back(cps_); + return oneSeg; + } + + std::vector selection(seg_upbound); + std::fill(selection.begin(), selection.end(), 0); + selection[0] = -1; // init + int max_traj_nums = static_cast(pow(VARIS, seg_upbound)); + for (int i = 0; i < max_traj_nums; i++) + { + // 2.1 Calculate the selection table. + int digit_id = 0; + selection[digit_id]++; + while (digit_id < seg_upbound && selection[digit_id] >= VARIS) + { + selection[digit_id] = 0; + digit_id++; + if (digit_id >= seg_upbound) + { + ROS_ERROR("Should not happen!!! digit_id=%d, seg_upbound=%d", digit_id, seg_upbound); + } + selection[digit_id]++; + } + + // 2.2 Assign params according to the selection table. + ControlPoints cpsOneSample; + cpsOneSample.resize(cps_.size); + cpsOneSample.clearance = cps_.clearance; + int cp_id = 0, seg_id = 0, cp_of_seg_id = 0; + while (/*seg_id < RichInfoSegs.size() ||*/ cp_id < cps_.size) + { + //cout << "A "; + // if ( seg_id >= RichInfoSegs.size() ) + // { + // cout << "seg_id=" << seg_id << " RichInfoSegs.size()=" << RichInfoSegs.size() << endl; + // } + // if ( cp_id >= cps_.base_point.size() ) + // { + // cout << "cp_id=" << cp_id << " cps_.base_point.size()=" << cps_.base_point.size() << endl; + // } + // if ( cp_of_seg_id >= RichInfoSegs[seg_id].first.base_point.size() ) + // { + // cout << "cp_of_seg_id=" << cp_of_seg_id << " RichInfoSegs[seg_id].first.base_point.size()=" << RichInfoSegs[seg_id].first.base_point.size() << endl; + // } + + if (seg_id >= seg_upbound || cp_id < segments[seg_id].first || cp_id > segments[seg_id].second) + { + cpsOneSample.points.col(cp_id) = cps_.points.col(cp_id); + cpsOneSample.base_point[cp_id] = cps_.base_point[cp_id]; + cpsOneSample.direction[cp_id] = cps_.direction[cp_id]; + } + else if (cp_id >= segments[seg_id].first && cp_id <= segments[seg_id].second) + { + if (!selection[seg_id]) // zx-todo + { + cpsOneSample.points.col(cp_id) = RichInfoSegs[seg_id].first.points.col(cp_of_seg_id); + cpsOneSample.base_point[cp_id] = RichInfoSegs[seg_id].first.base_point[cp_of_seg_id]; + cpsOneSample.direction[cp_id] = RichInfoSegs[seg_id].first.direction[cp_of_seg_id]; + cp_of_seg_id++; + } + else + { + if (RichInfoSegs[seg_id].second.size) + { + cpsOneSample.points.col(cp_id) = RichInfoSegs[seg_id].second.points.col(cp_of_seg_id); + cpsOneSample.base_point[cp_id] = RichInfoSegs[seg_id].second.base_point[cp_of_seg_id]; + cpsOneSample.direction[cp_id] = RichInfoSegs[seg_id].second.direction[cp_of_seg_id]; + cp_of_seg_id++; + } + else + { + // Abandon this trajectory. + goto abandon_this_trajectory; + } + } + + if (cp_id == segments[seg_id].second) + { + cp_of_seg_id = 0; + seg_id++; + } + } + else + { + ROS_ERROR("Shold not happen!!!!, cp_id=%d, seg_id=%d, segments.front().first=%d, segments.back().second=%d, segments[seg_id].first=%d, segments[seg_id].second=%d", + cp_id, seg_id, segments.front().first, segments.back().second, segments[seg_id].first, segments[seg_id].second); + } + + cp_id++; + } + + control_pts_buf.push_back(cpsOneSample); + + abandon_this_trajectory:; + } + + return control_pts_buf; + } // namespace ego_planner + + /* This function is very similar to check_collision_and_rebound(). + * It was written separately, just because I did it once and it has been running stably since March 2020. + * But I will merge then someday.*/ + std::vector> BsplineOptimizer::initControlPoints(Eigen::MatrixXd &init_points, bool flag_first_init /*= true*/) + { + + if (flag_first_init) + { + cps_.clearance = dist0_; + cps_.resize(init_points.cols()); + cps_.points = init_points; + } + + /*** Segment the initial trajectory according to obstacles ***/ + constexpr int ENOUGH_INTERVAL = 2; + double step_size = grid_map_->getResolution() / ((init_points.col(0) - init_points.rightCols(1)).norm() / (init_points.cols() - 1)) / 1.5; + int in_id = -1, out_id = -1; + vector> segment_ids; + int same_occ_state_times = ENOUGH_INTERVAL + 1; + bool occ, last_occ = false; + bool flag_got_start = false, flag_got_end = false, flag_got_end_maybe = false; + int i_end = (int)init_points.cols() - order_ - ((int)init_points.cols() - 2 * order_) / 3; // only check closed 2/3 points. + for (int i = order_; i <= i_end; ++i) + { + //cout << " *" << i-1 << "*" ; + for (double a = 1.0; a > 0.0; a -= step_size) + { + occ = grid_map_->getInflateOccupancy(a * init_points.col(i - 1) + (1 - a) * init_points.col(i)); + //cout << " " << occ; + // cout << setprecision(5); + // cout << (a * init_points.col(i-1) + (1-a) * init_points.col(i)).transpose() << " occ1=" << occ << endl; + + if (occ && !last_occ) + { + if (same_occ_state_times > ENOUGH_INTERVAL || i == order_) + { + in_id = i - 1; + flag_got_start = true; + } + same_occ_state_times = 0; + flag_got_end_maybe = false; // terminate in advance + } + else if (!occ && last_occ) + { + out_id = i; + flag_got_end_maybe = true; + same_occ_state_times = 0; + } + else + { + ++same_occ_state_times; + } + + if (flag_got_end_maybe && (same_occ_state_times > ENOUGH_INTERVAL || (i == (int)init_points.cols() - order_))) + { + flag_got_end_maybe = false; + flag_got_end = true; + } + + last_occ = occ; + + if (flag_got_start && flag_got_end) + { + flag_got_start = false; + flag_got_end = false; + segment_ids.push_back(std::pair(in_id, out_id)); + } + } + } + // cout << endl; + + // for (size_t i = 0; i < segment_ids.size(); i++) + // { + // cout << "segment_ids=" << segment_ids[i].first << " ~ " << segment_ids[i].second << endl; + // } + + // return in advance + if (segment_ids.size() == 0) + { + vector> blank_ret; + return blank_ret; + } + + /*** a star search ***/ + vector> a_star_pathes; + for (size_t i = 0; i < segment_ids.size(); ++i) + { + //cout << "in=" << in.transpose() << " out=" << out.transpose() << endl; + Eigen::Vector3d in(init_points.col(segment_ids[i].first)), out(init_points.col(segment_ids[i].second)); + if (a_star_->AstarSearch(/*(in-out).norm()/10+0.05*/ 0.1, in, out)) + { + a_star_pathes.push_back(a_star_->getPath()); + } + else + { + ROS_ERROR("a star error, force return!"); + vector> blank_ret; + return blank_ret; + } + } + + /*** calculate bounds ***/ + int id_low_bound, id_up_bound; + vector> bounds(segment_ids.size()); + for (size_t i = 0; i < segment_ids.size(); i++) + { + + if (i == 0) // first segment + { + id_low_bound = order_; + if (segment_ids.size() > 1) + { + id_up_bound = (int)(((segment_ids[0].second + segment_ids[1].first) - 1.0f) / 2); // id_up_bound : -1.0f fix() + } + else + { + id_up_bound = init_points.cols() - order_ - 1; + } + } + else if (i == segment_ids.size() - 1) // last segment, i != 0 here + { + id_low_bound = (int)(((segment_ids[i].first + segment_ids[i - 1].second) + 1.0f) / 2); // id_low_bound : +1.0f ceil() + id_up_bound = init_points.cols() - order_ - 1; + } + else + { + id_low_bound = (int)(((segment_ids[i].first + segment_ids[i - 1].second) + 1.0f) / 2); // id_low_bound : +1.0f ceil() + id_up_bound = (int)(((segment_ids[i].second + segment_ids[i + 1].first) - 1.0f) / 2); // id_up_bound : -1.0f fix() + } + + bounds[i] = std::pair(id_low_bound, id_up_bound); + } + + // cout << "+++++++++" << endl; + // for ( int j=0; j> adjusted_segment_ids(segment_ids.size()); + constexpr double MINIMUM_PERCENT = 0.0; // Each segment is guaranteed to have sufficient points to generate sufficient force + int minimum_points = round(init_points.cols() * MINIMUM_PERCENT), num_points; + for (size_t i = 0; i < segment_ids.size(); i++) + { + /*** Adjust segment length ***/ + num_points = segment_ids[i].second - segment_ids[i].first + 1; + //cout << "i = " << i << " first = " << segment_ids[i].first << " second = " << segment_ids[i].second << endl; + if (num_points < minimum_points) + { + double add_points_each_side = (int)(((minimum_points - num_points) + 1.0f) / 2); + + adjusted_segment_ids[i].first = segment_ids[i].first - add_points_each_side >= bounds[i].first ? segment_ids[i].first - add_points_each_side : bounds[i].first; + + adjusted_segment_ids[i].second = segment_ids[i].second + add_points_each_side <= bounds[i].second ? segment_ids[i].second + add_points_each_side : bounds[i].second; + } + else + { + adjusted_segment_ids[i].first = segment_ids[i].first; + adjusted_segment_ids[i].second = segment_ids[i].second; + } + + //cout << "final:" << "i = " << i << " first = " << adjusted_segment_ids[i].first << " second = " << adjusted_segment_ids[i].second << endl; + } + for (size_t i = 1; i < adjusted_segment_ids.size(); i++) // Avoid overlap + { + if (adjusted_segment_ids[i - 1].second >= adjusted_segment_ids[i].first) + { + double middle = (double)(adjusted_segment_ids[i - 1].second + adjusted_segment_ids[i].first) / 2.0; + adjusted_segment_ids[i - 1].second = static_cast(middle - 0.1); + adjusted_segment_ids[i].first = static_cast(middle + 1.1); + } + } + + // Used for return + vector> final_segment_ids; + + /*** Assign data to each segment ***/ + for (size_t i = 0; i < segment_ids.size(); i++) + { + // step 1 + for (int j = adjusted_segment_ids[i].first; j <= adjusted_segment_ids[i].second; ++j) + cps_.flag_temp[j] = false; + + // step 2 + int got_intersection_id = -1; + for (int j = segment_ids[i].first + 1; j < segment_ids[i].second; ++j) + { + Eigen::Vector3d ctrl_pts_law(init_points.col(j + 1) - init_points.col(j - 1)), intersection_point; + int Astar_id = a_star_pathes[i].size() / 2, last_Astar_id; // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better, but it needs more computation + double val = (a_star_pathes[i][Astar_id] - init_points.col(j)).dot(ctrl_pts_law), last_val = val; + while (Astar_id >= 0 && Astar_id < (int)a_star_pathes[i].size()) + { + last_Astar_id = Astar_id; + + if (val >= 0) + --Astar_id; + else + ++Astar_id; + + val = (a_star_pathes[i][Astar_id] - init_points.col(j)).dot(ctrl_pts_law); + + if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) // val = last_val = 0.0 is not allowed + { + intersection_point = + a_star_pathes[i][Astar_id] + + ((a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id]) * + (ctrl_pts_law.dot(init_points.col(j) - a_star_pathes[i][Astar_id]) / ctrl_pts_law.dot(a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id])) // = t + ); + + //cout << "i=" << i << " j=" << j << " Astar_id=" << Astar_id << " last_Astar_id=" << last_Astar_id << " intersection_point = " << intersection_point.transpose() << endl; + + got_intersection_id = j; + break; + } + } + + if (got_intersection_id >= 0) + { + double length = (intersection_point - init_points.col(j)).norm(); + if (length > 1e-5) + { + cps_.flag_temp[j] = true; + for (double a = length; a >= 0.0; a -= grid_map_->getResolution()) + { + occ = grid_map_->getInflateOccupancy((a / length) * intersection_point + (1 - a / length) * init_points.col(j)); + + if (occ || a < grid_map_->getResolution()) + { + if (occ) + a += grid_map_->getResolution(); + cps_.base_point[j].push_back((a / length) * intersection_point + (1 - a / length) * init_points.col(j)); + cps_.direction[j].push_back((intersection_point - init_points.col(j)).normalized()); + // cout << "A " << j << endl; + break; + } + } + } + else + { + got_intersection_id = -1; + } + } + } + + /* Corner case: the segment length is too short. Here the control points may outside the A* path, leading to opposite gradient direction. So I have to take special care of it */ + if (segment_ids[i].second - segment_ids[i].first == 1) + { + Eigen::Vector3d ctrl_pts_law(init_points.col(segment_ids[i].second) - init_points.col(segment_ids[i].first)), intersection_point; + Eigen::Vector3d middle_point = (init_points.col(segment_ids[i].second) + init_points.col(segment_ids[i].first)) / 2; + int Astar_id = a_star_pathes[i].size() / 2, last_Astar_id; // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better, but it needs more computation + double val = (a_star_pathes[i][Astar_id] - middle_point).dot(ctrl_pts_law), last_val = val; + while (Astar_id >= 0 && Astar_id < (int)a_star_pathes[i].size()) + { + last_Astar_id = Astar_id; + + if (val >= 0) + --Astar_id; + else + ++Astar_id; + + val = (a_star_pathes[i][Astar_id] - middle_point).dot(ctrl_pts_law); + + if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) // val = last_val = 0.0 is not allowed + { + intersection_point = + a_star_pathes[i][Astar_id] + + ((a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id]) * + (ctrl_pts_law.dot(middle_point - a_star_pathes[i][Astar_id]) / ctrl_pts_law.dot(a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id])) // = t + ); + + if ((intersection_point - middle_point).norm() > 0.01) // 1cm. + { + cps_.flag_temp[segment_ids[i].first] = true; + cps_.base_point[segment_ids[i].first].push_back(init_points.col(segment_ids[i].first)); + cps_.direction[segment_ids[i].first].push_back((intersection_point - middle_point).normalized()); + + got_intersection_id = segment_ids[i].first; + } + break; + } + } + } + + //step 3 + if (got_intersection_id >= 0) + { + for (int j = got_intersection_id + 1; j <= adjusted_segment_ids[i].second; ++j) + if (!cps_.flag_temp[j]) + { + cps_.base_point[j].push_back(cps_.base_point[j - 1].back()); + cps_.direction[j].push_back(cps_.direction[j - 1].back()); + // cout << "AAA " << j << endl; + } + + for (int j = got_intersection_id - 1; j >= adjusted_segment_ids[i].first; --j) + if (!cps_.flag_temp[j]) + { + cps_.base_point[j].push_back(cps_.base_point[j + 1].back()); + cps_.direction[j].push_back(cps_.direction[j + 1].back()); + // cout << "AAAA " << j << endl; + } + + final_segment_ids.push_back(adjusted_segment_ids[i]); + } + else + { + // Just ignore, it does not matter ^_^. + // ROS_ERROR("Failed to generate direction! segment_id=%d", i); + } + } + + return final_segment_ids; + } + + int BsplineOptimizer::earlyExit(void *func_data, const double *x, const double *g, const double fx, const double xnorm, const double gnorm, const double step, int n, int k, int ls) + { + BsplineOptimizer *opt = reinterpret_cast(func_data); + // cout << "k=" << k << endl; + // cout << "opt->flag_continue_to_optimize_=" << opt->flag_continue_to_optimize_ << endl; + return (opt->force_stop_type_ == STOP_FOR_ERROR || opt->force_stop_type_ == STOP_FOR_REBOUND); + } + + double BsplineOptimizer::costFunctionRebound(void *func_data, const double *x, double *grad, const int n) + { + BsplineOptimizer *opt = reinterpret_cast(func_data); + + double cost; + opt->combineCostRebound(x, grad, cost, n); + + opt->iter_num_ += 1; + return cost; + } + + double BsplineOptimizer::costFunctionRefine(void *func_data, const double *x, double *grad, const int n) + { + BsplineOptimizer *opt = reinterpret_cast(func_data); + + double cost; + opt->combineCostRefine(x, grad, cost, n); + + opt->iter_num_ += 1; + return cost; + } + + void BsplineOptimizer::calcSwarmCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient) + { + cost = 0.0; + int end_idx = q.cols() - order_ - (double)(q.cols() - 2 * order_) * 1.0 / 3.0; // Only check the first 2/3 points + const double CLEARANCE = swarm_clearance_ * 2; + double t_now = ros::Time::now().toSec(); + constexpr double a = 2.0, b = 1.0, inv_a2 = 1 / a / a, inv_b2 = 1 / b / b; + + for (int i = order_; i < end_idx; i++) + { + double glb_time = t_now + ((double)(order_ - 1) / 2 + (i - order_ + 1)) * bspline_interval_; + + for (size_t id = 0; id < swarm_trajs_->size(); id++) + { + if ((swarm_trajs_->at(id).drone_id != (int)id) || swarm_trajs_->at(id).drone_id == drone_id_) + { + continue; + } + + double traj_i_satrt_time = swarm_trajs_->at(id).start_time_.toSec(); + if (glb_time < traj_i_satrt_time + swarm_trajs_->at(id).duration_ - 0.1) + { + /* def cost=(c-sqrt([Q-O]'D[Q-O]))^2, D=[1/b^2,0,0;0,1/b^2,0;0,0,1/a^2] */ + Eigen::Vector3d swarm_prid = swarm_trajs_->at(id).position_traj_.evaluateDeBoorT(glb_time - traj_i_satrt_time); + Eigen::Vector3d dist_vec = cps_.points.col(i) - swarm_prid; + double ellip_dist = sqrt(dist_vec(2) * dist_vec(2) * inv_a2 + (dist_vec(0) * dist_vec(0) + dist_vec(1) * dist_vec(1)) * inv_b2); + double dist_err = CLEARANCE - ellip_dist; + + Eigen::Vector3d dist_grad = cps_.points.col(i) - swarm_prid; + Eigen::Vector3d Coeff; + Coeff(0) = -2 * (CLEARANCE / ellip_dist - 1) * inv_b2; + Coeff(1) = Coeff(0); + Coeff(2) = -2 * (CLEARANCE / ellip_dist - 1) * inv_a2; + + if (dist_err < 0) + { + /* do nothing */ + } + else + { + cost += pow(dist_err, 2); + gradient.col(i) += (Coeff.array() * dist_grad.array()).matrix(); + } + + if (min_ellip_dist_ > dist_err) + { + min_ellip_dist_ = dist_err; + } + } + } + } + } + + void BsplineOptimizer::calcMovingObjCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient) + { + cost = 0.0; + int end_idx = q.cols() - order_; + constexpr double CLEARANCE = 1.5; + double t_now = ros::Time::now().toSec(); + + for (int i = order_; i < end_idx; i++) + { + double time = ((double)(order_ - 1) / 2 + (i - order_ + 1)) * bspline_interval_; + + for (int id = 0; id < moving_objs_->getObjNums(); id++) + { + Eigen::Vector3d obj_prid = moving_objs_->evaluateConstVel(id, t_now + time); + double dist = (cps_.points.col(i) - obj_prid).norm(); + //cout /*<< "cps_.points.col(i)=" << cps_.points.col(i).transpose()*/ << " moving_objs_=" << obj_prid.transpose() << " dist=" << dist << endl; + double dist_err = CLEARANCE - dist; + Eigen::Vector3d dist_grad = (cps_.points.col(i) - obj_prid).normalized(); + + if (dist_err < 0) + { + /* do nothing */ + } + else + { + cost += pow(dist_err, 2); + gradient.col(i) += -2.0 * dist_err * dist_grad; + } + } + // cout << "time=" << time << " i=" << i << " order_=" << order_ << " end_idx=" << end_idx << endl; + // cout << "--" << endl; + } + // cout << "---------------" << endl; + } + + void BsplineOptimizer::calcDistanceCostRebound(const Eigen::MatrixXd &q, double &cost, + Eigen::MatrixXd &gradient, int iter_num, double smoothness_cost) + { + cost = 0.0; + int end_idx = q.cols() - order_; + double demarcation = cps_.clearance; + double a = 3 * demarcation, b = -3 * pow(demarcation, 2), c = pow(demarcation, 3); + + force_stop_type_ = DONT_STOP; + if (iter_num > 3 && smoothness_cost / (cps_.size - 2 * order_) < 0.1) // 0.1 is an experimental value that indicates the trajectory is smooth enough. + { + check_collision_and_rebound(); + } + + /*** calculate distance cost and gradient ***/ + for (auto i = order_; i < end_idx; ++i) + { + for (size_t j = 0; j < cps_.direction[i].size(); ++j) + { + double dist = (cps_.points.col(i) - cps_.base_point[i][j]).dot(cps_.direction[i][j]); + double dist_err = cps_.clearance - dist; + Eigen::Vector3d dist_grad = cps_.direction[i][j]; + + if (dist_err < 0) + { + /* do nothing */ + } + else if (dist_err < demarcation) + { + cost += pow(dist_err, 3); + gradient.col(i) += -3.0 * dist_err * dist_err * dist_grad; + } + else + { + cost += a * dist_err * dist_err + b * dist_err + c; + gradient.col(i) += -(2.0 * a * dist_err + b) * dist_grad; + } + } + } + } + + void BsplineOptimizer::calcFitnessCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient) + { + + cost = 0.0; + + int end_idx = q.cols() - order_; + + // def: f = |x*v|^2/a^2 + |x×v|^2/b^2 + double a2 = 25, b2 = 1; + for (auto i = order_ - 1; i < end_idx + 1; ++i) + { + Eigen::Vector3d x = (q.col(i - 1) + 4 * q.col(i) + q.col(i + 1)) / 6.0 - ref_pts_[i - 1]; + Eigen::Vector3d v = (ref_pts_[i] - ref_pts_[i - 2]).normalized(); + + double xdotv = x.dot(v); + Eigen::Vector3d xcrossv = x.cross(v); + + double f = pow((xdotv), 2) / a2 + pow(xcrossv.norm(), 2) / b2; + cost += f; + + Eigen::Matrix3d m; + m << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0; + Eigen::Vector3d df_dx = 2 * xdotv / a2 * v + 2 / b2 * m * xcrossv; + + gradient.col(i - 1) += df_dx / 6; + gradient.col(i) += 4 * df_dx / 6; + gradient.col(i + 1) += df_dx / 6; + } + } + + void BsplineOptimizer::calcSmoothnessCost(const Eigen::MatrixXd &q, double &cost, + Eigen::MatrixXd &gradient, bool falg_use_jerk /* = true*/) + { + + cost = 0.0; + + if (falg_use_jerk) + { + Eigen::Vector3d jerk, temp_j; + + for (int i = 0; i < q.cols() - 3; i++) + { + /* evaluate jerk */ + jerk = q.col(i + 3) - 3 * q.col(i + 2) + 3 * q.col(i + 1) - q.col(i); + cost += jerk.squaredNorm(); + temp_j = 2.0 * jerk; + /* jerk gradient */ + gradient.col(i + 0) += -temp_j; + gradient.col(i + 1) += 3.0 * temp_j; + gradient.col(i + 2) += -3.0 * temp_j; + gradient.col(i + 3) += temp_j; + } + } + else + { + Eigen::Vector3d acc, temp_acc; + + for (int i = 0; i < q.cols() - 2; i++) + { + /* evaluate acc */ + acc = q.col(i + 2) - 2 * q.col(i + 1) + q.col(i); + cost += acc.squaredNorm(); + temp_acc = 2.0 * acc; + /* acc gradient */ + gradient.col(i + 0) += temp_acc; + gradient.col(i + 1) += -2.0 * temp_acc; + gradient.col(i + 2) += temp_acc; + } + } + } + + void BsplineOptimizer::calcTerminalCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient) + { + cost = 0.0; + + // zero cost and gradient in hard constraints + Eigen::Vector3d q_3, q_2, q_1, dq; + q_3 = q.col(q.cols() - 3); + q_2 = q.col(q.cols() - 2); + q_1 = q.col(q.cols() - 1); + + dq = 1 / 6.0 * (q_3 + 4 * q_2 + q_1) - local_target_pt_; + cost += dq.squaredNorm(); + + gradient.col(q.cols() - 3) += 2 * dq * (1 / 6.0); + gradient.col(q.cols() - 2) += 2 * dq * (4 / 6.0); + gradient.col(q.cols() - 1) += 2 * dq * (1 / 6.0); + } + + void BsplineOptimizer::calcFeasibilityCost(const Eigen::MatrixXd &q, double &cost, + Eigen::MatrixXd &gradient) + { + + //#define SECOND_DERIVATIVE_CONTINOUS + +#ifdef SECOND_DERIVATIVE_CONTINOUS + + cost = 0.0; + double demarcation = 1.0; // 1m/s, 1m/s/s + double ar = 3 * demarcation, br = -3 * pow(demarcation, 2), cr = pow(demarcation, 3); + double al = ar, bl = -br, cl = cr; + + /* abbreviation */ + double ts, ts_inv2, ts_inv3; + ts = bspline_interval_; + ts_inv2 = 1 / ts / ts; + ts_inv3 = 1 / ts / ts / ts; + + /* velocity feasibility */ + for (int i = 0; i < q.cols() - 1; i++) + { + Eigen::Vector3d vi = (q.col(i + 1) - q.col(i)) / ts; + + for (int j = 0; j < 3; j++) + { + if (vi(j) > max_vel_ + demarcation) + { + double diff = vi(j) - max_vel_; + cost += (ar * diff * diff + br * diff + cr) * ts_inv3; // multiply ts_inv3 to make vel and acc has similar magnitude + + double grad = (2.0 * ar * diff + br) / ts * ts_inv3; + gradient(j, i + 0) += -grad; + gradient(j, i + 1) += grad; + } + else if (vi(j) > max_vel_) + { + double diff = vi(j) - max_vel_; + cost += pow(diff, 3) * ts_inv3; + ; + + double grad = 3 * diff * diff / ts * ts_inv3; + ; + gradient(j, i + 0) += -grad; + gradient(j, i + 1) += grad; + } + else if (vi(j) < -(max_vel_ + demarcation)) + { + double diff = vi(j) + max_vel_; + cost += (al * diff * diff + bl * diff + cl) * ts_inv3; + + double grad = (2.0 * al * diff + bl) / ts * ts_inv3; + gradient(j, i + 0) += -grad; + gradient(j, i + 1) += grad; + } + else if (vi(j) < -max_vel_) + { + double diff = vi(j) + max_vel_; + cost += -pow(diff, 3) * ts_inv3; + + double grad = -3 * diff * diff / ts * ts_inv3; + gradient(j, i + 0) += -grad; + gradient(j, i + 1) += grad; + } + else + { + /* nothing happened */ + } + } + } + + /* acceleration feasibility */ + for (int i = 0; i < q.cols() - 2; i++) + { + Eigen::Vector3d ai = (q.col(i + 2) - 2 * q.col(i + 1) + q.col(i)) * ts_inv2; + + for (int j = 0; j < 3; j++) + { + if (ai(j) > max_acc_ + demarcation) + { + double diff = ai(j) - max_acc_; + cost += ar * diff * diff + br * diff + cr; + + double grad = (2.0 * ar * diff + br) * ts_inv2; + gradient(j, i + 0) += grad; + gradient(j, i + 1) += -2 * grad; + gradient(j, i + 2) += grad; + } + else if (ai(j) > max_acc_) + { + double diff = ai(j) - max_acc_; + cost += pow(diff, 3); + + double grad = 3 * diff * diff * ts_inv2; + gradient(j, i + 0) += grad; + gradient(j, i + 1) += -2 * grad; + gradient(j, i + 2) += grad; + } + else if (ai(j) < -(max_acc_ + demarcation)) + { + double diff = ai(j) + max_acc_; + cost += al * diff * diff + bl * diff + cl; + + double grad = (2.0 * al * diff + bl) * ts_inv2; + gradient(j, i + 0) += grad; + gradient(j, i + 1) += -2 * grad; + gradient(j, i + 2) += grad; + } + else if (ai(j) < -max_acc_) + { + double diff = ai(j) + max_acc_; + cost += -pow(diff, 3); + + double grad = -3 * diff * diff * ts_inv2; + gradient(j, i + 0) += grad; + gradient(j, i + 1) += -2 * grad; + gradient(j, i + 2) += grad; + } + else + { + /* nothing happened */ + } + } + } + +#else + + cost = 0.0; + /* abbreviation */ + double ts, /*vm2, am2, */ ts_inv2; + // vm2 = max_vel_ * max_vel_; + // am2 = max_acc_ * max_acc_; + + ts = bspline_interval_; + ts_inv2 = 1 / ts / ts; + + /* velocity feasibility */ + for (int i = 0; i < q.cols() - 1; i++) + { + Eigen::Vector3d vi = (q.col(i + 1) - q.col(i)) / ts; + + //cout << "temp_v * vi=" ; + for (int j = 0; j < 3; j++) + { + if (vi(j) > max_vel_) + { + // cout << "zx-todo VEL" << endl; + // cout << vi(j) << endl; + cost += pow(vi(j) - max_vel_, 2) * ts_inv2; // multiply ts_inv3 to make vel and acc has similar magnitude + + gradient(j, i + 0) += -2 * (vi(j) - max_vel_) / ts * ts_inv2; + gradient(j, i + 1) += 2 * (vi(j) - max_vel_) / ts * ts_inv2; + } + else if (vi(j) < -max_vel_) + { + cost += pow(vi(j) + max_vel_, 2) * ts_inv2; + + gradient(j, i + 0) += -2 * (vi(j) + max_vel_) / ts * ts_inv2; + gradient(j, i + 1) += 2 * (vi(j) + max_vel_) / ts * ts_inv2; + } + else + { + /* code */ + } + } + } + + /* acceleration feasibility */ + for (int i = 0; i < q.cols() - 2; i++) + { + Eigen::Vector3d ai = (q.col(i + 2) - 2 * q.col(i + 1) + q.col(i)) * ts_inv2; + + //cout << "temp_a * ai=" ; + for (int j = 0; j < 3; j++) + { + if (ai(j) > max_acc_) + { + // cout << "zx-todo ACC" << endl; + // cout << ai(j) << endl; + cost += pow(ai(j) - max_acc_, 2); + + gradient(j, i + 0) += 2 * (ai(j) - max_acc_) * ts_inv2; + gradient(j, i + 1) += -4 * (ai(j) - max_acc_) * ts_inv2; + gradient(j, i + 2) += 2 * (ai(j) - max_acc_) * ts_inv2; + } + else if (ai(j) < -max_acc_) + { + cost += pow(ai(j) + max_acc_, 2); + + gradient(j, i + 0) += 2 * (ai(j) + max_acc_) * ts_inv2; + gradient(j, i + 1) += -4 * (ai(j) + max_acc_) * ts_inv2; + gradient(j, i + 2) += 2 * (ai(j) + max_acc_) * ts_inv2; + } + else + { + /* code */ + } + } + //cout << endl; + } + +#endif + } + + bool BsplineOptimizer::check_collision_and_rebound(void) + { + + int end_idx = cps_.size - order_; + + /*** Check and segment the initial trajectory according to obstacles ***/ + int in_id, out_id; + vector> segment_ids; + bool flag_new_obs_valid = false; + int i_end = end_idx - (end_idx - order_) / 3; + for (int i = order_ - 1; i <= i_end; ++i) + { + + bool occ = grid_map_->getInflateOccupancy(cps_.points.col(i)); + + /*** check if the new collision will be valid ***/ + if (occ) + { + for (size_t k = 0; k < cps_.direction[i].size(); ++k) + { + cout.precision(2); + if ((cps_.points.col(i) - cps_.base_point[i][k]).dot(cps_.direction[i][k]) < 1 * grid_map_->getResolution()) // current point is outside all the collision_points. + { + occ = false; // Not really takes effect, just for better hunman understanding. + break; + } + } + } + + if (occ) + { + flag_new_obs_valid = true; + + int j; + for (j = i - 1; j >= 0; --j) + { + occ = grid_map_->getInflateOccupancy(cps_.points.col(j)); + if (!occ) + { + in_id = j; + break; + } + } + if (j < 0) // fail to get the obs free point + { + ROS_ERROR("ERROR! the drone is in obstacle. This should not happen."); + in_id = 0; + } + + for (j = i + 1; j < cps_.size; ++j) + { + occ = grid_map_->getInflateOccupancy(cps_.points.col(j)); + + if (!occ) + { + out_id = j; + break; + } + } + if (j >= cps_.size) // fail to get the obs free point + { + ROS_WARN("WARN! terminal point of the current trajectory is in obstacle, skip this planning."); + + force_stop_type_ = STOP_FOR_ERROR; + return false; + } + + i = j + 1; + + segment_ids.push_back(std::pair(in_id, out_id)); + } + } + + if (flag_new_obs_valid) + { + vector> a_star_pathes; + for (size_t i = 0; i < segment_ids.size(); ++i) + { + /*** a star search ***/ + Eigen::Vector3d in(cps_.points.col(segment_ids[i].first)), out(cps_.points.col(segment_ids[i].second)); + if (a_star_->AstarSearch(/*(in-out).norm()/10+0.05*/ 0.1, in, out)) + { + a_star_pathes.push_back(a_star_->getPath()); + } + else + { + ROS_ERROR("a star error"); + segment_ids.erase(segment_ids.begin() + i); + i--; + } + } + + for (size_t i = 1; i < segment_ids.size(); i++) // Avoid overlap + { + if (segment_ids[i - 1].second >= segment_ids[i].first) + { + double middle = (double)(segment_ids[i - 1].second + segment_ids[i].first) / 2.0; + segment_ids[i - 1].second = static_cast(middle - 0.1); + segment_ids[i].first = static_cast(middle + 1.1); + } + } + + /*** Assign parameters to each segment ***/ + for (size_t i = 0; i < segment_ids.size(); ++i) + { + // step 1 + for (int j = segment_ids[i].first; j <= segment_ids[i].second; ++j) + cps_.flag_temp[j] = false; + + // step 2 + int got_intersection_id = -1; + for (int j = segment_ids[i].first + 1; j < segment_ids[i].second; ++j) + { + Eigen::Vector3d ctrl_pts_law(cps_.points.col(j + 1) - cps_.points.col(j - 1)), intersection_point; + int Astar_id = a_star_pathes[i].size() / 2, last_Astar_id; // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better, but it needs more computation + double val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law), last_val = val; + while (Astar_id >= 0 && Astar_id < (int)a_star_pathes[i].size()) + { + last_Astar_id = Astar_id; + + if (val >= 0) + --Astar_id; + else + ++Astar_id; + + val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law); + + // cout << val << endl; + + if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) // val = last_val = 0.0 is not allowed + { + intersection_point = + a_star_pathes[i][Astar_id] + + ((a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id]) * + (ctrl_pts_law.dot(cps_.points.col(j) - a_star_pathes[i][Astar_id]) / ctrl_pts_law.dot(a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id])) // = t + ); + + got_intersection_id = j; + break; + } + } + + if (got_intersection_id >= 0) + { + double length = (intersection_point - cps_.points.col(j)).norm(); + if (length > 1e-5) + { + cps_.flag_temp[j] = true; + for (double a = length; a >= 0.0; a -= grid_map_->getResolution()) + { + bool occ = grid_map_->getInflateOccupancy((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j)); + + if (occ || a < grid_map_->getResolution()) + { + if (occ) + a += grid_map_->getResolution(); + cps_.base_point[j].push_back((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j)); + cps_.direction[j].push_back((intersection_point - cps_.points.col(j)).normalized()); + break; + } + } + } + else + { + got_intersection_id = -1; + } + } + } + + //step 3 + if (got_intersection_id >= 0) + { + for (int j = got_intersection_id + 1; j <= segment_ids[i].second; ++j) + if (!cps_.flag_temp[j]) + { + cps_.base_point[j].push_back(cps_.base_point[j - 1].back()); + cps_.direction[j].push_back(cps_.direction[j - 1].back()); + } + + for (int j = got_intersection_id - 1; j >= segment_ids[i].first; --j) + if (!cps_.flag_temp[j]) + { + cps_.base_point[j].push_back(cps_.base_point[j + 1].back()); + cps_.direction[j].push_back(cps_.direction[j + 1].back()); + } + } + else + ROS_WARN("Failed to generate direction. It doesn't matter."); + } + + force_stop_type_ = STOP_FOR_REBOUND; + return true; + } + + return false; + } + + bool BsplineOptimizer::BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double ts) + { + setBsplineInterval(ts); + + double final_cost; + bool flag_success = rebound_optimize(final_cost); + + optimal_points = cps_.points; + + return flag_success; + } + + bool BsplineOptimizer::BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double &final_cost, const ControlPoints &control_points, double ts) + { + setBsplineInterval(ts); + + cps_ = control_points; + + bool flag_success = rebound_optimize(final_cost); + + optimal_points = cps_.points; + + return flag_success; + } + + bool BsplineOptimizer::BsplineOptimizeTrajRefine(const Eigen::MatrixXd &init_points, const double ts, Eigen::MatrixXd &optimal_points) + { + + setControlPoints(init_points); + setBsplineInterval(ts); + + bool flag_success = refine_optimize(); + + optimal_points = cps_.points; + + return flag_success; + } + + bool BsplineOptimizer::rebound_optimize(double &final_cost) + { + iter_num_ = 0; + int start_id = order_; + // int end_id = this->cps_.size - order_; //Fixed end + int end_id = this->cps_.size; // Free end + variable_num_ = 3 * (end_id - start_id); + + ros::Time t0 = ros::Time::now(), t1, t2; + int restart_nums = 0, rebound_times = 0; + ; + bool flag_force_return, flag_occ, success; + new_lambda2_ = lambda2_; + constexpr int MAX_RESART_NUMS_SET = 3; + do + { + /* ---------- prepare ---------- */ + min_cost_ = std::numeric_limits::max(); + min_ellip_dist_ = INIT_min_ellip_dist_; + iter_num_ = 0; + flag_force_return = false; + flag_occ = false; + success = false; + + double q[variable_num_]; + memcpy(q, cps_.points.data() + 3 * start_id, variable_num_ * sizeof(q[0])); + + lbfgs::lbfgs_parameter_t lbfgs_params; + lbfgs::lbfgs_load_default_parameters(&lbfgs_params); + lbfgs_params.mem_size = 16; + lbfgs_params.max_iterations = 200; + lbfgs_params.g_epsilon = 0.01; + + /* ---------- optimize ---------- */ + t1 = ros::Time::now(); + int result = lbfgs::lbfgs_optimize(variable_num_, q, &final_cost, BsplineOptimizer::costFunctionRebound, NULL, BsplineOptimizer::earlyExit, this, &lbfgs_params); + t2 = ros::Time::now(); + double time_ms = (t2 - t1).toSec() * 1000; + double total_time_ms = (t2 - t0).toSec() * 1000; + + /* ---------- success temporary, check collision again ---------- */ + if (result == lbfgs::LBFGS_CONVERGENCE || + result == lbfgs::LBFGSERR_MAXIMUMITERATION || + result == lbfgs::LBFGS_ALREADY_MINIMIZED || + result == lbfgs::LBFGS_STOP) + { + //ROS_WARN("Solver error in planning!, return = %s", lbfgs::lbfgs_strerror(result)); + flag_force_return = false; + + /*** collision check, phase 1 ***/ + if ((min_ellip_dist_ != INIT_min_ellip_dist_) && (min_ellip_dist_ > swarm_clearance_)) + { + success = false; + restart_nums++; + initControlPoints(cps_.points, false); + new_lambda2_ *= 2; + + printf("\033[32miter(+1)=%d,time(ms)=%5.3f, swarm too close, keep optimizing\n\033[0m", iter_num_, time_ms); + + continue; + } + + /*** collision check, phase 2 ***/ + UniformBspline traj = UniformBspline(cps_.points, 3, bspline_interval_); + double tm, tmp; + traj.getTimeSpan(tm, tmp); + double t_step = (tmp - tm) / ((traj.evaluateDeBoorT(tmp) - traj.evaluateDeBoorT(tm)).norm() / grid_map_->getResolution()); + for (double t = tm; t < tmp * 2 / 3; t += t_step) // Only check the closest 2/3 partition of the whole trajectory. + { + flag_occ = grid_map_->getInflateOccupancy(traj.evaluateDeBoorT(t)); + if (flag_occ) + { + //cout << "hit_obs, t=" << t << " P=" << traj.evaluateDeBoorT(t).transpose() << endl; + + if (t <= bspline_interval_) // First 3 control points in obstacles! + { + // cout << cps_.points.col(1).transpose() << "\n" + // << cps_.points.col(2).transpose() << "\n" + // << cps_.points.col(3).transpose() << "\n" + // << cps_.points.col(4).transpose() << endl; + ROS_WARN("First 3 control points in obstacles! return false, t=%f", t); + return false; + } + + break; + } + } + + // cout << "XXXXXX" << ((cps_.points.col(cps_.points.cols()-1) + 4*cps_.points.col(cps_.points.cols()-2) + cps_.points.col(cps_.points.cols()-3))/6 - local_target_pt_).norm() << endl; + + /*** collision check, phase 3 ***/ +//#define USE_SECOND_CLEARENCE_CHECK +#ifdef USE_SECOND_CLEARENCE_CHECK + bool flag_cls_xyp, flag_cls_xyn, flag_cls_zp, flag_cls_zn; + Eigen::Vector3d start_end_vec = traj.evaluateDeBoorT(tmp) - traj.evaluateDeBoorT(tm); + Eigen::Vector3d offset_xy(-start_end_vec(0), start_end_vec(1), 0); + offset_xy.normalize(); + Eigen::Vector3d offset_z = start_end_vec.cross(offset_xy); + offset_z.normalize(); + offset_xy *= cps_.clearance / 2; + offset_z *= cps_.clearance / 2; + + Eigen::MatrixXd check_pts(cps_.points.rows(), cps_.points.cols()); + for (Eigen::Index i = 0; i < cps_.points.cols(); i++) + { + check_pts.col(i) = cps_.points.col(i); + check_pts(0, i) += offset_xy(0); + check_pts(1, i) += offset_xy(1); + check_pts(2, i) += offset_xy(2); + } + flag_cls_xyp = initControlPoints(check_pts, false).size() > 0; + for (Eigen::Index i = 0; i < cps_.points.cols(); i++) + { + check_pts(0, i) -= 2 * offset_xy(0); + check_pts(1, i) -= 2 * offset_xy(1); + check_pts(2, i) -= 2 * offset_xy(2); + } + flag_cls_xyn = initControlPoints(check_pts, false).size() > 0; + for (Eigen::Index i = 0; i < cps_.points.cols(); i++) + { + check_pts(0, i) += offset_xy(0) + offset_z(0); + check_pts(1, i) += offset_xy(1) + offset_z(1); + check_pts(2, i) += offset_xy(2) + offset_z(2); + } + flag_cls_zp = initControlPoints(check_pts, false).size() > 0; + for (Eigen::Index i = 0; i < cps_.points.cols(); i++) + { + check_pts(0, i) -= 2 * offset_z(0); + check_pts(1, i) -= 2 * offset_z(1); + check_pts(2, i) -= 2 * offset_z(2); + } + flag_cls_zn = initControlPoints(check_pts, false).size() > 0; + if ((flag_cls_xyp ^ flag_cls_xyn) || (flag_cls_zp ^ flag_cls_zn)) + flag_occ = true; +#endif + + if (!flag_occ) + { + // comment + // printf("\033[32miter(+1)=%d,time(ms)=%5.3f,total_t(ms)=%5.3f,cost=%5.3f\n\033[0m", iter_num_, time_ms, total_time_ms, final_cost); + success = true; + } + else // restart + { + restart_nums++; + initControlPoints(cps_.points, false); + new_lambda2_ *= 2; + // comment + // printf("\033[32miter(+1)=%d,time(ms)=%5.3f, collided, keep optimizing\n\033[0m", iter_num_, time_ms); + } + } + else if (result == lbfgs::LBFGSERR_CANCELED) + { + flag_force_return = true; + rebound_times++; + // comment + // cout << "iter=" << iter_num_ << ",time(ms)=" << time_ms << ",rebound." << endl; + } + else + { + ROS_WARN("Solver error. Return = %d, %s. Skip this planning.", result, lbfgs::lbfgs_strerror(result)); + // while (ros::ok()); + } + + } while ( + ((flag_occ || ((min_ellip_dist_ != INIT_min_ellip_dist_) && (min_ellip_dist_ > swarm_clearance_))) && restart_nums < MAX_RESART_NUMS_SET) || + (flag_force_return && force_stop_type_ == STOP_FOR_REBOUND && rebound_times <= 20)); + + return success; + } + + bool BsplineOptimizer::refine_optimize() + { + iter_num_ = 0; + int start_id = order_; + int end_id = this->cps_.points.cols() - order_; + variable_num_ = 3 * (end_id - start_id); + + double q[variable_num_]; + double final_cost; + + memcpy(q, cps_.points.data() + 3 * start_id, variable_num_ * sizeof(q[0])); + + double origin_lambda4 = lambda4_; + bool flag_safe = true; + int iter_count = 0; + do + { + lbfgs::lbfgs_parameter_t lbfgs_params; + lbfgs::lbfgs_load_default_parameters(&lbfgs_params); + lbfgs_params.mem_size = 16; + lbfgs_params.max_iterations = 200; + lbfgs_params.g_epsilon = 0.001; + + int result = lbfgs::lbfgs_optimize(variable_num_, q, &final_cost, BsplineOptimizer::costFunctionRefine, NULL, NULL, this, &lbfgs_params); + if (result == lbfgs::LBFGS_CONVERGENCE || + result == lbfgs::LBFGSERR_MAXIMUMITERATION || + result == lbfgs::LBFGS_ALREADY_MINIMIZED || + result == lbfgs::LBFGS_STOP) + { + //pass + } + else + { + ROS_ERROR("Solver error in refining!, return = %d, %s", result, lbfgs::lbfgs_strerror(result)); + } + + UniformBspline traj = UniformBspline(cps_.points, 3, bspline_interval_); + double tm, tmp; + traj.getTimeSpan(tm, tmp); + double t_step = (tmp - tm) / ((traj.evaluateDeBoorT(tmp) - traj.evaluateDeBoorT(tm)).norm() / grid_map_->getResolution()); // Step size is defined as the maximum size that can passes throgth every gird. + for (double t = tm; t < tmp * 2 / 3; t += t_step) + { + if (grid_map_->getInflateOccupancy(traj.evaluateDeBoorT(t))) + { + // cout << "Refined traj hit_obs, t=" << t << " P=" << traj.evaluateDeBoorT(t).transpose() << endl; + + Eigen::MatrixXd ref_pts(ref_pts_.size(), 3); + for (size_t i = 0; i < ref_pts_.size(); i++) + { + ref_pts.row(i) = ref_pts_[i].transpose(); + } + + flag_safe = false; + break; + } + } + + if (!flag_safe) + lambda4_ *= 2; + + iter_count++; + } while (!flag_safe && iter_count <= 0); + + lambda4_ = origin_lambda4; + + //cout << "iter_num_=" << iter_num_ << endl; + + return flag_safe; + } + + void BsplineOptimizer::combineCostRebound(const double *x, double *grad, double &f_combine, const int n) + { + // cout << "drone_id_=" << drone_id_ << endl; + // cout << "cps_.points.size()=" << cps_.points.size() << endl; + // cout << "n=" << n << endl; + // cout << "sizeof(x[0])=" << sizeof(x[0]) << endl; + + memcpy(cps_.points.data() + 3 * order_, x, n * sizeof(x[0])); + + /* ---------- evaluate cost and gradient ---------- */ + double f_smoothness, f_distance, f_feasibility /*, f_mov_objs*/, f_swarm, f_terminal; + + Eigen::MatrixXd g_smoothness = Eigen::MatrixXd::Zero(3, cps_.size); + Eigen::MatrixXd g_distance = Eigen::MatrixXd::Zero(3, cps_.size); + Eigen::MatrixXd g_feasibility = Eigen::MatrixXd::Zero(3, cps_.size); + // Eigen::MatrixXd g_mov_objs = Eigen::MatrixXd::Zero(3, cps_.size); + Eigen::MatrixXd g_swarm = Eigen::MatrixXd::Zero(3, cps_.size); + Eigen::MatrixXd g_terminal = Eigen::MatrixXd::Zero(3, cps_.size); + + calcSmoothnessCost(cps_.points, f_smoothness, g_smoothness); + calcDistanceCostRebound(cps_.points, f_distance, g_distance, iter_num_, f_smoothness); + calcFeasibilityCost(cps_.points, f_feasibility, g_feasibility); + // calcMovingObjCost(cps_.points, f_mov_objs, g_mov_objs); + calcSwarmCost(cps_.points, f_swarm, g_swarm); + calcTerminalCost(cps_.points, f_terminal, g_terminal); + + f_combine = lambda1_ * f_smoothness + new_lambda2_ * f_distance + lambda3_ * f_feasibility + new_lambda2_ * f_swarm + lambda2_ * f_terminal; + //f_combine = lambda1_ * f_smoothness + new_lambda2_ * f_distance + lambda3_ * f_feasibility + new_lambda2_ * f_mov_objs; + //printf("origin %f %f %f %f\n", f_smoothness, f_distance, f_feasibility, f_combine); + + Eigen::MatrixXd grad_3D = lambda1_ * g_smoothness + new_lambda2_ * g_distance + lambda3_ * g_feasibility + new_lambda2_ * g_swarm + lambda2_ * g_terminal; + //Eigen::MatrixXd grad_3D = lambda1_ * g_smoothness + new_lambda2_ * g_distance + lambda3_ * g_feasibility + new_lambda2_ * g_mov_objs; + memcpy(grad, grad_3D.data() + 3 * order_, n * sizeof(grad[0])); + } + + void BsplineOptimizer::combineCostRefine(const double *x, double *grad, double &f_combine, const int n) + { + + memcpy(cps_.points.data() + 3 * order_, x, n * sizeof(x[0])); + + /* ---------- evaluate cost and gradient ---------- */ + double f_smoothness, f_fitness, f_feasibility; + + Eigen::MatrixXd g_smoothness = Eigen::MatrixXd::Zero(3, cps_.points.cols()); + Eigen::MatrixXd g_fitness = Eigen::MatrixXd::Zero(3, cps_.points.cols()); + Eigen::MatrixXd g_feasibility = Eigen::MatrixXd::Zero(3, cps_.points.cols()); + + //time_satrt = ros::Time::now(); + + calcSmoothnessCost(cps_.points, f_smoothness, g_smoothness); + calcFitnessCost(cps_.points, f_fitness, g_fitness); + calcFeasibilityCost(cps_.points, f_feasibility, g_feasibility); + + /* ---------- convert to solver format...---------- */ + f_combine = lambda1_ * f_smoothness + lambda4_ * f_fitness + lambda3_ * f_feasibility; + // printf("origin %f %f %f %f\n", f_smoothness, f_fitness, f_feasibility, f_combine); + + Eigen::MatrixXd grad_3D = lambda1_ * g_smoothness + lambda4_ * g_fitness + lambda3_ * g_feasibility; + memcpy(grad, grad_3D.data() + 3 * order_, n * sizeof(grad[0])); + } + +} // namespace ego_planner \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/src/gradient_descent_optimizer.cpp b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/src/gradient_descent_optimizer.cpp new file mode 100755 index 00000000..c3371d6c --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/src/gradient_descent_optimizer.cpp @@ -0,0 +1,94 @@ +#include + +#define RESET "\033[0m" +#define RED "\033[31m" + +GradientDescentOptimizer::RESULT +GradientDescentOptimizer::optimize(Eigen::VectorXd &x_init_optimal, double &opt_f) +{ + if (min_grad_ < 1e-10) + { + cout << RED << "min_grad_ is invalid:" << min_grad_ << RESET << endl; + return FAILED; + } + if (iter_limit_ <= 2) + { + cout << RED << "iter_limit_ is invalid:" << iter_limit_ << RESET << endl; + return FAILED; + } + + void *f_data = f_data_; + int iter = 2; + int invoke_count = 2; + bool force_return; + Eigen::VectorXd x_k(x_init_optimal), x_kp1(x_init_optimal.rows()); + double cost_k, cost_kp1, cost_min; + Eigen::VectorXd grad_k(x_init_optimal.rows()), grad_kp1(x_init_optimal.rows()); + + cost_k = objfun_(x_k, grad_k, force_return, f_data); + if (force_return) + return RETURN_BY_ORDER; + cost_min = cost_k; + double max_grad = max(abs(grad_k.maxCoeff()), abs(grad_k.minCoeff())); + constexpr double MAX_MOVEMENT_AT_FIRST_ITERATION = 0.1; // meter + double alpha0 = max_grad < MAX_MOVEMENT_AT_FIRST_ITERATION ? 1.0 : (MAX_MOVEMENT_AT_FIRST_ITERATION / max_grad); + x_kp1 = x_k - alpha0 * grad_k; + cost_kp1 = objfun_(x_kp1, grad_kp1, force_return, f_data); + if (force_return) + return RETURN_BY_ORDER; + if (cost_min > cost_kp1) + cost_min = cost_kp1; + + /*** start iteration ***/ + while (++iter <= iter_limit_ && invoke_count <= invoke_limit_) + { + Eigen::VectorXd s = x_kp1 - x_k; + Eigen::VectorXd y = grad_kp1 - grad_k; + double alpha = s.dot(y) / y.dot(y); + if (isnan(alpha) || isinf(alpha)) + { + cout << RED << "step size invalid! alpha=" << alpha << RESET << endl; + return FAILED; + } + + if (iter % 2) // to aviod copying operations + { + do + { + x_k = x_kp1 - alpha * grad_kp1; + cost_k = objfun_(x_k, grad_k, force_return, f_data); + invoke_count++; + if (force_return) + return RETURN_BY_ORDER; + alpha *= 0.5; + } while (cost_k > cost_kp1 - 1e-4 * alpha * grad_kp1.transpose() * grad_kp1); // Armijo condition + + if (grad_k.norm() < min_grad_) + { + opt_f = cost_k; + return FIND_MIN; + } + } + else + { + do + { + x_kp1 = x_k - alpha * grad_k; + cost_kp1 = objfun_(x_kp1, grad_kp1, force_return, f_data); + invoke_count++; + if (force_return) + return RETURN_BY_ORDER; + alpha *= 0.5; + } while (cost_kp1 > cost_k - 1e-4 * alpha * grad_k.transpose() * grad_k); // Armijo condition + + if (grad_kp1.norm() < min_grad_) + { + opt_f = cost_kp1; + return FIND_MIN; + } + } + } + + opt_f = iter_limit_ % 2 ? cost_k : cost_kp1; + return REACH_MAX_ITERATION; +} diff --git a/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/src/uniform_bspline.cpp b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/src/uniform_bspline.cpp new file mode 100644 index 00000000..ff798f01 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/bspline_opt/src/uniform_bspline.cpp @@ -0,0 +1,377 @@ +#include "bspline_opt/uniform_bspline.h" +#include + +namespace ego_planner +{ + + UniformBspline::UniformBspline(const Eigen::MatrixXd &points, const int &order, + const double &interval) + { + setUniformBspline(points, order, interval); + } + + UniformBspline::~UniformBspline() {} + + void UniformBspline::setUniformBspline(const Eigen::MatrixXd &points, const int &order, + const double &interval) + { + control_points_ = points; + p_ = order; + interval_ = interval; + + n_ = points.cols() - 1; + m_ = n_ + p_ + 1; + + u_ = Eigen::VectorXd::Zero(m_ + 1); + for (int i = 0; i <= m_; ++i) + { + + if (i <= p_) + { + u_(i) = double(-p_ + i) * interval_; + } + else if (i > p_ && i <= m_ - p_) + { + u_(i) = u_(i - 1) + interval_; + } + else if (i > m_ - p_) + { + u_(i) = u_(i - 1) + interval_; + } + } + } + + void UniformBspline::setKnot(const Eigen::VectorXd &knot) { this->u_ = knot; } + + Eigen::VectorXd UniformBspline::getKnot() { return this->u_; } + + bool UniformBspline::getTimeSpan(double &um, double &um_p) + { + if (p_ > u_.rows() || m_ - p_ > u_.rows()) + return false; + + um = u_(p_); + um_p = u_(m_ - p_); + + return true; + } + + Eigen::MatrixXd UniformBspline::getControlPoint() { return control_points_; } + + Eigen::VectorXd UniformBspline::evaluateDeBoor(const double &u) + { + + double ub = min(max(u_(p_), u), u_(m_ - p_)); + + // determine which [ui,ui+1] lay in + int k = p_; + while (true) + { + if (u_(k + 1) >= ub) + break; + ++k; + } + + /* deBoor's alg */ + vector d; + for (int i = 0; i <= p_; ++i) + { + d.push_back(control_points_.col(k - p_ + i)); + // cout << d[i].transpose() << endl; + } + + for (int r = 1; r <= p_; ++r) + { + for (int i = p_; i >= r; --i) + { + double alpha = (ub - u_[i + k - p_]) / (u_[i + 1 + k - r] - u_[i + k - p_]); + // cout << "alpha: " << alpha << endl; + d[i] = (1 - alpha) * d[i - 1] + alpha * d[i]; + } + } + + return d[p_]; + } + + // Eigen::VectorXd UniformBspline::evaluateDeBoorT(const double& t) { + // return evaluateDeBoor(t + u_(p_)); + // } + + Eigen::MatrixXd UniformBspline::getDerivativeControlPoints() + { + // The derivative of a b-spline is also a b-spline, its order become p_-1 + // control point Qi = p_*(Pi+1-Pi)/(ui+p_+1-ui+1) + Eigen::MatrixXd ctp(control_points_.rows(), control_points_.cols() - 1); + for (int i = 0; i < ctp.cols(); ++i) + { + ctp.col(i) = + p_ * (control_points_.col(i + 1) - control_points_.col(i)) / (u_(i + p_ + 1) - u_(i + 1)); + } + return ctp; + } + + UniformBspline UniformBspline::getDerivative() + { + Eigen::MatrixXd ctp = getDerivativeControlPoints(); + UniformBspline derivative(ctp, p_ - 1, interval_); + + /* cut the first and last knot */ + Eigen::VectorXd knot(u_.rows() - 2); + knot = u_.segment(1, u_.rows() - 2); + derivative.setKnot(knot); + + return derivative; + } + + double UniformBspline::getInterval() { return interval_; } + + void UniformBspline::setPhysicalLimits(const double &vel, const double &acc, const double &tolerance) + { + limit_vel_ = vel; + limit_acc_ = acc; + limit_ratio_ = 1.1; + feasibility_tolerance_ = tolerance; + } + + bool UniformBspline::checkFeasibility(double &ratio, bool show) + { + bool fea = true; + + Eigen::MatrixXd P = control_points_; + int dimension = control_points_.rows(); + + /* check vel feasibility and insert points */ + double max_vel = -1.0; + double enlarged_vel_lim = limit_vel_ * (1.0 + feasibility_tolerance_) + 1e-4; + for (int i = 0; i < P.cols() - 1; ++i) + { + Eigen::VectorXd vel = p_ * (P.col(i + 1) - P.col(i)) / (u_(i + p_ + 1) - u_(i + 1)); + + if (fabs(vel(0)) > enlarged_vel_lim || fabs(vel(1)) > enlarged_vel_lim || + fabs(vel(2)) > enlarged_vel_lim) + { + + if (show) + cout << "[Check]: Infeasible vel " << i << " :" << vel.transpose() << endl; + fea = false; + + for (int j = 0; j < dimension; ++j) + { + max_vel = max(max_vel, fabs(vel(j))); + } + } + } + + /* acc feasibility */ + double max_acc = -1.0; + double enlarged_acc_lim = limit_acc_ * (1.0 + feasibility_tolerance_) + 1e-4; + for (int i = 0; i < P.cols() - 2; ++i) + { + + Eigen::VectorXd acc = p_ * (p_ - 1) * + ((P.col(i + 2) - P.col(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) - + (P.col(i + 1) - P.col(i)) / (u_(i + p_ + 1) - u_(i + 1))) / + (u_(i + p_ + 1) - u_(i + 2)); + + if (fabs(acc(0)) > enlarged_acc_lim || fabs(acc(1)) > enlarged_acc_lim || + fabs(acc(2)) > enlarged_acc_lim) + { + + if (show) + cout << "[Check]: Infeasible acc " << i << " :" << acc.transpose() << endl; + fea = false; + + for (int j = 0; j < dimension; ++j) + { + max_acc = max(max_acc, fabs(acc(j))); + } + } + } + + ratio = max(max_vel / limit_vel_, sqrt(fabs(max_acc) / limit_acc_)); + + return fea; + } + + void UniformBspline::lengthenTime(const double &ratio) + { + int num1 = 5; + int num2 = getKnot().rows() - 1 - 5; + + double delta_t = (ratio - 1.0) * (u_(num2) - u_(num1)); + double t_inc = delta_t / double(num2 - num1); + for (int i = num1 + 1; i <= num2; ++i) + u_(i) += double(i - num1) * t_inc; + for (int i = num2 + 1; i < u_.rows(); ++i) + u_(i) += delta_t; + } + + // void UniformBspline::recomputeInit() {} + + void UniformBspline::parameterizeToBspline(const double &ts, const vector &point_set, + const vector &start_end_derivative, + Eigen::MatrixXd &ctrl_pts) + { + if (ts <= 0) + { + cout << "[B-spline]:time step error." << endl; + return; + } + + if (point_set.size() <= 3) + { + cout << "[B-spline]:point set have only " << point_set.size() << " points." << endl; + return; + } + + if (start_end_derivative.size() != 4) + { + cout << "[B-spline]:derivatives error." << endl; + } + + int K = point_set.size(); + + // write A + Eigen::Vector3d prow(3), vrow(3), arow(3); + prow << 1, 4, 1; + vrow << -1, 0, 1; + arow << 1, -2, 1; + + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(K + 4, K + 2); + + for (int i = 0; i < K; ++i) + A.block(i, i, 1, 3) = (1 / 6.0) * prow.transpose(); + + A.block(K, 0, 1, 3) = (1 / 2.0 / ts) * vrow.transpose(); + A.block(K + 1, K - 1, 1, 3) = (1 / 2.0 / ts) * vrow.transpose(); + + A.block(K + 2, 0, 1, 3) = (1 / ts / ts) * arow.transpose(); + A.block(K + 3, K - 1, 1, 3) = (1 / ts / ts) * arow.transpose(); + + //cout << "A" << endl << A << endl << endl; + + // write b + Eigen::VectorXd bx(K + 4), by(K + 4), bz(K + 4); + for (int i = 0; i < K; ++i) + { + bx(i) = point_set[i](0); + by(i) = point_set[i](1); + bz(i) = point_set[i](2); + } + + for (int i = 0; i < 4; ++i) + { + bx(K + i) = start_end_derivative[i](0); + by(K + i) = start_end_derivative[i](1); + bz(K + i) = start_end_derivative[i](2); + } + + // solve Ax = b + Eigen::VectorXd px = A.colPivHouseholderQr().solve(bx); + Eigen::VectorXd py = A.colPivHouseholderQr().solve(by); + Eigen::VectorXd pz = A.colPivHouseholderQr().solve(bz); + + // convert to control pts + ctrl_pts.resize(3, K + 2); + ctrl_pts.row(0) = px.transpose(); + ctrl_pts.row(1) = py.transpose(); + ctrl_pts.row(2) = pz.transpose(); + + // cout << "[B-spline]: parameterization ok." << endl; + } + + double UniformBspline::getTimeSum() + { + double tm, tmp; + if (getTimeSpan(tm, tmp)) + return tmp - tm; + else + return -1.0; + } + + double UniformBspline::getLength(const double &res) + { + double length = 0.0; + double dur = getTimeSum(); + Eigen::VectorXd p_l = evaluateDeBoorT(0.0), p_n; + for (double t = res; t <= dur + 1e-4; t += res) + { + p_n = evaluateDeBoorT(t); + length += (p_n - p_l).norm(); + p_l = p_n; + } + return length; + } + + double UniformBspline::getJerk() + { + UniformBspline jerk_traj = getDerivative().getDerivative().getDerivative(); + + Eigen::VectorXd times = jerk_traj.getKnot(); + Eigen::MatrixXd ctrl_pts = jerk_traj.getControlPoint(); + int dimension = ctrl_pts.rows(); + + double jerk = 0.0; + for (int i = 0; i < ctrl_pts.cols(); ++i) + { + for (int j = 0; j < dimension; ++j) + { + jerk += (times(i + 1) - times(i)) * ctrl_pts(j, i) * ctrl_pts(j, i); + } + } + + return jerk; + } + + void UniformBspline::getMeanAndMaxVel(double &mean_v, double &max_v) + { + UniformBspline vel = getDerivative(); + double tm, tmp; + vel.getTimeSpan(tm, tmp); + + double max_vel = -1.0, mean_vel = 0.0; + int num = 0; + for (double t = tm; t <= tmp; t += 0.01) + { + Eigen::VectorXd vxd = vel.evaluateDeBoor(t); + double vn = vxd.norm(); + + mean_vel += vn; + ++num; + if (vn > max_vel) + { + max_vel = vn; + } + } + + mean_vel = mean_vel / double(num); + mean_v = mean_vel; + max_v = max_vel; + } + + void UniformBspline::getMeanAndMaxAcc(double &mean_a, double &max_a) + { + UniformBspline acc = getDerivative().getDerivative(); + double tm, tmp; + acc.getTimeSpan(tm, tmp); + + double max_acc = -1.0, mean_acc = 0.0; + int num = 0; + for (double t = tm; t <= tmp; t += 0.01) + { + Eigen::VectorXd axd = acc.evaluateDeBoor(t); + double an = axd.norm(); + + mean_acc += an; + ++num; + if (an > max_acc) + { + max_acc = an; + } + } + + mean_acc = mean_acc / double(num); + mean_a = mean_acc; + max_a = max_acc; + } +} // namespace ego_planner diff --git a/src/Prometheus/Modules/ego_planner_swarm/drone_detect/CMakeLists.txt b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/CMakeLists.txt new file mode 100644 index 00000000..1eacae46 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/CMakeLists.txt @@ -0,0 +1,130 @@ +cmake_minimum_required(VERSION 2.8.3) +project(drone_detect) + +set(CMAKE_BUILD_TYPE "Release") +ADD_COMPILE_OPTIONS(-std=c++11 ) +ADD_COMPILE_OPTIONS(-std=c++14 ) +## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, +## enforcing cleaner code. +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") + +## Find catkin macros and libraries +find_package(catkin REQUIRED + COMPONENTS + roscpp + sensor_msgs + roslint + cv_bridge + message_filters +) + +## Find system libraries +find_package(Eigen3 REQUIRED) +find_package(OpenCV REQUIRED) +find_package(Boost REQUIRED) + +################################### +## 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 + ## This is only necessary because Eigen3 sets a non-standard EIGEN3_INCLUDE_DIR variable + ${EIGEN3_INCLUDE_DIR} + LIBRARIES + CATKIN_DEPENDS + roscpp + sensor_msgs + DEPENDS OpenCV Eigen Boost + ## find_package(Eigen3) provides a non standard EIGEN3_INCLUDE_DIR instead of Eigen3_INCLUDE_DIRS. + ## Therefore, the DEPEND does not work as expected and we need to add the directory to the INCLUDE_DIRS + # Eigen3 + + ## Boost is not part of the DEPENDS since it is only used in source files, + ## Dependees do not depend on Boost when they depend on this package. +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + # Set manually because Eigen sets a non standard INCLUDE DIR + ${EIGEN3_INCLUDE_DIR} + # Set because Boost is an internal dependency, not transitive. + ${Boost_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + +) + +## Declare cpp executables +add_executable(${PROJECT_NAME} + src/${PROJECT_NAME}_node.cpp + src/drone_detector.cpp +) +target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_11) + +## Add dependencies to exported targets, like ROS msgs or srvs +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) + + +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${OpenCV_LIBS} +) + +############# +## Install ## +############# + +# Mark executables and/or libraries for installation +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +# Mark other files for installation +install( + DIRECTORY doc + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +# if(${CATKIN_ENABLE_TESTING}) +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") +# ## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test +# test/test_drone_detector.cpp) + +# target_link_libraries(${PROJECT_NAME}-test) +# endif() + +########################## +## Static code analysis ## +########################## + +roslint_cpp() diff --git a/src/Prometheus/Modules/ego_planner_swarm/drone_detect/LICENSE b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/LICENSE new file mode 100644 index 00000000..9b02d46f --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/LICENSE @@ -0,0 +1,29 @@ +Software License Agreement (BSD License) + +Copyright (c) 2017, Peter Fankhauser +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 Autonomous Systems Lab nor ETH Zurich 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 HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/src/Prometheus/Modules/ego_planner_swarm/drone_detect/README.md b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/README.md new file mode 100644 index 00000000..2388e527 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/README.md @@ -0,0 +1,97 @@ +# Drone Detect + +## Overview + +This is a package for detecting other drones in depth image and then calculating their true pose error in the world frame of the observer drone. + + + +![Example image](doc/demo.jpg) + + + +## Usage + +You can launch the node alongside the main after you assigning the right topic name + +``` +roslaunch drone_detect drone_detect.launch +``` + + + +or add the following code in `run_in_sim.launch` + +```xml + + + + + + + + + + + + + + +``` + + + +## Config files + +* **camera.yaml** : The camera intrinsics are stored in this file +* **default.yaml**: Some parameters related to drone detection. + +```yaml +debug_flag: true +# the proportion of the pixel threshold to the total pixels projected from the drone to the depth map +estimate/pixel_ratio: 0.1 +# the radius of the pose errror sphere +estimate/max_pose_error: 0.4 +# the width and height of the model of drone +estimate/drone_width: 0.5 +estimate/drone_height: 0.1 +``` + + + +## Nodes + + +#### Subscribed Topics + +* **`/depth`** ([sensor_msgs::Image]) + + The depth image from pcl_render_node. + +* **`/colordepth`**([sensor_msgs::Image]) + + The color image from pcl_render_node + +* **`/camera_pose`** ([geometry_msgs::PoseStamped]) + + The camere pose from pcl_render_node + +The above three topics are synchronized when in use, the callback function is **`rcvDepthColorCamPoseCallback`** + +- **`/dronex_odom_sub_`** ([nav_msgs::Odometry]) + + The odometry of other drones + +#### Published Topics + +* **`/new_depth`** ([sensor_msgs::Image]) + + The new depth image after erasing the moving drones + +* **`/new_colordepth`**([sensor_msgs::Image]) + + The color image with some debug marks + +* **`/camera_pose_error`** ([geometry_msgs::PoseStamped]) + + The pose error of detected drones in world frame of the observer drone. \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/drone_detect/config/camera.yaml b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/config/camera.yaml new file mode 100755 index 00000000..556e5962 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/config/camera.yaml @@ -0,0 +1,7 @@ +cam_width: 640 +cam_height: 480 +cam_fx: 386.02557373046875 +cam_fy: 386.02557373046875 +cam_cx: 321.8554382324219 +cam_cy: 241.2396240234375 + diff --git a/src/Prometheus/Modules/ego_planner_swarm/drone_detect/config/default.yaml b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/config/default.yaml new file mode 100644 index 00000000..37a666d2 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/config/default.yaml @@ -0,0 +1,5 @@ +# debug_flag: true +pixel_ratio: 0.1 +estimate/max_pose_error: 0.4 +estimate/drone_width: 0.5 +estimate/drone_height: 0.2 \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/drone_detect/doc/demo.jpg b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/doc/demo.jpg new file mode 100644 index 00000000..690972dd Binary files /dev/null and b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/doc/demo.jpg differ diff --git a/src/Prometheus/Modules/ego_planner_swarm/drone_detect/doc/example.jpg b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/doc/example.jpg new file mode 100644 index 00000000..d047c4fa Binary files /dev/null and b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/doc/example.jpg differ diff --git a/src/Prometheus/Modules/ego_planner_swarm/drone_detect/include/drone_detector/drone_detector.h b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/include/drone_detector/drone_detector.h new file mode 100644 index 00000000..271210ea --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/include/drone_detector/drone_detector.h @@ -0,0 +1,156 @@ +#pragma once +#include +#include +// ROS +#include +#include "std_msgs/String.h" +#include "std_msgs/Bool.h" +#include +#include +#include +// synchronize topic +#include +#include +#include +#include + +#include + +//include opencv and eigen +#include +#include +#include +#include +#include + +namespace detect { + +const int max_drone_num_ = 3; + +/*! + * Main class for the node to handle the ROS interfacing. + */ +class DroneDetector +{ + public: + /*! + * Constructor. + * @param nodeHandle the ROS node handle. + */ + DroneDetector(ros::NodeHandle& nodeHandle); + + /*! + * Destructor. + */ + virtual ~DroneDetector(); + + void test(); + private: + void readParameters(); + + // inline functions + double getDist2(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2); + double getDist2(const Eigen::Vector4d &p1, const Eigen::Vector4d &p2); + Eigen::Vector4d depth2Pos(int u, int v, float depth); + Eigen::Vector4d depth2Pos(const Eigen::Vector2i &pixel, float depth) ; + Eigen::Vector2i pos2Depth(const Eigen::Vector4d &pose_in_camera) ; + bool isInSensorRange(const Eigen::Vector2i &pixel); + + bool countPixel(int drone_id, Eigen::Vector2i &true_pixel, Eigen::Vector4d &true_pose_cam); + void detect(int drone_id, Eigen::Vector2i &true_pixel); + + // subscribe callback function + void rcvDepthColorCamPoseCallback(const sensor_msgs::ImageConstPtr& depth_img, \ + const sensor_msgs::ImageConstPtr& color_img, \ + const geometry_msgs::PoseStampedConstPtr& camera_pose); + + void rcvDepthCamPoseCallback(const sensor_msgs::ImageConstPtr& depth_img, \ + const geometry_msgs::PoseStampedConstPtr& camera_pose); + + void rcvMyOdomCallback(const nav_msgs::Odometry& odom); + void rcvDepthImgCallback(const sensor_msgs::ImageConstPtr& depth_img); + + + void rcvDroneOdomCallbackBase(const nav_msgs::Odometry& odom, const int drone_id); + + void rcvDrone0OdomCallback(const nav_msgs::Odometry& odom); + void rcvDrone1OdomCallback(const nav_msgs::Odometry& odom); + void rcvDrone2OdomCallback(const nav_msgs::Odometry& odom); + void rcvDroneXOdomCallback(const nav_msgs::Odometry& odom); + + //! ROS node handle. + ros::NodeHandle& nh_; + + //! ROS topic subscriber. + // depth, colordepth, camera_pos subscriber + typedef message_filters::sync_policies::ApproximateTime SyncPolicyDepthColorImagePose; + typedef std::shared_ptr> SynchronizerDepthColorImagePose; + typedef message_filters::sync_policies::ApproximateTime SyncPolicyDepthImagePose; + typedef std::shared_ptr> SynchronizerDepthImagePose; + + // std::shared_ptr> depth_img_sub_; + std::shared_ptr> colordepth_img_sub_; + std::shared_ptr> camera_pos_sub_; + + SynchronizerDepthColorImagePose sync_depth_color_img_pose_; + SynchronizerDepthImagePose sync_depth_img_pose_; + // other drones subscriber + ros::Subscriber drone0_odom_sub_, drone1_odom_sub_, drone2_odom_sub_, droneX_odom_sub_; + std::string drone1_odom_topic_, drone2_odom_topic_; + + ros::Subscriber my_odom_sub_, depth_img_sub_; + bool has_odom_; + nav_msgs::Odometry my_odom_; + // ROS topic publisher + // new_depth_img: erase the detected drones + // new_colordepth_img: for debug + ros::Publisher new_depth_img_pub_; + ros::Publisher debug_depth_img_pub_; + + // parameters + //camera param + int img_width_, img_height_; + double fx_,fy_,cx_,cy_; + + double max_pose_error_; + double max_pose_error2_; + double drone_width_, drone_height_; + double pixel_ratio_; + int pixel_threshold_; + + // for debug + bool debug_flag_; + int debug_detect_result_[max_drone_num_]; + std::stringstream debug_img_text_[max_drone_num_]; + ros::Time debug_start_time_, debug_end_time_; + + ros::Publisher debug_info_pub_; + ros::Publisher drone_pose_err_pub_[max_drone_num_]; + + int my_id_; + cv::Mat depth_img_, color_img_; + + Eigen::Matrix4d cam2body_; + Eigen::Matrix4d cam2world_; + Eigen::Quaterniond cam2world_quat_; + Eigen::Vector4d my_pose_world_; + Eigen::Quaterniond my_attitude_world_; + Eigen::Vector4d my_last_pose_world_; + ros::Time my_last_odom_stamp_ = ros::TIME_MAX; + ros::Time my_last_camera_stamp_ = ros::TIME_MAX; + + Eigen::Matrix4d drone2world_[max_drone_num_]; + Eigen::Vector4d drone_pose_world_[max_drone_num_]; + Eigen::Quaterniond drone_attitude_world_[max_drone_num_]; + Eigen::Vector4d drone_pose_cam_[max_drone_num_]; + Eigen::Vector2i drone_ref_pixel_[max_drone_num_]; + + std::vector hit_pixels_[max_drone_num_]; + int valid_pixel_cnt_[max_drone_num_]; + + bool in_depth_[max_drone_num_] = {false}; + cv::Point searchbox_lu_[max_drone_num_], searchbox_rd_[max_drone_num_]; + cv::Point boundingbox_lu_[max_drone_num_], boundingbox_rd_[max_drone_num_]; +}; + +} /* namespace */ diff --git a/src/Prometheus/Modules/ego_planner_swarm/drone_detect/launch/drone_detect.launch b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/launch/drone_detect.launch new file mode 100644 index 00000000..6aa4a254 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/launch/drone_detect.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/drone_detect/launch/ros_package_template_overlying_params.launch b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/launch/ros_package_template_overlying_params.launch new file mode 100644 index 00000000..0ac3e223 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/launch/ros_package_template_overlying_params.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/drone_detect/package.xml b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/package.xml new file mode 100644 index 00000000..bef32704 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/package.xml @@ -0,0 +1,20 @@ + + + drone_detect + 0.1.0 + Detect other drones in depth image. + Jiangchao Zhu + BSD + Jiangchao Zhu + + + catkin + + boost + + eigen + roscpp + sensor_msgs + + roslint + diff --git a/src/Prometheus/Modules/ego_planner_swarm/drone_detect/src/drone_detect_node.cpp b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/src/drone_detect_node.cpp new file mode 100644 index 00000000..8e29362d --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/src/drone_detect_node.cpp @@ -0,0 +1,14 @@ +#include +#include "drone_detector/drone_detector.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "drone_detect"); + ros::NodeHandle nh("~"); + + detect::DroneDetector drone_detector(nh); + drone_detector.test(); + + ros::spin(); + return 0; +} diff --git a/src/Prometheus/Modules/ego_planner_swarm/drone_detect/src/drone_detector.cpp b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/src/drone_detector.cpp new file mode 100644 index 00000000..d3bacd3d --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/src/drone_detector.cpp @@ -0,0 +1,423 @@ +#include "drone_detector/drone_detector.h" + +// STD +#include + +namespace detect { + +DroneDetector::DroneDetector(ros::NodeHandle& nodeHandle) + : nh_(nodeHandle) +{ + readParameters(); + + // depth_img_sub_.reset(new message_filters::Subscriber(nh_, "depth", 50, ros::TransportHints().tcpNoDelay())); + // colordepth_img_sub_.reset(new message_filters::Subscriber(nh_, "colordepth", 50)); + // camera_pos_sub_.reset(new message_filters::Subscriber(nh_, "camera_pose", 50)); + + my_odom_sub_ = nh_.subscribe("odometry", 100, &DroneDetector::rcvMyOdomCallback, this, ros::TransportHints().tcpNoDelay()); + depth_img_sub_ = nh_.subscribe("depth", 50, &DroneDetector::rcvDepthImgCallback, this, ros::TransportHints().tcpNoDelay()); + // sync_depth_color_img_pose_->registerCallback(boost::bind(&DroneDetector::rcvDepthColorCamPoseCallback, this, _1, _2, _3)); + + // drone0_odom_sub_ = nh_.subscribe("drone0", 50, &DroneDetector::rcvDrone0OdomCallback, this); + // drone1_odom_sub_ = nh_.subscribe("drone1", 50, &DroneDetector::rcvDrone1OdomCallback, this); + // drone2_odom_sub_ = nh_.subscribe("drone2", 50, &DroneDetector::rcvDrone2OdomCallback, this); + droneX_odom_sub_ = nh_.subscribe("/others_odom", 100, &DroneDetector::rcvDroneXOdomCallback, this, ros::TransportHints().tcpNoDelay()); + + new_depth_img_pub_ = nh_.advertise("new_depth_image", 50); + debug_depth_img_pub_ = nh_.advertise("debug_depth_image", 50); + + debug_info_pub_ = nh_.advertise("/debug_info", 50); + + cam2body_ << 0.0, 0.0, 1.0, 0.0, + -1.0, 0.0, 0.0, 0.0, + 0.0, -1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0; + + // init drone_pose_err_pub + for(int i = 0; i < max_drone_num_; i++) { + if(i != my_id_) + drone_pose_err_pub_[i] = nh_.advertise("drone"+std::to_string(i)+"to"+std::to_string(my_id_)+"_pose_err", 50); + } + + ROS_INFO("Successfully launched node."); +} + +DroneDetector::~DroneDetector() +{ +} + +void DroneDetector::readParameters() +{ + // camera params + nh_.getParam("cam_width", img_width_); + nh_.getParam("cam_height", img_height_); + nh_.getParam("cam_fx", fx_); + nh_.getParam("cam_fy", fy_); + nh_.getParam("cam_cx", cx_); + nh_.getParam("cam_cy", cy_); + + // + nh_.getParam("debug_flag", debug_flag_); + nh_.getParam("pixel_ratio", pixel_ratio_); + nh_.getParam("my_id", my_id_); + nh_.getParam("estimate/drone_width", drone_width_); + nh_.getParam("estimate/drone_height", drone_height_); + nh_.getParam("estimate/max_pose_error", max_pose_error_); + + max_pose_error2_ = max_pose_error_*max_pose_error_; +} + +// inline functions +inline double DroneDetector::getDist2(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2) +{ + double delta_x = p1(0)-p2(0); + double delta_y = p1(1)-p2(1); + double delta_z = p1(2)-p2(2); + return delta_x*delta_x+delta_y*delta_y+delta_z*delta_z; +} + +inline double DroneDetector::getDist2(const Eigen::Vector4d &p1, const Eigen::Vector4d &p2) +{ + double delta_x = p1(0)-p2(0); + double delta_y = p1(1)-p2(1); + double delta_z = p1(2)-p2(2); + return delta_x*delta_x+delta_y*delta_y+delta_z*delta_z; +} + +inline Eigen::Vector4d DroneDetector::depth2Pos(int u, int v, float depth) +{ + Eigen::Vector4d pose_in_camera; + pose_in_camera(0) = (u - cx_) * depth / fx_; + pose_in_camera(1) = (v - cy_) * depth / fy_; + pose_in_camera(2) = depth; + pose_in_camera(3) = 1.0; + return pose_in_camera; +} + +inline Eigen::Vector4d DroneDetector::depth2Pos(const Eigen::Vector2i &pixel, float depth) +{ + Eigen::Vector4d pose_in_camera; + pose_in_camera(0) = (pixel(0) - cx_) * depth / fx_; + pose_in_camera(1) = (pixel(1) - cy_) * depth / fy_; + pose_in_camera(2) = depth; + pose_in_camera(3) = 1.0; + return pose_in_camera; +} + +inline Eigen::Vector2i DroneDetector::pos2Depth(const Eigen::Vector4d &pose_in_camera) +{ + float depth = pose_in_camera(2); + Eigen::Vector2i pixel; + pixel(0) = pose_in_camera(0) * fx_ / depth + cx_ + 0.5; + pixel(1) = pose_in_camera(1) * fy_ / depth + cy_ + 0.5; + return pixel; +} + +inline bool DroneDetector::isInSensorRange(const Eigen::Vector2i &pixel) +{ + if (pixel(0)>=0 && pixel(1) >= 0 && pixel(0) <= img_width_ && pixel(1) <= img_height_) return true; + else + return false; +} + +void DroneDetector::rcvMyOdomCallback(const nav_msgs::Odometry& odom) +{ + my_odom_ = odom; + Eigen::Matrix4d body2world = Eigen::Matrix4d::Identity(); + + my_pose_world_(0) = odom.pose.pose.position.x; + my_pose_world_(1) = odom.pose.pose.position.y; + my_pose_world_(2) = odom.pose.pose.position.z; + my_pose_world_(3) = 1.0; + my_attitude_world_.x() = odom.pose.pose.orientation.x; + my_attitude_world_.y() = odom.pose.pose.orientation.y; + my_attitude_world_.z() = odom.pose.pose.orientation.z; + my_attitude_world_.w() = odom.pose.pose.orientation.w; + body2world.block<3,3>(0,0) = my_attitude_world_.toRotationMatrix(); + body2world(0,3) = my_pose_world_(0); + body2world(1,3) = my_pose_world_(1); + body2world(2,3) = my_pose_world_(2); + + //convert to cam pose + cam2world_ = body2world * cam2body_; + cam2world_quat_ = cam2world_.block<3,3>(0,0); + + // my_last_odom_stamp_ = odom.header.stamp; + + // my_last_pose_world_(0) = odom.pose.pose.position.x; + // my_last_pose_world_(1) = odom.pose.pose.position.y; + // my_last_pose_world_(2) = odom.pose.pose.position.z; + // my_last_pose_world_(3) = 1.0; + + //publish tf + // static tf::TransformBroadcaster br; + // tf::Transform transform; + // transform.setOrigin( tf::Vector3(cam2world(0,3), cam2world(1,3), cam2world(2,3) )); + // transform.setRotation(tf::Quaternion(cam2world_quat.x(), cam2world_quat.y(), cam2world_quat.z(), cam2world_quat.w())); + // br.sendTransform(tf::StampedTransform(transform, my_last_odom_stamp, "world", "camera")); + //publish transform from world frame to quadrotor frame. +} +void DroneDetector::rcvDepthImgCallback(const sensor_msgs::ImageConstPtr& depth_img) +{ + /* get depth image */ + cv_bridge::CvImagePtr cv_ptr; + cv_ptr = cv_bridge::toCvCopy(depth_img, depth_img->encoding); + cv_ptr->image.copyTo(depth_img_); + + debug_start_time_ = ros::Time::now(); + + Eigen::Vector2i true_pixel[max_drone_num_]; + for (int i = 0; i < max_drone_num_; i++) { + if (in_depth_[i]) { + detect(i, true_pixel[i]); + } + } + + cv_bridge::CvImage out_msg; + for (int i = 0; i < max_drone_num_; i++) { + if (in_depth_[i]) { + // erase hit pixels in depth + for(int k = 0; k < int(hit_pixels_[i].size()); k++) { + // depth_img_.at(hit_pixels_[i][k](1), hit_pixels_[i][k](0)) = 0; + uint16_t *row_ptr; + row_ptr = depth_img_.ptr(hit_pixels_[i][k](1)); + (*(row_ptr+hit_pixels_[i][k](0))) = 0.0; + } + } + } + debug_end_time_ = ros::Time::now(); + // ROS_WARN("cost_total_time = %lf", (debug_end_time_ - debug_start_time_).toSec()*1000.0); + out_msg.header = depth_img->header; + out_msg.encoding = depth_img->encoding; + out_msg.image = depth_img_.clone(); + new_depth_img_pub_.publish(out_msg.toImageMsg()); + + std_msgs::String msg; + std::stringstream ss; + if(debug_flag_) { + for (int i = 0; i < max_drone_num_; i++) { + if (in_depth_[i]) { + // add bound box in colormap + // cv::Rect rect(_bbox_lu.x, _bbox_lu.y, _bbox_rd.x, _bbox_rd.y);//左上坐标（x,y）和矩形的长(x)宽(y) + cv::rectangle(depth_img_, cv::Rect(searchbox_lu_[i], searchbox_rd_[i]), cv::Scalar(0, 0, 0), 5, cv::LINE_8, 0); + cv::rectangle(depth_img_, cv::Rect(boundingbox_lu_[i], boundingbox_rd_[i]), cv::Scalar(0, 0, 0), 5, cv::LINE_8, 0); + if (debug_detect_result_[i] == 1) { + ss << "no enough " << hit_pixels_[i].size(); + } else if(debug_detect_result_[i] == 2) { + ss << "success"; + } + } else { + ss << "no detect"; + } + } + out_msg.header = depth_img->header; + out_msg.encoding = depth_img->encoding; + out_msg.image = depth_img_.clone(); + debug_depth_img_pub_.publish(out_msg.toImageMsg()); + msg.data = ss.str(); + debug_info_pub_.publish(msg); + } +} + +void DroneDetector::rcvDroneOdomCallbackBase(const nav_msgs::Odometry& odom, int drone_id) +{ + if (drone_id == my_id_) { + return; + } + Eigen::Matrix4d drone2world = Eigen::Matrix4d::Identity(); + drone_pose_world_[drone_id](0) = odom.pose.pose.position.x; + drone_pose_world_[drone_id](1) = odom.pose.pose.position.y; + drone_pose_world_[drone_id](2) = odom.pose.pose.position.z; + drone_pose_world_[drone_id](3) = 1.0; + + drone_attitude_world_[drone_id].x() = odom.pose.pose.orientation.x; + drone_attitude_world_[drone_id].y() = odom.pose.pose.orientation.y; + drone_attitude_world_[drone_id].z() = odom.pose.pose.orientation.z; + drone_attitude_world_[drone_id].w() = odom.pose.pose.orientation.w; + drone2world.block<3,3>(0,0) = drone_attitude_world_[drone_id].toRotationMatrix(); + + drone2world(0,3) = drone_pose_world_[drone_id](0); + drone2world(1,3) = drone_pose_world_[drone_id](1); + drone2world(2,3) = drone_pose_world_[drone_id](2); + + drone_pose_cam_[drone_id] = cam2world_.inverse() * drone_pose_world_[drone_id]; + // if the drone is in sensor range + drone_ref_pixel_[drone_id] = pos2Depth(drone_pose_cam_[drone_id]); + if (drone_pose_cam_[drone_id](2) > 0 && isInSensorRange(drone_ref_pixel_[drone_id])) { + in_depth_[drone_id] = true; + } else { + in_depth_[drone_id] = false; + debug_detect_result_[drone_id] = 0; + } +} + +void DroneDetector::rcvDrone0OdomCallback(const nav_msgs::Odometry& odom) +{ + rcvDroneOdomCallbackBase(odom, 0); +} + +void DroneDetector::rcvDrone1OdomCallback(const nav_msgs::Odometry& odom) +{ + rcvDroneOdomCallbackBase(odom, 1); +} + +void DroneDetector::rcvDrone2OdomCallback(const nav_msgs::Odometry& odom) +{ + rcvDroneOdomCallbackBase(odom, 2); +} + +void DroneDetector::rcvDroneXOdomCallback(const nav_msgs::Odometry& odom) +{ + std::string numstr = odom.child_frame_id.substr(6); + try + { + int drone_id = std::stoi(numstr); + rcvDroneOdomCallbackBase(odom, drone_id); + } + catch(const std::exception& e) + { + std::cout << e.what() << '\n'; + } +} + +bool DroneDetector::countPixel(int drone_id, Eigen::Vector2i &true_pixel, Eigen::Vector4d &true_pose_cam) +{ + boundingbox_lu_[drone_id].x = img_width_; + boundingbox_rd_[drone_id].x = 0; + boundingbox_lu_[drone_id].y = img_height_; + boundingbox_rd_[drone_id].y = 0; + + valid_pixel_cnt_[drone_id] = 0; + hit_pixels_[drone_id].clear(); + + Eigen::Vector2i tmp_pixel; + Eigen::Vector4d tmp_pose_cam; + int search_radius = 2*max_pose_error_*fx_/drone_pose_cam_[drone_id](2); + float depth; + searchbox_lu_[drone_id].x = drone_ref_pixel_[drone_id](0) - search_radius; + searchbox_lu_[drone_id].y = drone_ref_pixel_[drone_id](1) - search_radius; + searchbox_rd_[drone_id].x = drone_ref_pixel_[drone_id](0) + search_radius; + searchbox_rd_[drone_id].y = drone_ref_pixel_[drone_id](1) + search_radius; + // check the tmp_p around ref_pixel + for(int i = -search_radius; i <= search_radius; i++) + for(int j = -search_radius; j <= search_radius; j++) + { + tmp_pixel(0) = drone_ref_pixel_[drone_id](0) + j; + tmp_pixel(1) = drone_ref_pixel_[drone_id](1) + i; + if(tmp_pixel(0) < 0 || tmp_pixel(0) >= img_width_ || tmp_pixel(1) < 0 || tmp_pixel(1) >= img_height_) + continue; + // depth = depth_img_.at(tmp_pixel(1), tmp_pixel(0)); + uint16_t *row_ptr; + row_ptr = depth_img_.ptr(tmp_pixel(1)); + depth = (*(row_ptr+tmp_pixel(0))) / 1000.0; + // ROS_WARN("depth = %lf", depth); + // get tmp_pose in cam frame + tmp_pose_cam = depth2Pos(tmp_pixel(0), tmp_pixel(1), depth); + double dist2 = getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]); + // ROS_WARN("dist2 = %lf", dist2); + if (dist2 < max_pose_error2_) { + valid_pixel_cnt_[drone_id]++; + hit_pixels_[drone_id].push_back(tmp_pixel); + boundingbox_lu_[drone_id].x = tmp_pixel(0) < boundingbox_lu_[drone_id].x ? tmp_pixel(0) : boundingbox_lu_[drone_id].x; + boundingbox_lu_[drone_id].y = tmp_pixel(1) < boundingbox_lu_[drone_id].y ? tmp_pixel(1) : boundingbox_lu_[drone_id].y; + boundingbox_rd_[drone_id].x = tmp_pixel(0) > boundingbox_rd_[drone_id].x ? tmp_pixel(0) : boundingbox_rd_[drone_id].x; + boundingbox_rd_[drone_id].y = tmp_pixel(1) > boundingbox_rd_[drone_id].y ? tmp_pixel(1) : boundingbox_rd_[drone_id].y; + } + } + pixel_threshold_ = (drone_width_*fx_/drone_pose_cam_[drone_id](2)) * (drone_height_*fy_/drone_pose_cam_[drone_id](2))*pixel_ratio_; + if (valid_pixel_cnt_[drone_id] > pixel_threshold_) { + int step = 1, size = (boundingbox_rd_[drone_id].y-boundingbox_lu_[drone_id].y) < (boundingbox_rd_[drone_id].x-boundingbox_lu_[drone_id].x) ? (boundingbox_rd_[drone_id].y-boundingbox_lu_[drone_id].y) : (boundingbox_rd_[drone_id].x-boundingbox_lu_[drone_id].x); + int init_x = (boundingbox_lu_[drone_id].x+boundingbox_rd_[drone_id].x)/2, init_y = (boundingbox_lu_[drone_id].y+boundingbox_rd_[drone_id].y)/2; + int x_flag = 1, y_flag = 1; + int x_idx = 0, y_idx = 0; + uint16_t *row_ptr; + row_ptr = depth_img_.ptr(tmp_pixel(1)); + depth = (*(row_ptr+tmp_pixel(0))) / 1000.0; + tmp_pose_cam = depth2Pos(init_x, init_y, depth); + if (getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]) < max_pose_error2_){ + true_pixel(0) = init_x; + true_pixel(1) = init_y; + true_pose_cam = tmp_pose_cam; + return true; + } + while(step(tmp_pixel(1)); + depth = (*(row_ptr+tmp_pixel(0))) / 1000.0; + tmp_pose_cam = depth2Pos(init_x, init_y, depth); + if (getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]) < max_pose_error2_) { + true_pixel(0) = init_x; + true_pixel(1) = init_y; + true_pose_cam = tmp_pose_cam; + return true; + } + x_idx++; + } + x_idx = 0; + x_flag = -x_flag; + while(y_idx(tmp_pixel(1)); + depth = (*(row_ptr+tmp_pixel(0))) / 1000.0; + tmp_pose_cam = depth2Pos(init_x, init_y, depth); + if (getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]) < max_pose_error2_){ + true_pixel(0) = init_x; + true_pixel(1) = init_y; + true_pose_cam = tmp_pose_cam; + return true; + } + y_idx++; + } + y_idx = 0; + y_flag = -y_flag; + step++; + } + while(x_idx(tmp_pixel(1)); + depth = (*(row_ptr+tmp_pixel(0))) / 1000.0; + tmp_pose_cam = depth2Pos(init_x, init_y, depth); + if (getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]) < max_pose_error2_){ + true_pixel(0) = init_x; + true_pixel(1) = init_y; + true_pose_cam = tmp_pose_cam; + return true; + } + x_idx++; + } + } + return false; +} + +void DroneDetector::detect(int drone_id, Eigen::Vector2i &true_pixel) +{ + Eigen::Vector4d true_pose_cam, pose_error; + bool found = countPixel(drone_id, true_pixel, true_pose_cam); + if (found) { + // ROS_WARN("FOUND"); + pose_error = cam2world_*true_pose_cam - drone_pose_world_[drone_id]; + debug_detect_result_[drone_id] = 2; + + geometry_msgs::PoseStamped out_msg; + out_msg.header.stamp = my_last_camera_stamp_; + out_msg.header.frame_id = "/drone_detect"; + out_msg.pose.position.x = pose_error(0); + out_msg.pose.position.y = pose_error(1); + out_msg.pose.position.z = pose_error(2); + drone_pose_err_pub_[drone_id].publish(out_msg); + + } else { + // ROS_WARN("NOT FOUND"); + debug_detect_result_[drone_id] = 1; + } +} + +void DroneDetector::test() { + ROS_WARN("my_id = %d", my_id_); +} + +} /* namespace */ diff --git a/src/Prometheus/Modules/ego_planner_swarm/drone_detect/test/test_drone_detector.cpp b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/test/test_drone_detector.cpp new file mode 100644 index 00000000..2fc9dd58 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/drone_detect/test/test_drone_detector.cpp @@ -0,0 +1,10 @@ +// gtest +#include + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + srand((int)time(0)); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/ego_planner.md b/src/Prometheus/Modules/ego_planner_swarm/ego_planner.md new file mode 100644 index 00000000..747e4000 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/ego_planner.md @@ -0,0 +1,24 @@ +## EGO_planner_swarm + +map_generator +- 局部点云 +- 全局点云 + +uav_control +- fake_odom +- PX4 + +#### 情况讨论 + + - 局部点云情况 + - 对应真实情况:D435i等RGBD相机,三维激光雷达 + - map_generator生成全局点云,模拟得到局部点云信息 + - 无人机不需要搭载传感器 + - PX4动力学 & fake_odom模块 + +roslaunch prometheus_simulator_utils map_generator_with_fake_odom.launch +roslaunch ego_planner sitl_ego_planner_basic.launch + + +## 运行 + diff --git a/src/Prometheus/Modules/ego_planner_swarm/path_searching/CMakeLists.txt b/src/Prometheus/Modules/ego_planner_swarm/path_searching/CMakeLists.txt new file mode 100755 index 00000000..13f1f661 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/path_searching/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 2.8.3) +project(path_searching) + +set(CMAKE_BUILD_TYPE "Release") +ADD_COMPILE_OPTIONS(-std=c++11 ) +ADD_COMPILE_OPTIONS(-std=c++14 ) +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") + +find_package(Eigen3 REQUIRED) +find_package(PCL 1.7 REQUIRED) + +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + visualization_msgs + plan_env + cv_bridge +) + + +catkin_package( + INCLUDE_DIRS include + LIBRARIES path_searching + CATKIN_DEPENDS plan_env +# DEPENDS system_lib +) + +include_directories( + SYSTEM + include + ${catkin_INCLUDE_DIRS} + ${Eigen3_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} +) + +add_library( path_searching + src/dyn_a_star.cpp + ) +target_link_libraries( path_searching + ${catkin_LIBRARIES} + ) diff --git a/src/Prometheus/Modules/ego_planner_swarm/path_searching/include/path_searching/dyn_a_star.h b/src/Prometheus/Modules/ego_planner_swarm/path_searching/include/path_searching/dyn_a_star.h new file mode 100755 index 00000000..388668b2 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/path_searching/include/path_searching/dyn_a_star.h @@ -0,0 +1,115 @@ +#ifndef _DYN_A_STAR_H_ +#define _DYN_A_STAR_H_ + +#include +#include +#include +#include +#include +#include + +constexpr double inf = 1 >> 20; +struct GridNode; +typedef GridNode *GridNodePtr; + +struct GridNode +{ + enum enum_state + { + OPENSET = 1, + CLOSEDSET = 2, + UNDEFINED = 3 + }; + + int rounds{0}; // Distinguish every call + enum enum_state state + { + UNDEFINED + }; + Eigen::Vector3i index; + + double gScore{inf}, fScore{inf}; + GridNodePtr cameFrom{NULL}; +}; + +class NodeComparator +{ +public: + bool operator()(GridNodePtr node1, GridNodePtr node2) + { + return node1->fScore > node2->fScore; + } +}; + +class AStar +{ +private: + GridMap::Ptr grid_map_; + + inline void coord2gridIndexFast(const double x, const double y, const double z, int &id_x, int &id_y, int &id_z); + + double getDiagHeu(GridNodePtr node1, GridNodePtr node2); + double getManhHeu(GridNodePtr node1, GridNodePtr node2); + double getEuclHeu(GridNodePtr node1, GridNodePtr node2); + inline double getHeu(GridNodePtr node1, GridNodePtr node2); + + bool ConvertToIndexAndAdjustStartEndPoints(const Eigen::Vector3d start_pt, const Eigen::Vector3d end_pt, Eigen::Vector3i &start_idx, Eigen::Vector3i &end_idx); + + inline Eigen::Vector3d Index2Coord(const Eigen::Vector3i &index) const; + inline bool Coord2Index(const Eigen::Vector3d &pt, Eigen::Vector3i &idx) const; + + //bool (*checkOccupancyPtr)( const Eigen::Vector3d &pos ); + + inline bool checkOccupancy(const Eigen::Vector3d &pos) { return (bool)grid_map_->getInflateOccupancy(pos); } + + std::vector retrievePath(GridNodePtr current); + + double step_size_, inv_step_size_; + Eigen::Vector3d center_; + Eigen::Vector3i CENTER_IDX_, POOL_SIZE_; + const double tie_breaker_ = 1.0 + 1.0 / 10000; + + std::vector gridPath_; + + GridNodePtr ***GridNodeMap_; + std::priority_queue, NodeComparator> openSet_; + + int rounds_{0}; + +public: + typedef std::shared_ptr Ptr; + + AStar(){}; + ~AStar(); + + void initGridMap(GridMap::Ptr occ_map, const Eigen::Vector3i pool_size); + + bool AstarSearch(const double step_size, Eigen::Vector3d start_pt, Eigen::Vector3d end_pt); + + std::vector getPath(); +}; + +inline double AStar::getHeu(GridNodePtr node1, GridNodePtr node2) +{ + return tie_breaker_ * getDiagHeu(node1, node2); +} + +inline Eigen::Vector3d AStar::Index2Coord(const Eigen::Vector3i &index) const +{ + return ((index - CENTER_IDX_).cast() * step_size_) + center_; +}; + +inline bool AStar::Coord2Index(const Eigen::Vector3d &pt, Eigen::Vector3i &idx) const +{ + idx = ((pt - center_) * inv_step_size_ + Eigen::Vector3d(0.5, 0.5, 0.5)).cast() + CENTER_IDX_; + + if (idx(0) < 0 || idx(0) >= POOL_SIZE_(0) || idx(1) < 0 || idx(1) >= POOL_SIZE_(1) || idx(2) < 0 || idx(2) >= POOL_SIZE_(2)) + { + ROS_ERROR("Ran out of pool, index=%d %d %d", idx(0), idx(1), idx(2)); + return false; + } + + return true; +}; + +#endif diff --git a/src/Prometheus/Modules/ego_planner_swarm/path_searching/package.xml b/src/Prometheus/Modules/ego_planner_swarm/path_searching/package.xml new file mode 100755 index 00000000..0394974a --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/path_searching/package.xml @@ -0,0 +1,71 @@ + + + path_searching + 0.0.0 + The path_searching package + + + + + iszhouxin + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + plan_env + roscpp + rospy + std_msgs + plan_env + roscpp + rospy + std_msgs + plan_env + + + + + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/path_searching/src/dyn_a_star.cpp b/src/Prometheus/Modules/ego_planner_swarm/path_searching/src/dyn_a_star.cpp new file mode 100755 index 00000000..744daa94 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/path_searching/src/dyn_a_star.cpp @@ -0,0 +1,260 @@ +#include "path_searching/dyn_a_star.h" + +using namespace std; +using namespace Eigen; + +AStar::~AStar() +{ + for (int i = 0; i < POOL_SIZE_(0); i++) + for (int j = 0; j < POOL_SIZE_(1); j++) + for (int k = 0; k < POOL_SIZE_(2); k++) + delete GridNodeMap_[i][j][k]; +} + +void AStar::initGridMap(GridMap::Ptr occ_map, const Eigen::Vector3i pool_size) +{ + POOL_SIZE_ = pool_size; + CENTER_IDX_ = pool_size / 2; + + GridNodeMap_ = new GridNodePtr **[POOL_SIZE_(0)]; + for (int i = 0; i < POOL_SIZE_(0); i++) + { + GridNodeMap_[i] = new GridNodePtr *[POOL_SIZE_(1)]; + for (int j = 0; j < POOL_SIZE_(1); j++) + { + GridNodeMap_[i][j] = new GridNodePtr[POOL_SIZE_(2)]; + for (int k = 0; k < POOL_SIZE_(2); k++) + { + GridNodeMap_[i][j][k] = new GridNode; + } + } + } + + grid_map_ = occ_map; +} + +double AStar::getDiagHeu(GridNodePtr node1, GridNodePtr node2) +{ + double dx = abs(node1->index(0) - node2->index(0)); + double dy = abs(node1->index(1) - node2->index(1)); + double dz = abs(node1->index(2) - node2->index(2)); + + double h = 0.0; + int diag = min(min(dx, dy), dz); + dx -= diag; + dy -= diag; + dz -= diag; + + if (dx == 0) + { + h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dy, dz) + 1.0 * abs(dy - dz); + } + if (dy == 0) + { + h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dz) + 1.0 * abs(dx - dz); + } + if (dz == 0) + { + h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dy) + 1.0 * abs(dx - dy); + } + return h; +} + +double AStar::getManhHeu(GridNodePtr node1, GridNodePtr node2) +{ + double dx = abs(node1->index(0) - node2->index(0)); + double dy = abs(node1->index(1) - node2->index(1)); + double dz = abs(node1->index(2) - node2->index(2)); + + return dx + dy + dz; +} + +double AStar::getEuclHeu(GridNodePtr node1, GridNodePtr node2) +{ + return (node2->index - node1->index).norm(); +} + +vector AStar::retrievePath(GridNodePtr current) +{ + vector path; + path.push_back(current); + + while (current->cameFrom != NULL) + { + current = current->cameFrom; + path.push_back(current); + } + + return path; +} + +bool AStar::ConvertToIndexAndAdjustStartEndPoints(Vector3d start_pt, Vector3d end_pt, Vector3i &start_idx, Vector3i &end_idx) +{ + if (!Coord2Index(start_pt, start_idx) || !Coord2Index(end_pt, end_idx)) + return false; + + if (checkOccupancy(Index2Coord(start_idx))) + { + //ROS_WARN("Start point is insdide an obstacle."); + do + { + start_pt = (start_pt - end_pt).normalized() * step_size_ + start_pt; + if (!Coord2Index(start_pt, start_idx)) + return false; + } while (checkOccupancy(Index2Coord(start_idx))); + } + + if (checkOccupancy(Index2Coord(end_idx))) + { + //ROS_WARN("End point is insdide an obstacle."); + do + { + end_pt = (end_pt - start_pt).normalized() * step_size_ + end_pt; + if (!Coord2Index(end_pt, end_idx)) + return false; + } while (checkOccupancy(Index2Coord(end_idx))); + } + + return true; +} + +bool AStar::AstarSearch(const double step_size, Vector3d start_pt, Vector3d end_pt) +{ + ros::Time time_1 = ros::Time::now(); + ++rounds_; + + step_size_ = step_size; + inv_step_size_ = 1 / step_size; + center_ = (start_pt + end_pt) / 2; + + Vector3i start_idx, end_idx; + if (!ConvertToIndexAndAdjustStartEndPoints(start_pt, end_pt, start_idx, end_idx)) + { + ROS_ERROR("Unable to handle the initial or end point, force return!"); + return false; + } + + // if ( start_pt(0) > -1 && start_pt(0) < 0 ) + // cout << "start_pt=" << start_pt.transpose() << " end_pt=" << end_pt.transpose() << endl; + + GridNodePtr startPtr = GridNodeMap_[start_idx(0)][start_idx(1)][start_idx(2)]; + GridNodePtr endPtr = GridNodeMap_[end_idx(0)][end_idx(1)][end_idx(2)]; + + std::priority_queue, NodeComparator> empty; + openSet_.swap(empty); + + GridNodePtr neighborPtr = NULL; + GridNodePtr current = NULL; + + startPtr->index = start_idx; + startPtr->rounds = rounds_; + startPtr->gScore = 0; + startPtr->fScore = getHeu(startPtr, endPtr); + startPtr->state = GridNode::OPENSET; //put start node in open set + startPtr->cameFrom = NULL; + openSet_.push(startPtr); //put start in open set + + endPtr->index = end_idx; + + double tentative_gScore; + + int num_iter = 0; + while (!openSet_.empty()) + { + num_iter++; + current = openSet_.top(); + openSet_.pop(); + + // if ( num_iter < 10000 ) + // cout << "current=" << current->index.transpose() << endl; + + if (current->index(0) == endPtr->index(0) && current->index(1) == endPtr->index(1) && current->index(2) == endPtr->index(2)) + { + // ros::Time time_2 = ros::Time::now(); + // printf("\033[34mA star iter:%d, time:%.3f\033[0m\n",num_iter, (time_2 - time_1).toSec()*1000); + // if((time_2 - time_1).toSec() > 0.1) + // ROS_WARN("Time consume in A star path finding is %f", (time_2 - time_1).toSec() ); + gridPath_ = retrievePath(current); + return true; + } + current->state = GridNode::CLOSEDSET; //move current node from open set to closed set. + + for (int dx = -1; dx <= 1; dx++) + for (int dy = -1; dy <= 1; dy++) + for (int dz = -1; dz <= 1; dz++) + { + if (dx == 0 && dy == 0 && dz == 0) + continue; + + Vector3i neighborIdx; + neighborIdx(0) = (current->index)(0) + dx; + neighborIdx(1) = (current->index)(1) + dy; + neighborIdx(2) = (current->index)(2) + dz; + + if (neighborIdx(0) < 1 || neighborIdx(0) >= POOL_SIZE_(0) - 1 || neighborIdx(1) < 1 || neighborIdx(1) >= POOL_SIZE_(1) - 1 || neighborIdx(2) < 1 || neighborIdx(2) >= POOL_SIZE_(2) - 1) + { + continue; + } + + neighborPtr = GridNodeMap_[neighborIdx(0)][neighborIdx(1)][neighborIdx(2)]; + neighborPtr->index = neighborIdx; + + bool flag_explored = neighborPtr->rounds == rounds_; + + if (flag_explored && neighborPtr->state == GridNode::CLOSEDSET) + { + continue; //in closed set. + } + + neighborPtr->rounds = rounds_; + + if (checkOccupancy(Index2Coord(neighborPtr->index))) + { + continue; + } + + double static_cost = sqrt(dx * dx + dy * dy + dz * dz); + tentative_gScore = current->gScore + static_cost; + + if (!flag_explored) + { + //discover a new node + neighborPtr->state = GridNode::OPENSET; + neighborPtr->cameFrom = current; + neighborPtr->gScore = tentative_gScore; + neighborPtr->fScore = tentative_gScore + getHeu(neighborPtr, endPtr); + openSet_.push(neighborPtr); //put neighbor in open set and record it. + } + else if (tentative_gScore < neighborPtr->gScore) + { //in open set and need update + neighborPtr->cameFrom = current; + neighborPtr->gScore = tentative_gScore; + neighborPtr->fScore = tentative_gScore + getHeu(neighborPtr, endPtr); + } + } + ros::Time time_2 = ros::Time::now(); + if ((time_2 - time_1).toSec() > 0.2) + { + ROS_WARN("Failed in A star path searching !!! 0.2 seconds time limit exceeded."); + return false; + } + } + + ros::Time time_2 = ros::Time::now(); + + if ((time_2 - time_1).toSec() > 0.1) + ROS_WARN("Time consume in A star path finding is %.3fs, iter=%d", (time_2 - time_1).toSec(), num_iter); + + return false; +} + +vector AStar::getPath() +{ + vector path; + + for (auto ptr : gridPath_) + path.push_back(Index2Coord(ptr->index)); + + reverse(path.begin(), path.end()); + return path; +} diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_env/CMakeLists.txt b/src/Prometheus/Modules/ego_planner_swarm/plan_env/CMakeLists.txt new file mode 100755 index 00000000..701e9af2 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_env/CMakeLists.txt @@ -0,0 +1,59 @@ +cmake_minimum_required(VERSION 2.8.3) +project(plan_env) + +set(CMAKE_BUILD_TYPE "Release") +ADD_COMPILE_OPTIONS(-std=c++11 ) +ADD_COMPILE_OPTIONS(-std=c++14 ) +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") + +find_package(OpenCV REQUIRED) + + +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + visualization_msgs + cv_bridge + message_filters +) + +find_package(Eigen3 REQUIRED) +find_package(PCL 1.7 REQUIRED) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES plan_env + CATKIN_DEPENDS roscpp std_msgs + DEPENDS OpenCV +# DEPENDS system_lib +) + +include_directories( + SYSTEM + include + ${catkin_INCLUDE_DIRS} + ${Eigen3_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + +link_directories(${PCL_LIBRARY_DIRS}) + +add_library( plan_env + src/grid_map.cpp + src/raycast.cpp + src/obj_predictor.cpp + ) +target_link_libraries( plan_env + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} + ${OpenCV_LIBS} + ) + +add_executable(obj_generator + src/obj_generator.cpp +) +target_link_libraries(obj_generator + ${catkin_LIBRARIES} + ) diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_env/include/plan_env/grid_map.h b/src/Prometheus/Modules/ego_planner_swarm/plan_env/include/plan_env/grid_map.h new file mode 100644 index 00000000..bba2603d --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_env/include/plan_env/grid_map.h @@ -0,0 +1,804 @@ +#ifndef _GRID_MAP_H +#define _GRID_MAP_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#define logit(x) (log((x) / (1 - (x)))) + +using namespace std; + +// voxel hashing +template +struct matrix_hash : std::unary_function { + std::size_t operator()(T const& matrix) const { + size_t seed = 0; + for (size_t i = 0; i < matrix.size(); ++i) { + auto elem = *(matrix.data() + i); + seed ^= std::hash()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + } + return seed; + } +}; + +// constant parameters + +struct MappingParameters { + + /* map properties */ + Eigen::Vector3d map_origin_, map_size_; + Eigen::Vector3d map_min_boundary_, map_max_boundary_; // map range in pos + Eigen::Vector3i map_voxel_num_; // map range in index + Eigen::Vector3d local_update_range_; + double resolution_, resolution_inv_; + double obstacles_inflation_; + string frame_id_; + int pose_type_; + + /* camera parameters */ + double cx_, cy_, fx_, fy_; + + /* time out */ + double odom_depth_timeout_; + + /* depth image projection filtering */ + double depth_filter_maxdist_, depth_filter_mindist_, depth_filter_tolerance_; + int depth_filter_margin_; + bool use_depth_filter_; + double k_depth_scaling_factor_; + int skip_pixel_; + + /* raycasting */ + double p_hit_, p_miss_, p_min_, p_max_, p_occ_; // occupancy probability + double prob_hit_log_, prob_miss_log_, clamp_min_log_, clamp_max_log_, + min_occupancy_log_; // logit of occupancy probability + double min_ray_length_, max_ray_length_; // range of doing raycasting + + /* local map update and clear */ + int local_map_margin_; + + /* visualization and computation time display */ + double visualization_truncate_height_, virtual_ceil_height_, ground_height_, virtual_ceil_yp_, virtual_ceil_yn_; + bool show_occ_time_; + + /* active mapping */ + double unknown_flag_; +}; + +// intermediate mapping data for fusion + +struct MappingData { + // main map data, occupancy of each voxel and Euclidean distance + + std::vector occupancy_buffer_; + std::vector occupancy_buffer_inflate_; + + // camera position and pose data + + Eigen::Vector3d camera_pos_, last_camera_pos_; + Eigen::Matrix3d camera_r_m_, last_camera_r_m_; + Eigen::Matrix4d cam2body_; + + // depth image data + + cv::Mat depth_image_, last_depth_image_; + int image_cnt_; + + // flags of map state + + bool occ_need_update_, local_updated_; + bool has_first_depth_; + bool has_odom_, has_cloud_; + + // odom_depth_timeout_ + ros::Time last_occ_update_time_; + bool flag_depth_odom_timeout_; + bool flag_use_depth_fusion; + + // depth image projected point cloud + + vector proj_points_; + int proj_points_cnt; + + // flag buffers for speeding up raycasting + + vector count_hit_, count_hit_and_miss_; + vector flag_traverse_, flag_rayend_; + char raycast_num_; + queue cache_voxel_; + + // range of updating grid + + Eigen::Vector3i local_bound_min_, local_bound_max_; + + // computation time + + double fuse_time_, max_fuse_time_; + int update_num_; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +class GridMap { +public: + GridMap() {} + ~GridMap() {} + + enum { POSE_STAMPED = 1, ODOMETRY = 2, INVALID_IDX = -10000 }; + + // occupancy map management + void resetBuffer(); + void resetBuffer(Eigen::Vector3d min, Eigen::Vector3d max); + + inline void posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id); + inline void indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos); + inline int toAddress(const Eigen::Vector3i& id); + inline int toAddress(int& x, int& y, int& z); + inline bool isInMap(const Eigen::Vector3d& pos); + inline bool isInMap(const Eigen::Vector3i& idx); + + inline void setOccupancy(Eigen::Vector3d pos, double occ = 1); + inline void setOccupied(Eigen::Vector3d pos); + inline int getOccupancy(Eigen::Vector3d pos); + inline int getOccupancy(Eigen::Vector3i id); + inline int getInflateOccupancy(Eigen::Vector3d pos); + + inline void boundIndex(Eigen::Vector3i& id); + inline bool isUnknown(const Eigen::Vector3i& id); + inline bool isUnknown(const Eigen::Vector3d& pos); + inline bool isKnownFree(const Eigen::Vector3i& id); + inline bool isKnownOccupied(const Eigen::Vector3i& id); + + void initMap(ros::NodeHandle& nh); + + void publishMap(); + void publishMapInflate(bool all_info = false); + + void publishDepth(); + + bool hasDepthObservation(); + bool odomValid(); + void getRegion(Eigen::Vector3d& ori, Eigen::Vector3d& size); + inline double getResolution(); + Eigen::Vector3d getOrigin(); + int getVoxelNum(); + bool getOdomDepthTimeout() { return md_.flag_depth_odom_timeout_; } + + typedef std::shared_ptr Ptr; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +private: + MappingParameters mp_; + MappingData md_; + + // get depth image and camera pose + void depthPoseCallback(const sensor_msgs::ImageConstPtr& img, + const geometry_msgs::PoseStampedConstPtr& pose); + void extrinsicCallback(const nav_msgs::OdometryConstPtr& odom); + void depthOdomCallback(const sensor_msgs::ImageConstPtr& img, const nav_msgs::OdometryConstPtr& odom); + void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& img); + void odomCallback(const nav_msgs::OdometryConstPtr& odom); + + // update occupancy by raycasting + void updateOccupancyCallback(const ros::TimerEvent& /*event*/); + void visCallback(const ros::TimerEvent& /*event*/); + + // main update process + void projectDepthImage(); + void raycastProcess(); + void clearAndInflateLocalMap(); + + inline void inflatePoint(const Eigen::Vector3i& pt, int step, vector& pts); + int setCacheOccupancy(Eigen::Vector3d pos, int occ); + Eigen::Vector3d closetPointInMap(const Eigen::Vector3d& pt, const Eigen::Vector3d& camera_pt); + + // typedef message_filters::sync_policies::ExactTime SyncPolicyImageOdom; typedef + // message_filters::sync_policies::ExactTime SyncPolicyImagePose; + typedef message_filters::sync_policies::ApproximateTime + SyncPolicyImageOdom; + typedef message_filters::sync_policies::ApproximateTime + SyncPolicyImagePose; + typedef shared_ptr> SynchronizerImagePose; + typedef shared_ptr> SynchronizerImageOdom; + + ros::NodeHandle node_; + shared_ptr> depth_sub_; + shared_ptr> pose_sub_; + shared_ptr> odom_sub_; + SynchronizerImagePose sync_image_pose_; + SynchronizerImageOdom sync_image_odom_; + + ros::Subscriber indep_cloud_sub_, indep_odom_sub_, extrinsic_sub_; + ros::Publisher map_pub_, map_inf_pub_; + ros::Timer occ_timer_, vis_timer_; + + // + uniform_real_distribution rand_noise_; + normal_distribution rand_noise2_; + default_random_engine eng_; +}; + +/* ============================== definition of inline function + * ============================== */ + +inline int GridMap::toAddress(const Eigen::Vector3i& id) { + return id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + id(1) * mp_.map_voxel_num_(2) + id(2); +} + +inline int GridMap::toAddress(int& x, int& y, int& z) { + return x * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + y * mp_.map_voxel_num_(2) + z; +} + +inline void GridMap::boundIndex(Eigen::Vector3i& id) { + Eigen::Vector3i id1; + id1(0) = max(min(id(0), mp_.map_voxel_num_(0) - 1), 0); + id1(1) = max(min(id(1), mp_.map_voxel_num_(1) - 1), 0); + id1(2) = max(min(id(2), mp_.map_voxel_num_(2) - 1), 0); + id = id1; +} + +inline bool GridMap::isUnknown(const Eigen::Vector3i& id) { + Eigen::Vector3i id1 = id; + boundIndex(id1); + return md_.occupancy_buffer_[toAddress(id1)] < mp_.clamp_min_log_ - 1e-3; +} + +inline bool GridMap::isUnknown(const Eigen::Vector3d& pos) { + Eigen::Vector3i idc; + posToIndex(pos, idc); + return isUnknown(idc); +} + +inline bool GridMap::isKnownFree(const Eigen::Vector3i& id) { + Eigen::Vector3i id1 = id; + boundIndex(id1); + int adr = toAddress(id1); + + // return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ && + // md_.occupancy_buffer_[adr] < mp_.min_occupancy_log_; + return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ && md_.occupancy_buffer_inflate_[adr] == 0; +} + +inline bool GridMap::isKnownOccupied(const Eigen::Vector3i& id) { + Eigen::Vector3i id1 = id; + boundIndex(id1); + int adr = toAddress(id1); + + return md_.occupancy_buffer_inflate_[adr] == 1; +} + +inline void GridMap::setOccupied(Eigen::Vector3d pos) { + if (!isInMap(pos)) return; + + Eigen::Vector3i id; + posToIndex(pos, id); + + md_.occupancy_buffer_inflate_[id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + + id(1) * mp_.map_voxel_num_(2) + id(2)] = 1; +} + +inline void GridMap::setOccupancy(Eigen::Vector3d pos, double occ) { + if (occ != 1 && occ != 0) { + cout << "occ value error!" << endl; + return; + } + + if (!isInMap(pos)) return; + + Eigen::Vector3i id; + posToIndex(pos, id); + + md_.occupancy_buffer_[toAddress(id)] = occ; +} + +inline int GridMap::getOccupancy(Eigen::Vector3d pos) { + if (!isInMap(pos)) return -1; + + Eigen::Vector3i id; + posToIndex(pos, id); + + return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0; +} + +inline int GridMap::getInflateOccupancy(Eigen::Vector3d pos) { + if (!isInMap(pos)) return -1; + + Eigen::Vector3i id; + posToIndex(pos, id); + + return int(md_.occupancy_buffer_inflate_[toAddress(id)]); +} + +inline int GridMap::getOccupancy(Eigen::Vector3i id) { + if (id(0) < 0 || id(0) >= mp_.map_voxel_num_(0) || id(1) < 0 || id(1) >= mp_.map_voxel_num_(1) || + id(2) < 0 || id(2) >= mp_.map_voxel_num_(2)) + return -1; + + return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0; +} + +inline bool GridMap::isInMap(const Eigen::Vector3d& pos) { + if (pos(0) < mp_.map_min_boundary_(0) + 1e-4 || pos(1) < mp_.map_min_boundary_(1) + 1e-4 || + pos(2) < mp_.map_min_boundary_(2) + 1e-4) { + // cout << "less than min range!" << endl; + return false; + } + if (pos(0) > mp_.map_max_boundary_(0) - 1e-4 || pos(1) > mp_.map_max_boundary_(1) - 1e-4 || + pos(2) > mp_.map_max_boundary_(2) - 1e-4) { + return false; + } + return true; +} + +inline bool GridMap::isInMap(const Eigen::Vector3i& idx) { + if (idx(0) < 0 || idx(1) < 0 || idx(2) < 0) { + return false; + } + if (idx(0) > mp_.map_voxel_num_(0) - 1 || idx(1) > mp_.map_voxel_num_(1) - 1 || + idx(2) > mp_.map_voxel_num_(2) - 1) { + return false; + } + return true; +} + +inline void GridMap::posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id) { + for (int i = 0; i < 3; ++i) id(i) = floor((pos(i) - mp_.map_origin_(i)) * mp_.resolution_inv_); +} + +inline void GridMap::indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos) { + for (int i = 0; i < 3; ++i) pos(i) = (id(i) + 0.5) * mp_.resolution_ + mp_.map_origin_(i); +} + +inline void GridMap::inflatePoint(const Eigen::Vector3i& pt, int step, vector& pts) { + int num = 0; + + /* ---------- + shape inflate ---------- */ + // for (int x = -step; x <= step; ++x) + // { + // if (x == 0) + // continue; + // pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1), pt(2)); + // } + // for (int y = -step; y <= step; ++y) + // { + // if (y == 0) + // continue; + // pts[num++] = Eigen::Vector3i(pt(0), pt(1) + y, pt(2)); + // } + // for (int z = -1; z <= 1; ++z) + // { + // pts[num++] = Eigen::Vector3i(pt(0), pt(1), pt(2) + z); + // } + + /* ---------- all inflate ---------- */ + for (int x = -step; x <= step; ++x) + for (int y = -step; y <= step; ++y) + for (int z = -step; z <= step; ++z) { + pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1) + y, pt(2) + z); + } +} + +inline double GridMap::getResolution() { return mp_.resolution_; } + +#endif + +// #ifndef _GRID_MAP_H +// #define _GRID_MAP_H + +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// #include +// #include +// #include + +// #include +// #include +// #include +// #include + +// #include + +// #define logit(x) (log((x) / (1 - (x)))) + +// using namespace std; + +// // voxel hashing +// template +// struct matrix_hash : std::unary_function { +// std::size_t operator()(T const& matrix) const { +// size_t seed = 0; +// for (size_t i = 0; i < matrix.size(); ++i) { +// auto elem = *(matrix.data() + i); +// seed ^= std::hash()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2); +// } +// return seed; +// } +// }; + +// // constant parameters + +// struct MappingParameters { + +// /* map properties */ +// Eigen::Vector3d map_origin_, map_size_; +// Eigen::Vector3d map_min_boundary_, map_max_boundary_; // map range in pos +// Eigen::Vector3i map_voxel_num_; // map range in index +// Eigen::Vector3d local_update_range_; +// double resolution_, resolution_inv_; +// double obstacles_inflation_; +// string frame_id_; +// int pose_type_; + +// /* camera parameters */ +// double cx_, cy_, fx_, fy_; + +// /* depth image projection filtering */ +// double depth_filter_maxdist_, depth_filter_mindist_, depth_filter_tolerance_; +// int depth_filter_margin_; +// bool use_depth_filter_; +// double k_depth_scaling_factor_; +// int skip_pixel_; + +// /* raycasting */ +// double p_hit_, p_miss_, p_min_, p_max_, p_occ_; // occupancy probability +// double prob_hit_log_, prob_miss_log_, clamp_min_log_, clamp_max_log_, +// min_occupancy_log_; // logit of occupancy probability +// double min_ray_length_, max_ray_length_; // range of doing raycasting + +// /* local map update and clear */ +// int local_map_margin_; + +// /* visualization and computation time display */ +// double visualization_truncate_height_, virtual_ceil_height_, ground_height_; +// bool show_occ_time_; + +// /* active mapping */ +// double unknown_flag_; +// }; + +// // intermediate mapping data for fusion + +// struct MappingData { +// // main map data, occupancy of each voxel and Euclidean distance + +// std::vector occupancy_buffer_; +// std::vector occupancy_buffer_inflate_; + +// // camera position and pose data + +// Eigen::Vector3d camera_pos_, last_camera_pos_; +// Eigen::Quaterniond camera_q_, last_camera_q_; + +// // depth image data + +// cv::Mat depth_image_, last_depth_image_; +// int image_cnt_; + +// // flags of map state + +// bool occ_need_update_, local_updated_; +// bool has_first_depth_; +// bool has_odom_, has_cloud_; + +// // depth image projected point cloud + +// vector proj_points_; +// int proj_points_cnt; + +// // flag buffers for speeding up raycasting + +// vector count_hit_, count_hit_and_miss_; +// vector flag_traverse_, flag_rayend_; +// char raycast_num_; +// queue cache_voxel_; + +// // range of updating grid + +// Eigen::Vector3i local_bound_min_, local_bound_max_; + +// // computation time + +// double fuse_time_, max_fuse_time_; +// int update_num_; + +// EIGEN_MAKE_ALIGNED_OPERATOR_NEW +// }; + +// class GridMap { +// public: +// GridMap() {} +// ~GridMap() {} + +// enum { POSE_STAMPED = 1, ODOMETRY = 2, INVALID_IDX = -10000 }; + +// // occupancy map management +// void resetBuffer(); +// void resetBuffer(Eigen::Vector3d min, Eigen::Vector3d max); + +// inline void posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id); +// inline void indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos); +// inline int toAddress(const Eigen::Vector3i& id); +// inline int toAddress(int& x, int& y, int& z); +// inline bool isInMap(const Eigen::Vector3d& pos); +// inline bool isInMap(const Eigen::Vector3i& idx); + +// inline void setOccupancy(Eigen::Vector3d pos, double occ = 1); +// inline void setOccupied(Eigen::Vector3d pos); +// inline int getOccupancy(Eigen::Vector3d pos); +// inline int getOccupancy(Eigen::Vector3i id); +// inline int getInflateOccupancy(Eigen::Vector3d pos); + +// inline void boundIndex(Eigen::Vector3i& id); +// inline bool isUnknown(const Eigen::Vector3i& id); +// inline bool isUnknown(const Eigen::Vector3d& pos); +// inline bool isKnownFree(const Eigen::Vector3i& id); +// inline bool isKnownOccupied(const Eigen::Vector3i& id); + +// void initMap(ros::NodeHandle& nh); + +// void publishMap(); +// void publishMapInflate(bool all_info = false); + +// void publishUnknown(); +// void publishDepth(); + +// bool hasDepthObservation(); +// bool odomValid(); +// void getRegion(Eigen::Vector3d& ori, Eigen::Vector3d& size); +// inline double getResolution(); +// Eigen::Vector3d getOrigin(); +// int getVoxelNum(); + +// typedef std::shared_ptr Ptr; + +// EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +// private: +// MappingParameters mp_; +// MappingData md_; + +// // get depth image and camera pose +// void depthPoseCallback(const sensor_msgs::ImageConstPtr& img, +// const geometry_msgs::PoseStampedConstPtr& pose); +// void depthOdomCallback(const sensor_msgs::ImageConstPtr& img, const nav_msgs::OdometryConstPtr& odom); +// void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& img); +// void odomCallback(const nav_msgs::OdometryConstPtr& odom); + +// // update occupancy by raycasting +// void updateOccupancyCallback(const ros::TimerEvent& /*event*/); +// void visCallback(const ros::TimerEvent& /*event*/); + +// // main update process +// void projectDepthImage(); +// void raycastProcess(); +// void clearAndInflateLocalMap(); + +// inline void inflatePoint(const Eigen::Vector3i& pt, int step, vector& pts); +// int setCacheOccupancy(Eigen::Vector3d pos, int occ); +// Eigen::Vector3d closetPointInMap(const Eigen::Vector3d& pt, const Eigen::Vector3d& camera_pt); + +// // typedef message_filters::sync_policies::ExactTime SyncPolicyImageOdom; typedef +// // message_filters::sync_policies::ExactTime SyncPolicyImagePose; +// typedef message_filters::sync_policies::ApproximateTime +// SyncPolicyImageOdom; +// typedef message_filters::sync_policies::ApproximateTime +// SyncPolicyImagePose; +// typedef shared_ptr> SynchronizerImagePose; +// typedef shared_ptr> SynchronizerImageOdom; + +// ros::NodeHandle node_; +// shared_ptr> depth_sub_; +// shared_ptr> pose_sub_; +// shared_ptr> odom_sub_; +// SynchronizerImagePose sync_image_pose_; +// SynchronizerImageOdom sync_image_odom_; + +// ros::Subscriber indep_cloud_sub_, indep_odom_sub_; +// ros::Publisher map_pub_, map_inf_pub_; +// ros::Publisher unknown_pub_; +// ros::Timer occ_timer_, vis_timer_; + +// // +// uniform_real_distribution rand_noise_; +// normal_distribution rand_noise2_; +// default_random_engine eng_; +// }; + +// /* ============================== definition of inline function +// * ============================== */ + +// inline int GridMap::toAddress(const Eigen::Vector3i& id) { +// return id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + id(1) * mp_.map_voxel_num_(2) + id(2); +// } + +// inline int GridMap::toAddress(int& x, int& y, int& z) { +// return x * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + y * mp_.map_voxel_num_(2) + z; +// } + +// inline void GridMap::boundIndex(Eigen::Vector3i& id) { +// Eigen::Vector3i id1; +// id1(0) = max(min(id(0), mp_.map_voxel_num_(0) - 1), 0); +// id1(1) = max(min(id(1), mp_.map_voxel_num_(1) - 1), 0); +// id1(2) = max(min(id(2), mp_.map_voxel_num_(2) - 1), 0); +// id = id1; +// } + +// inline bool GridMap::isUnknown(const Eigen::Vector3i& id) { +// Eigen::Vector3i id1 = id; +// boundIndex(id1); +// return md_.occupancy_buffer_[toAddress(id1)] < mp_.clamp_min_log_ - 1e-3; +// } + +// inline bool GridMap::isUnknown(const Eigen::Vector3d& pos) { +// Eigen::Vector3i idc; +// posToIndex(pos, idc); +// return isUnknown(idc); +// } + +// inline bool GridMap::isKnownFree(const Eigen::Vector3i& id) { +// Eigen::Vector3i id1 = id; +// boundIndex(id1); +// int adr = toAddress(id1); + +// // return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ && +// // md_.occupancy_buffer_[adr] < mp_.min_occupancy_log_; +// return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ && md_.occupancy_buffer_inflate_[adr] == 0; +// } + +// inline bool GridMap::isKnownOccupied(const Eigen::Vector3i& id) { +// Eigen::Vector3i id1 = id; +// boundIndex(id1); +// int adr = toAddress(id1); + +// return md_.occupancy_buffer_inflate_[adr] == 1; +// } + +// inline void GridMap::setOccupied(Eigen::Vector3d pos) { +// if (!isInMap(pos)) return; + +// Eigen::Vector3i id; +// posToIndex(pos, id); + +// md_.occupancy_buffer_inflate_[id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + +// id(1) * mp_.map_voxel_num_(2) + id(2)] = 1; +// } + +// inline void GridMap::setOccupancy(Eigen::Vector3d pos, double occ) { +// if (occ != 1 && occ != 0) { +// cout << "occ value error!" << endl; +// return; +// } + +// if (!isInMap(pos)) return; + +// Eigen::Vector3i id; +// posToIndex(pos, id); + +// md_.occupancy_buffer_[toAddress(id)] = occ; +// } + +// inline int GridMap::getOccupancy(Eigen::Vector3d pos) { +// if (!isInMap(pos)) return -1; + +// Eigen::Vector3i id; +// posToIndex(pos, id); + +// return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0; +// } + +// inline int GridMap::getInflateOccupancy(Eigen::Vector3d pos) { +// if (!isInMap(pos)) return -1; + +// Eigen::Vector3i id; +// posToIndex(pos, id); + +// return int(md_.occupancy_buffer_inflate_[toAddress(id)]); +// } + +// inline int GridMap::getOccupancy(Eigen::Vector3i id) { +// if (id(0) < 0 || id(0) >= mp_.map_voxel_num_(0) || id(1) < 0 || id(1) >= mp_.map_voxel_num_(1) || +// id(2) < 0 || id(2) >= mp_.map_voxel_num_(2)) +// return -1; + +// return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0; +// } + +// inline bool GridMap::isInMap(const Eigen::Vector3d& pos) { +// if (pos(0) < mp_.map_min_boundary_(0) + 1e-4 || pos(1) < mp_.map_min_boundary_(1) + 1e-4 || +// pos(2) < mp_.map_min_boundary_(2) + 1e-4) { +// // cout << "less than min range!" << endl; +// return false; +// } +// if (pos(0) > mp_.map_max_boundary_(0) - 1e-4 || pos(1) > mp_.map_max_boundary_(1) - 1e-4 || +// pos(2) > mp_.map_max_boundary_(2) - 1e-4) { +// return false; +// } +// return true; +// } + +// inline bool GridMap::isInMap(const Eigen::Vector3i& idx) { +// if (idx(0) < 0 || idx(1) < 0 || idx(2) < 0) { +// return false; +// } +// if (idx(0) > mp_.map_voxel_num_(0) - 1 || idx(1) > mp_.map_voxel_num_(1) - 1 || +// idx(2) > mp_.map_voxel_num_(2) - 1) { +// return false; +// } +// return true; +// } + +// inline void GridMap::posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id) { +// for (int i = 0; i < 3; ++i) id(i) = floor((pos(i) - mp_.map_origin_(i)) * mp_.resolution_inv_); +// } + +// inline void GridMap::indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos) { +// for (int i = 0; i < 3; ++i) pos(i) = (id(i) + 0.5) * mp_.resolution_ + mp_.map_origin_(i); +// } + +// inline void GridMap::inflatePoint(const Eigen::Vector3i& pt, int step, vector& pts) { +// int num = 0; + +// /* ---------- + shape inflate ---------- */ +// // for (int x = -step; x <= step; ++x) +// // { +// // if (x == 0) +// // continue; +// // pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1), pt(2)); +// // } +// // for (int y = -step; y <= step; ++y) +// // { +// // if (y == 0) +// // continue; +// // pts[num++] = Eigen::Vector3i(pt(0), pt(1) + y, pt(2)); +// // } +// // for (int z = -1; z <= 1; ++z) +// // { +// // pts[num++] = Eigen::Vector3i(pt(0), pt(1), pt(2) + z); +// // } + +// /* ---------- all inflate ---------- */ +// for (int x = -step; x <= step; ++x) +// for (int y = -step; y <= step; ++y) +// for (int z = -step; z <= step; ++z) { +// pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1) + y, pt(2) + z); +// } +// } + +// inline double GridMap::getResolution() { return mp_.resolution_; } + +// #endif \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_env/include/plan_env/linear_obj_model.hpp b/src/Prometheus/Modules/ego_planner_swarm/plan_env/include/plan_env/linear_obj_model.hpp new file mode 100644 index 00000000..574e3b00 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_env/include/plan_env/linear_obj_model.hpp @@ -0,0 +1,267 @@ +/** +* This file is part of Fast-Planner. +* +* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, +* Developed by Boyu Zhou , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* Fast-Planner is free software: you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Fast-Planner is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with Fast-Planner. If not, see . +*/ + + + +#ifndef _LINEAR_OBJ_MODEL_H_ +#define _LINEAR_OBJ_MODEL_H_ + +#include +#include +#include + +class LinearObjModel { +private: + bool last_out_bound_{false}; + int input_type_; + /* data */ +public: + LinearObjModel(/* args */); + ~LinearObjModel(); + + void initialize(Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d a, double yaw, double yaw_dot, + Eigen::Vector3d color, Eigen::Vector3d scale, int input_type); + + void setLimits(Eigen::Vector3d bound, Eigen::Vector2d vel, Eigen::Vector2d acc); + + void update(double dt); // linear trippler integrator model + + static bool collide(LinearObjModel& obj1, LinearObjModel& obj2); + + // void setInput(Eigen::Vector3d acc) { + // acc_ = acc; + // } + void setInput(Eigen::Vector3d vel) { + vel_ = vel; + } + + void setYawDot(double yaw_dot) { + yaw_dot_ = yaw_dot; + } + + Eigen::Vector3d getPosition() { + return pos_; + } + void setPosition(Eigen::Vector3d pos) { + pos_ = pos; + } + + Eigen::Vector3d getVelocity() { + return vel_; + } + + void setVelocity(double x, double y, double z) { + vel_ = Eigen::Vector3d(x, y, z); + } + + Eigen::Vector3d getColor() { + return color_; + } + Eigen::Vector3d getScale() { + return scale_; + } + + double getYaw() { + return yaw_; + } + +private: + Eigen::Vector3d pos_, vel_, acc_; + Eigen::Vector3d color_, scale_; + double yaw_, yaw_dot_; + + Eigen::Vector3d bound_; + Eigen::Vector2d limit_v_, limit_a_; +}; + +LinearObjModel::LinearObjModel(/* args */) { +} + +LinearObjModel::~LinearObjModel() { +} + +void LinearObjModel::initialize(Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d a, double yaw, + double yaw_dot, Eigen::Vector3d color, Eigen::Vector3d scale, int input_type) { + pos_ = p; + vel_ = v; + acc_ = a; + color_ = color; + scale_ = scale; + input_type_ = input_type; + + yaw_ = yaw; + yaw_dot_ = yaw_dot; +} + +void LinearObjModel::setLimits(Eigen::Vector3d bound, Eigen::Vector2d vel, Eigen::Vector2d acc) { + bound_ = bound; + limit_v_ = vel; + limit_a_ = acc; +} + +void LinearObjModel::update(double dt) { + Eigen::Vector3d p0, v0, a0; + p0 = pos_, v0 = vel_, a0 = acc_; + //std::cout << v0.transpose() << std::endl; + + /* ---------- use acc as input ---------- */ + if ( input_type_ == 2 ) + { + vel_ = v0 + acc_ * dt; + for (int i = 0; i < 3; ++i) + { + if (vel_(i) > 0) vel_(i) = std::max(limit_v_(0), std::min(vel_(i), + limit_v_(1))); + if (vel_(i) <= 0) vel_(i) = std::max(-limit_v_(1), std::min(vel_(i), + -limit_v_(0))); + } + + pos_ = p0 + v0 * dt + 0.5 * acc_ * pow(dt, 2); + + /* ---------- reflect acc when collide with bound ---------- */ + if ( pos_(0) <= bound_(0) && pos_(0) >= -bound_(0) && + pos_(1) <= bound_(1) && pos_(1) >= -bound_(1) && + pos_(2) <= bound_(2) && pos_(2) >= 0 + ) + { + last_out_bound_ = false; + } + else if ( !last_out_bound_ ) + { + last_out_bound_ = true; + + // if ( pos_(0) > bound_(0) || pos_(0) < -bound_(0) ) acc_(0) = -acc_(0); + // if ( pos_(1) > bound_(1) || pos_(1) < -bound_(1) ) acc_(1) = -acc_(1); + // if ( pos_(2) > bound_(2) || pos_(2) < -bound_(2) ) acc_(2) = -acc_(2); + acc_ = -acc_; + //ROS_ERROR("AAAAAAAAAAAAAAAAAAa"); + } + } + // for (int i = 0; i < 2; ++i) + // { + // pos_(i) = std::min(pos_(i), bound_(i)); + // pos_(i) = std::max(pos_(i), -bound_(i)); + // } + // pos_(2) = std::min(pos_(2), bound_(2)); + // pos_(2) = std::max(pos_(2), 0.0); + + /* ---------- use vel as input ---------- */ + else if ( input_type_ == 1 ) + { + pos_ = p0 + v0 * dt; + for (int i = 0; i < 2; ++i) { + pos_(i) = std::min(pos_(i), bound_(i)); + pos_(i) = std::max(pos_(i), -bound_(i)); + } + pos_(2) = std::min(pos_(2), bound_(2)); + pos_(2) = std::max(pos_(2), 0.0); + + yaw_ += yaw_dot_ * dt; + + const double PI = 3.1415926; + if (yaw_ > 2 * PI) yaw_ -= 2 * PI; + + const double tol = 0.1; + if (pos_(0) > bound_(0) - tol) { + pos_(0) = bound_(0) - tol; + vel_(0) = -vel_(0); + } + if (pos_(0) < -bound_(0) + tol) { + pos_(0) = -bound_(0) + tol; + vel_(0) = -vel_(0); + } + + if (pos_(1) > bound_(1) - tol) { + pos_(1) = bound_(1) - tol; + vel_(1) = -vel_(1); + } + if (pos_(1) < -bound_(1) + tol) { + pos_(1) = -bound_(1) + tol; + vel_(1) = -vel_(1); + } + + if (pos_(2) > bound_(2) - tol) { + pos_(2) = bound_(2) - tol; + vel_(2) = -vel_(2); + } + if (pos_(2) < tol) { + pos_(2) = tol; + vel_(2) = -vel_(2); + } + } + + // /* ---------- reflect when collide with bound ---------- */ + + + //std::cout << pos_.transpose() << " " << bound_.transpose() << std::endl; +} + +bool LinearObjModel::collide(LinearObjModel& obj1, LinearObjModel& obj2) { + Eigen::Vector3d pos1, pos2, vel1, vel2, scale1, scale2; + pos1 = obj1.getPosition(); + vel1 = obj1.getVelocity(); + scale1 = obj1.getScale(); + + pos2 = obj2.getPosition(); + vel2 = obj2.getVelocity(); + scale2 = obj2.getScale(); + + /* ---------- collide ---------- */ + bool collide = fabs(pos1(0) - pos2(0)) < 0.5 * (scale1(0) + scale2(0)) && + fabs(pos1(1) - pos2(1)) < 0.5 * (scale1(1) + scale2(1)) && + fabs(pos1(2) - pos2(2)) < 0.5 * (scale1(2) + scale2(2)); + + if (collide) { + double tol[3]; + tol[0] = 0.5 * (scale1(0) + scale2(0)) - fabs(pos1(0) - pos2(0)); + tol[1] = 0.5 * (scale1(1) + scale2(1)) - fabs(pos1(1) - pos2(1)); + tol[2] = 0.5 * (scale1(2) + scale2(2)) - fabs(pos1(2) - pos2(2)); + + for (int i = 0; i < 3; ++i) { + if (tol[i] < tol[(i + 1) % 3] && tol[i] < tol[(i + 2) % 3]) { + vel1(i) = -vel1(i); + vel2(i) = -vel2(i); + obj1.setVelocity(vel1(0), vel1(1), vel1(2)); + obj2.setVelocity(vel2(0), vel2(1), vel2(2)); + + if (pos1(i) >= pos2(i)) { + pos1(i) += tol[i]; + pos2(i) -= tol[i]; + } else { + pos1(i) -= tol[i]; + pos2(i) += tol[i]; + } + obj1.setPosition(pos1); + obj2.setPosition(pos2); + + break; + } + } + + return true; + } else { + return false; + } +} + +#endif \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_env/include/plan_env/obj_predictor.h b/src/Prometheus/Modules/ego_planner_swarm/plan_env/include/plan_env/obj_predictor.h new file mode 100644 index 00000000..fb2e86bc --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_env/include/plan_env/obj_predictor.h @@ -0,0 +1,176 @@ +/** +* This file is part of Fast-Planner. +* +* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, +* Developed by Boyu Zhou , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* Fast-Planner is free software: you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Fast-Planner is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with Fast-Planner. If not, see . +*/ + + + +#ifndef _OBJ_PREDICTOR_H_ +#define _OBJ_PREDICTOR_H_ + +#include +#include +#include +#include +#include +#include +#include + +using std::cout; +using std::endl; +using std::list; +using std::shared_ptr; +using std::unique_ptr; +using std::vector; + +namespace fast_planner { +class PolynomialPrediction; +typedef shared_ptr> ObjPrediction; +typedef shared_ptr> ObjScale; + +/* ========== prediction polynomial ========== */ +class PolynomialPrediction { +private: + vector> polys; + double t1, t2; // start / end + ros::Time global_start_time_; + +public: + PolynomialPrediction(/* args */) { + } + ~PolynomialPrediction() { + } + + void setPolynomial(vector>& pls) { + polys = pls; + } + void setTime(double t1, double t2) { + this->t1 = t1; + this->t2 = t2; + } + void setGlobalStartTime(ros::Time global_start_time) { + global_start_time_ = global_start_time; + } + + bool valid() { + return polys.size() == 3; + } + + /* note that t should be in [t1, t2] */ + Eigen::Vector3d evaluate(double t) { + Eigen::Matrix tv; + tv << 1.0, pow(t, 1), pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5); + + Eigen::Vector3d pt; + pt(0) = tv.dot(polys[0]), pt(1) = tv.dot(polys[1]), pt(2) = tv.dot(polys[2]); + + return pt; + } + + Eigen::Vector3d evaluateConstVel(double t) { + Eigen::Matrix tv; + tv << 1.0, pow(t-global_start_time_.toSec(), 1); + + // cout << t-global_start_time_.toSec() << endl; + + Eigen::Vector3d pt; + pt(0) = tv.dot(polys[0].head(2)), pt(1) = tv.dot(polys[1].head(2)), pt(2) = tv.dot(polys[2].head(2)); + + return pt; + } +}; + +/* ========== subscribe and record object history ========== */ +class ObjHistory { +public: + int skip_num_; + int queue_size_; + ros::Time global_start_time_; + + ObjHistory() { + } + ~ObjHistory() { + } + + void init(int id, int skip_num, int queue_size, ros::Time global_start_time); + + void poseCallback(const geometry_msgs::PoseStampedConstPtr& msg); + + void clear() { + history_.clear(); + } + + void getHistory(list& his) { + his = history_; + } + +private: + list history_; // x,y,z;t + int skip_; + int obj_idx_; + Eigen::Vector3d scale_; +}; + +/* ========== predict future trajectory using history ========== */ +class ObjPredictor { +private: + ros::NodeHandle node_handle_; + + int obj_num_; + double lambda_; + double predict_rate_; + + vector pose_subs_; + ros::Subscriber marker_sub_; + ros::Timer predict_timer_; + vector> obj_histories_; + + /* share data with planner */ + ObjPrediction predict_trajs_; + ObjScale obj_scale_; + vector scale_init_; + + void markerCallback(const visualization_msgs::MarkerConstPtr& msg); + + void predictCallback(const ros::TimerEvent& e); + void predictPolyFit(); + void predictConstVel(); + +public: + ObjPredictor(/* args */); + ObjPredictor(ros::NodeHandle& node); + ~ObjPredictor(); + + void init(); + + ObjPrediction getPredictionTraj(); + ObjScale getObjScale(); + int getObjNums() {return obj_num_;} + + Eigen::Vector3d evaluatePoly(int obs_id, double time); + Eigen::Vector3d evaluateConstVel(int obs_id, double time); + + typedef shared_ptr Ptr; +}; + +} // namespace fast_planner + +#endif \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_env/include/plan_env/raycast.h b/src/Prometheus/Modules/ego_planner_swarm/plan_env/include/plan_env/raycast.h new file mode 100644 index 00000000..23e08766 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_env/include/plan_env/raycast.h @@ -0,0 +1,63 @@ +#ifndef RAYCAST_H_ +#define RAYCAST_H_ + +#include +#include + +double signum(double x); + +double mod(double value, double modulus); + +double intbound(double s, double ds); + +void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min, + const Eigen::Vector3d& max, int& output_points_cnt, Eigen::Vector3d* output); + +void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min, + const Eigen::Vector3d& max, std::vector* output); + +class RayCaster { +private: + /* data */ + Eigen::Vector3d start_; + Eigen::Vector3d end_; + Eigen::Vector3d direction_; + Eigen::Vector3d min_; + Eigen::Vector3d max_; + int x_; + int y_; + int z_; + int endX_; + int endY_; + int endZ_; + double maxDist_; + double dx_; + double dy_; + double dz_; + int stepX_; + int stepY_; + int stepZ_; + double tMaxX_; + double tMaxY_; + double tMaxZ_; + double tDeltaX_; + double tDeltaY_; + double tDeltaZ_; + double dist_; + + int step_num_; + +public: + RayCaster(/* args */) { + } + ~RayCaster() { + } + + bool setInput(const Eigen::Vector3d& start, + const Eigen::Vector3d& end /* , const Eigen::Vector3d& min, + const Eigen::Vector3d& max */); + + bool step(Eigen::Vector3d& ray_pt); +}; + +#endif // RAYCAST_H_ \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_env/package.xml b/src/Prometheus/Modules/ego_planner_swarm/plan_env/package.xml new file mode 100755 index 00000000..7a55ca11 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_env/package.xml @@ -0,0 +1,68 @@ + + + plan_env + 0.0.0 + The plan_env package + + + + + iszhouxin + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_env/src/grid_map.cpp b/src/Prometheus/Modules/ego_planner_swarm/plan_env/src/grid_map.cpp new file mode 100644 index 00000000..c9ef59b2 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_env/src/grid_map.cpp @@ -0,0 +1,1109 @@ +#include "plan_env/grid_map.h" + +// #define current_img_ md_.depth_image_[image_cnt_ & 1] +// #define last_img_ md_.depth_image_[!(image_cnt_ & 1)] + +void GridMap::initMap(ros::NodeHandle &nh) +{ + node_ = nh; + + /* get parameter */ + int uav_id; + double x_size, y_size, z_size, x_origin, y_origin; + // 分辨率 + node_.param("grid_map/uav_id", uav_id, 1); + node_.param("grid_map/resolution", mp_.resolution_, -1.0); + node_.param("grid_map/map_size_x", x_size, -1.0); + node_.param("grid_map/map_size_y", y_size, -1.0); + node_.param("grid_map/map_size_z", z_size, -1.0); + node_.param("grid_map/map_origin_x", x_origin, -1.0); + node_.param("grid_map/map_origin_y", y_origin, -1.0); + node_.param("grid_map/local_update_range_x", mp_.local_update_range_(0), -1.0); + node_.param("grid_map/local_update_range_y", mp_.local_update_range_(1), -1.0); + node_.param("grid_map/local_update_range_z", mp_.local_update_range_(2), -1.0); + // 膨胀距离 + node_.param("grid_map/obstacles_inflation", mp_.obstacles_inflation_, -1.0); + // 相机参数 + node_.param("grid_map/fx", mp_.fx_, -1.0); + node_.param("grid_map/fy", mp_.fy_, -1.0); + node_.param("grid_map/cx", mp_.cx_, -1.0); + node_.param("grid_map/cy", mp_.cy_, -1.0); + // depth filter + node_.param("grid_map/use_depth_filter", mp_.use_depth_filter_, true); + node_.param("grid_map/depth_filter_tolerance", mp_.depth_filter_tolerance_, -1.0); + node_.param("grid_map/depth_filter_maxdist", mp_.depth_filter_maxdist_, -1.0); + node_.param("grid_map/depth_filter_mindist", mp_.depth_filter_mindist_, -1.0); + node_.param("grid_map/depth_filter_margin", mp_.depth_filter_margin_, -1); + node_.param("grid_map/k_depth_scaling_factor", mp_.k_depth_scaling_factor_, -1.0); + node_.param("grid_map/skip_pixel", mp_.skip_pixel_, -1); + // local fusion + node_.param("grid_map/p_hit", mp_.p_hit_, 0.70); + node_.param("grid_map/p_miss", mp_.p_miss_, 0.35); + node_.param("grid_map/p_min", mp_.p_min_, 0.12); + node_.param("grid_map/p_max", mp_.p_max_, 0.97); + node_.param("grid_map/p_occ", mp_.p_occ_, 0.80); + node_.param("grid_map/min_ray_length", mp_.min_ray_length_, -0.1); + node_.param("grid_map/max_ray_length", mp_.max_ray_length_, -0.1); + + node_.param("grid_map/visualization_truncate_height", mp_.visualization_truncate_height_, -0.1); + node_.param("grid_map/virtual_ceil_height", mp_.virtual_ceil_height_, -0.1); + node_.param("grid_map/virtual_ceil_yp", mp_.virtual_ceil_yp_, -0.1); + node_.param("grid_map/virtual_ceil_yn", mp_.virtual_ceil_yn_, -0.1); + + node_.param("grid_map/show_occ_time", mp_.show_occ_time_, false); + node_.param("grid_map/pose_type", mp_.pose_type_, 1); + + node_.param("grid_map/frame_id", mp_.frame_id_, string("world")); + // 地图边缘 + node_.param("grid_map/local_map_margin", mp_.local_map_margin_, 1); + // 地面高度 + node_.param("grid_map/ground_height", mp_.ground_height_, 0.0); + + node_.param("grid_map/odom_depth_timeout", mp_.odom_depth_timeout_, 1.0); + + if( mp_.virtual_ceil_height_ - mp_.ground_height_ > z_size) + { + // 天花板高度 = 地面高度 + z_size + mp_.virtual_ceil_height_ = mp_.ground_height_ + z_size; + } + + mp_.resolution_inv_ = 1 / mp_.resolution_; + //todo: different map origin + if(x_origin < -1.0+1e-2 && x_origin > -1.0-1e-2) mp_.map_origin_ = Eigen::Vector3d(-x_size / 2.0, -y_size / 2.0, mp_.ground_height_); + else mp_.map_origin_ = Eigen::Vector3d(x_origin, y_origin, mp_.ground_height_); + mp_.map_size_ = Eigen::Vector3d(x_size, y_size, z_size); + + //logit(x) (log((x) / (1 - (x)))) + mp_.prob_hit_log_ = logit(mp_.p_hit_); + mp_.prob_miss_log_ = logit(mp_.p_miss_); + mp_.clamp_min_log_ = logit(mp_.p_min_); + mp_.clamp_max_log_ = logit(mp_.p_max_); + mp_.min_occupancy_log_ = logit(mp_.p_occ_); + mp_.unknown_flag_ = 0.01; + + // cout << "hit: " << mp_.prob_hit_log_ << endl; + // cout << "miss: " << mp_.prob_miss_log_ << endl; + // cout << "min log: " << mp_.clamp_min_log_ << endl; + // cout << "max: " << mp_.clamp_max_log_ << endl; + // cout << "thresh log: " << mp_.min_occupancy_log_ << endl; + + for (int i = 0; i < 3; ++i) + mp_.map_voxel_num_(i) = ceil(mp_.map_size_(i) / mp_.resolution_); + + mp_.map_min_boundary_ = mp_.map_origin_; + mp_.map_max_boundary_ = mp_.map_origin_ + mp_.map_size_; + + // initialize data buffers + + int buffer_size = mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2); + + md_.occupancy_buffer_ = vector(buffer_size, mp_.clamp_min_log_ - mp_.unknown_flag_); + md_.occupancy_buffer_inflate_ = vector(buffer_size, 0); + + md_.count_hit_and_miss_ = vector(buffer_size, 0); + md_.count_hit_ = vector(buffer_size, 0); + md_.flag_rayend_ = vector(buffer_size, -1); + md_.flag_traverse_ = vector(buffer_size, -1); + + md_.raycast_num_ = 0; + + md_.proj_points_.resize(640 * 480 / mp_.skip_pixel_ / mp_.skip_pixel_); + md_.proj_points_cnt = 0; + + // 相机外参数 + md_.cam2body_ << 0.0, 0.0, 1.0, 0.0, + -1.0, 0.0, 0.0, 0.0, + 0.0, -1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0; + + /* init callback */ + + depth_sub_.reset(new message_filters::Subscriber(node_, "grid_map/depth", 50)); + // 相机外参 + extrinsic_sub_ = node_.subscribe("/vins_estimator/extrinsic", 10, &GridMap::extrinsicCallback, this); //sub + + if (mp_.pose_type_ == POSE_STAMPED) + { + pose_sub_.reset( + new message_filters::Subscriber(node_, "grid_map/pose", 25)); + + sync_image_pose_.reset(new message_filters::Synchronizer( + SyncPolicyImagePose(100), *depth_sub_, *pose_sub_)); + sync_image_pose_->registerCallback(boost::bind(&GridMap::depthPoseCallback, this, _1, _2)); + } + else if (mp_.pose_type_ == ODOMETRY) + { + odom_sub_.reset(new message_filters::Subscriber(node_, "grid_map/odom", 100, ros::TransportHints().tcpNoDelay())); + + sync_image_odom_.reset(new message_filters::Synchronizer( + SyncPolicyImageOdom(100), *depth_sub_, *odom_sub_)); + sync_image_odom_->registerCallback(boost::bind(&GridMap::depthOdomCallback, this, _1, _2)); + } + + // use odometry and point cloud + indep_cloud_sub_ = + node_.subscribe("grid_map/cloud", 10, &GridMap::cloudCallback, this); + indep_odom_sub_ = + node_.subscribe("grid_map/odom", 10, &GridMap::odomCallback, this); + + occ_timer_ = node_.createTimer(ros::Duration(0.05), &GridMap::updateOccupancyCallback, this); + vis_timer_ = node_.createTimer(ros::Duration(0.11), &GridMap::visCallback, this); + + map_pub_ = node_.advertise("grid_map/occupancy", 10); + map_inf_pub_ = node_.advertise("grid_map/occupancy_inflate", 10); + + md_.occ_need_update_ = false; + md_.local_updated_ = false; + md_.has_first_depth_ = false; + md_.has_odom_ = false; + md_.has_cloud_ = false; + md_.image_cnt_ = 0; + md_.last_occ_update_time_.fromSec(0); + + md_.fuse_time_ = 0.0; + md_.update_num_ = 0; + md_.max_fuse_time_ = 0.0; + + md_.flag_depth_odom_timeout_ = false; + md_.flag_use_depth_fusion = false; + + // rand_noise_ = uniform_real_distribution(-0.2, 0.2); + // rand_noise2_ = normal_distribution(0, 0.2); + // random_device rd; + // eng_ = default_random_engine(rd()); +} + +void GridMap::resetBuffer() +{ + Eigen::Vector3d min_pos = mp_.map_min_boundary_; + Eigen::Vector3d max_pos = mp_.map_max_boundary_; + + resetBuffer(min_pos, max_pos); + + md_.local_bound_min_ = Eigen::Vector3i::Zero(); + md_.local_bound_max_ = mp_.map_voxel_num_ - Eigen::Vector3i::Ones(); +} + +void GridMap::resetBuffer(Eigen::Vector3d min_pos, Eigen::Vector3d max_pos) +{ + + Eigen::Vector3i min_id, max_id; + + /* reset occ and dist buffer */ + for (int x = min_id(0); x <= max_id(0); ++x) + for (int y = min_id(1); y <= max_id(1); ++y) + for (int z = min_id(2); z <= max_id(2); ++z) + { + md_.occupancy_buffer_inflate_[toAddress(x, y, z)] = 0; + } +} + +int GridMap::setCacheOccupancy(Eigen::Vector3d pos, int occ) +{ + if (occ != 1 && occ != 0) + return INVALID_IDX; + + Eigen::Vector3i id; + posToIndex(pos, id); + int idx_ctns = toAddress(id); + + md_.count_hit_and_miss_[idx_ctns] += 1; + + if (md_.count_hit_and_miss_[idx_ctns] == 1) + { + md_.cache_voxel_.push(id); + } + + if (occ == 1) + md_.count_hit_[idx_ctns] += 1; + + return idx_ctns; +} + +void GridMap::projectDepthImage() +{ + // md_.proj_points_.clear(); + md_.proj_points_cnt = 0; + + uint16_t *row_ptr; + // int cols = current_img_.cols, rows = current_img_.rows; + int cols = md_.depth_image_.cols; + int rows = md_.depth_image_.rows; + int skip_pix = mp_.skip_pixel_; + + double depth; + + Eigen::Matrix3d camera_r = md_.camera_r_m_; + + if (!mp_.use_depth_filter_) + { + for (int v = 0; v < rows; v+=skip_pix) + { + row_ptr = md_.depth_image_.ptr(v); + + for (int u = 0; u < cols; u+=skip_pix) + { + + Eigen::Vector3d proj_pt; + depth = (*row_ptr++) / mp_.k_depth_scaling_factor_; + proj_pt(0) = (u - mp_.cx_) * depth / mp_.fx_; + proj_pt(1) = (v - mp_.cy_) * depth / mp_.fy_; + proj_pt(2) = depth; + + proj_pt = camera_r * proj_pt + md_.camera_pos_; + + if (u == 320 && v == 240) + std::cout << "depth: " << depth << std::endl; + md_.proj_points_[md_.proj_points_cnt++] = proj_pt; + } + } + } + /* use depth filter */ + else + { + + if (!md_.has_first_depth_) + md_.has_first_depth_ = true; + else + { + Eigen::Vector3d pt_cur, pt_world, pt_reproj; + + Eigen::Matrix3d last_camera_r_inv; + last_camera_r_inv = md_.last_camera_r_m_.inverse(); + const double inv_factor = 1.0 / mp_.k_depth_scaling_factor_; + + for (int v = mp_.depth_filter_margin_; v < rows - mp_.depth_filter_margin_; v += mp_.skip_pixel_) + { + row_ptr = md_.depth_image_.ptr(v) + mp_.depth_filter_margin_; + + for (int u = mp_.depth_filter_margin_; u < cols - mp_.depth_filter_margin_; + u += mp_.skip_pixel_) + { + + depth = (*row_ptr) * inv_factor; + row_ptr = row_ptr + mp_.skip_pixel_; + + // filter depth + // depth += rand_noise_(eng_); + // if (depth > 0.01) depth += rand_noise2_(eng_); + + if (*row_ptr == 0) + { + depth = mp_.max_ray_length_ + 0.1; + } + else if (depth < mp_.depth_filter_mindist_) + { + continue; + } + else if (depth > mp_.depth_filter_maxdist_) + { + depth = mp_.max_ray_length_ + 0.1; + } + + // project to world frame + pt_cur(0) = (u - mp_.cx_) * depth / mp_.fx_; + pt_cur(1) = (v - mp_.cy_) * depth / mp_.fy_; + pt_cur(2) = depth; + + pt_world = camera_r * pt_cur + md_.camera_pos_; + // if (!isInMap(pt_world)) { + // pt_world = closetPointInMap(pt_world, md_.camera_pos_); + // } + + md_.proj_points_[md_.proj_points_cnt++] = pt_world; + + // check consistency with last image, disabled... + if (false) + { + pt_reproj = last_camera_r_inv * (pt_world - md_.last_camera_pos_); + double uu = pt_reproj.x() * mp_.fx_ / pt_reproj.z() + mp_.cx_; + double vv = pt_reproj.y() * mp_.fy_ / pt_reproj.z() + mp_.cy_; + + if (uu >= 0 && uu < cols && vv >= 0 && vv < rows) + { + if (fabs(md_.last_depth_image_.at((int)vv, (int)uu) * inv_factor - + pt_reproj.z()) < mp_.depth_filter_tolerance_) + { + md_.proj_points_[md_.proj_points_cnt++] = pt_world; + } + } + else + { + md_.proj_points_[md_.proj_points_cnt++] = pt_world; + } + } + } + } + } + } + + /* maintain camera pose for consistency check */ + + md_.last_camera_pos_ = md_.camera_pos_; + md_.last_camera_r_m_ = md_.camera_r_m_; + md_.last_depth_image_ = md_.depth_image_; +} + +void GridMap::raycastProcess() +{ + // if (md_.proj_points_.size() == 0) + if (md_.proj_points_cnt == 0) + return; + + ros::Time t1, t2; + + md_.raycast_num_ += 1; + + int vox_idx; + double length; + + // bounding box of updated region + double min_x = mp_.map_max_boundary_(0); + double min_y = mp_.map_max_boundary_(1); + double min_z = mp_.map_max_boundary_(2); + // printf("bounding box min:[%f,%f,%f]\n",min_x,min_y,min_z); + + double max_x = mp_.map_min_boundary_(0); + double max_y = mp_.map_min_boundary_(1); + double max_z = mp_.map_min_boundary_(2); + // printf("bounding box max:[%f,%f,%f]\n",max_x,max_y,max_z); + + RayCaster raycaster; + Eigen::Vector3d half = Eigen::Vector3d(0.5, 0.5, 0.5); + Eigen::Vector3d ray_pt, pt_w; + + for (int i = 0; i < md_.proj_points_cnt; ++i) + { + pt_w = md_.proj_points_[i]; + + // set flag for projected point + + if (!isInMap(pt_w)) + { + pt_w = closetPointInMap(pt_w, md_.camera_pos_); + + length = (pt_w - md_.camera_pos_).norm(); + if (length > mp_.max_ray_length_) + { + pt_w = (pt_w - md_.camera_pos_) / length * mp_.max_ray_length_ + md_.camera_pos_; + } + vox_idx = setCacheOccupancy(pt_w, 0); + } + else + { + length = (pt_w - md_.camera_pos_).norm(); + + if (length > mp_.max_ray_length_) + { + pt_w = (pt_w - md_.camera_pos_) / length * mp_.max_ray_length_ + md_.camera_pos_; + vox_idx = setCacheOccupancy(pt_w, 0); + } + else + { + vox_idx = setCacheOccupancy(pt_w, 1); + } + } + + max_x = max(max_x, pt_w(0)); + max_y = max(max_y, pt_w(1)); + max_z = max(max_z, pt_w(2)); + + min_x = min(min_x, pt_w(0)); + min_y = min(min_y, pt_w(1)); + min_z = min(min_z, pt_w(2)); + + // raycasting between camera center and point + + if (vox_idx != INVALID_IDX) + { + if (md_.flag_rayend_[vox_idx] == md_.raycast_num_) + { + continue; + } + else + { + md_.flag_rayend_[vox_idx] = md_.raycast_num_; + } + } + + raycaster.setInput(pt_w / mp_.resolution_, md_.camera_pos_ / mp_.resolution_); + + while (raycaster.step(ray_pt)) + { + Eigen::Vector3d tmp = (ray_pt + half) * mp_.resolution_; + length = (tmp - md_.camera_pos_).norm(); + + // if (length < mp_.min_ray_length_) break; + + vox_idx = setCacheOccupancy(tmp, 0); + + if (vox_idx != INVALID_IDX) + { + if (md_.flag_traverse_[vox_idx] == md_.raycast_num_) + { + break; + } + else + { + md_.flag_traverse_[vox_idx] = md_.raycast_num_; + } + } + } + } + + min_x = min(min_x, md_.camera_pos_(0)); + min_y = min(min_y, md_.camera_pos_(1)); + min_z = min(min_z, md_.camera_pos_(2)); + + max_x = max(max_x, md_.camera_pos_(0)); + max_y = max(max_y, md_.camera_pos_(1)); + max_z = max(max_z, md_.camera_pos_(2)); + max_z = max(max_z, mp_.ground_height_); + + // printf("bounding box max:000[%f,%f,%f]\n",max_x,max_y,max_z); + // printf("bounding box min:000[%f,%f,%f]\n",min_x,min_x,min_x); + posToIndex(Eigen::Vector3d(max_x, max_y, max_z), md_.local_bound_max_); + posToIndex(Eigen::Vector3d(min_x, min_y, min_z), md_.local_bound_min_); + // printf("md_.local_bound_min_000:[%d,%d,%d]\n",md_.local_bound_min_(0),md_.local_bound_min_(1),md_.local_bound_min_(2)); + boundIndex(md_.local_bound_min_); + // printf("md_.local_bound_max_000:[%d,%d,%d]\n",md_.local_bound_max_(0),md_.local_bound_max_(1),md_.local_bound_max_(2)); + boundIndex(md_.local_bound_max_); + + md_.local_updated_ = true; + + // update occupancy cached in queue + Eigen::Vector3d local_range_min = md_.camera_pos_ - mp_.local_update_range_; + Eigen::Vector3d local_range_max = md_.camera_pos_ + mp_.local_update_range_; + + Eigen::Vector3i min_id, max_id; + posToIndex(local_range_min, min_id); + posToIndex(local_range_max, max_id); + boundIndex(min_id); + boundIndex(max_id); + + // std::cout << "cache all: " << md_.cache_voxel_.size() << std::endl; + + while (!md_.cache_voxel_.empty()) + { + + Eigen::Vector3i idx = md_.cache_voxel_.front(); + int idx_ctns = toAddress(idx); + md_.cache_voxel_.pop(); + + double log_odds_update = + md_.count_hit_[idx_ctns] >= md_.count_hit_and_miss_[idx_ctns] - md_.count_hit_[idx_ctns] ? mp_.prob_hit_log_ : mp_.prob_miss_log_; + + md_.count_hit_[idx_ctns] = md_.count_hit_and_miss_[idx_ctns] = 0; + + if (log_odds_update >= 0 && md_.occupancy_buffer_[idx_ctns] >= mp_.clamp_max_log_) + { + continue; + } + else if (log_odds_update <= 0 && md_.occupancy_buffer_[idx_ctns] <= mp_.clamp_min_log_) + { + md_.occupancy_buffer_[idx_ctns] = mp_.clamp_min_log_; + continue; + } + + bool in_local = idx(0) >= min_id(0) && idx(0) <= max_id(0) && idx(1) >= min_id(1) && + idx(1) <= max_id(1) && idx(2) >= min_id(2) && idx(2) <= max_id(2); + if (!in_local) + { + md_.occupancy_buffer_[idx_ctns] = mp_.clamp_min_log_; + } + + md_.occupancy_buffer_[idx_ctns] = + std::min(std::max(md_.occupancy_buffer_[idx_ctns] + log_odds_update, mp_.clamp_min_log_), + mp_.clamp_max_log_); + } +} + +Eigen::Vector3d GridMap::closetPointInMap(const Eigen::Vector3d &pt, const Eigen::Vector3d &camera_pt) +{ + Eigen::Vector3d diff = pt - camera_pt; + Eigen::Vector3d max_tc = mp_.map_max_boundary_ - camera_pt; + Eigen::Vector3d min_tc = mp_.map_min_boundary_ - camera_pt; + + double min_t = 1000000; + + for (int i = 0; i < 3; ++i) + { + if (fabs(diff[i]) > 0) + { + + double t1 = max_tc[i] / diff[i]; + if (t1 > 0 && t1 < min_t) + min_t = t1; + + double t2 = min_tc[i] / diff[i]; + if (t2 > 0 && t2 < min_t) + min_t = t2; + } + } + + return camera_pt + (min_t - 1e-3) * diff; +} + +void GridMap::clearAndInflateLocalMap() +{ + /*clear outside local*/ + const int vec_margin = 5; + // Eigen::Vector3i min_vec_margin = min_vec - Eigen::Vector3i(vec_margin, + // vec_margin, vec_margin); Eigen::Vector3i max_vec_margin = max_vec + + // Eigen::Vector3i(vec_margin, vec_margin, vec_margin); + + // printf("md_.local_bound_min_:[%d,%d,%d]\n",md_.local_bound_min_(0),md_.local_bound_min_(1),md_.local_bound_min_(2)); + Eigen::Vector3i min_cut = md_.local_bound_min_ - + Eigen::Vector3i(mp_.local_map_margin_, mp_.local_map_margin_, mp_.local_map_margin_); + // printf("md_.local_bound_max_:[%d,%d,%d]\n",md_.local_bound_max_(0),md_.local_bound_max_(1),md_.local_bound_max_(2)); + Eigen::Vector3i max_cut = md_.local_bound_max_ + + Eigen::Vector3i(mp_.local_map_margin_, mp_.local_map_margin_, mp_.local_map_margin_); + boundIndex(min_cut); + // printf("min_cut:[%d,%d,%d]\n",min_cut(0),min_cut(1),min_cut(2)); + boundIndex(max_cut); + // printf("max_cut:[%d,%d,%d]\n",max_cut(0),max_cut(1),max_cut(2)); + + Eigen::Vector3i min_cut_m = min_cut - Eigen::Vector3i(vec_margin, vec_margin, vec_margin); + Eigen::Vector3i max_cut_m = max_cut + Eigen::Vector3i(vec_margin, vec_margin, vec_margin); + boundIndex(min_cut_m); + // printf("min_cut_m:[%d,%d,%d]\n",min_cut_m(0),min_cut_m(1),min_cut_m(2)); + boundIndex(max_cut_m); + // printf("max_cut_m:[%d,%d,%d]\n\n",max_cut_m(0),max_cut_m(1),max_cut_m(2)); + + // clear data outside the local range + + for (int x = min_cut_m(0); x <= max_cut_m(0); ++x) + for (int y = min_cut_m(1); y <= max_cut_m(1); ++y) + { + + for (int z = min_cut_m(2); z < min_cut(2); ++z) + { + int idx = toAddress(x, y, z); + md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; + } + + for (int z = max_cut(2) + 1; z <= max_cut_m(2); ++z) + { + int idx = toAddress(x, y, z); + md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; + } + } + + for (int z = min_cut_m(2); z <= max_cut_m(2); ++z) + for (int x = min_cut_m(0); x <= max_cut_m(0); ++x) + { + + for (int y = min_cut_m(1); y < min_cut(1); ++y) + { + int idx = toAddress(x, y, z); + md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; + } + + for (int y = max_cut(1) + 1; y <= max_cut_m(1); ++y) + { + int idx = toAddress(x, y, z); + md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; + } + } + + for (int y = min_cut_m(1); y <= max_cut_m(1); ++y) + for (int z = min_cut_m(2); z <= max_cut_m(2); ++z) + { + + for (int x = min_cut_m(0); x < min_cut(0); ++x) + { + int idx = toAddress(x, y, z); + md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; + } + + for (int x = max_cut(0) + 1; x <= max_cut_m(0); ++x) + { + int idx = toAddress(x, y, z); + md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; + } + } + + // inflate occupied voxels to compensate robot size + + int inf_step = ceil(mp_.obstacles_inflation_ / mp_.resolution_); + // int inf_step_z = 1; + vector inf_pts(pow(2 * inf_step + 1, 3)); + // inf_pts.resize(4 * inf_step + 3); + Eigen::Vector3i inf_pt; + + // clear outdated data + for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x) + for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y) + for (int z = md_.local_bound_min_(2); z <= md_.local_bound_max_(2); ++z) + { + md_.occupancy_buffer_inflate_[toAddress(x, y, z)] = 0; + } + + // inflate obstacles + for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x) + for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y) + for (int z = md_.local_bound_min_(2); z <= md_.local_bound_max_(2); ++z) + { + + if (md_.occupancy_buffer_[toAddress(x, y, z)] > mp_.min_occupancy_log_) + { + inflatePoint(Eigen::Vector3i(x, y, z), inf_step, inf_pts); + + for (int k = 0; k < (int)inf_pts.size(); ++k) + { + inf_pt = inf_pts[k]; + int idx_inf = toAddress(inf_pt); + if (idx_inf < 0 || + idx_inf >= mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2)) + { + continue; + } + md_.occupancy_buffer_inflate_[idx_inf] = 1; + } + } + } + + // add virtual ceiling to limit flight height + if (mp_.virtual_ceil_height_ > -0.5) { + int ceil_id = floor((mp_.virtual_ceil_height_ - mp_.map_origin_(2)) * mp_.resolution_inv_) - 1; + for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x) + for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y) { + md_.occupancy_buffer_inflate_[toAddress(x, y, ceil_id)] = 1; + } + } +} + +void GridMap::visCallback(const ros::TimerEvent & /*event*/) +{ + + publishMapInflate(true); + publishMap(); +} + +void GridMap::updateOccupancyCallback(const ros::TimerEvent & /*event*/) +{ + if (md_.last_occ_update_time_.toSec() < 1.0 ) md_.last_occ_update_time_ = ros::Time::now(); + + if (!md_.occ_need_update_) + { + if ( md_.flag_use_depth_fusion && (ros::Time::now() - md_.last_occ_update_time_).toSec() > mp_.odom_depth_timeout_ ) + { + ROS_ERROR("odom or depth lost! ros::Time::now()=%f, md_.last_occ_update_time_=%f, mp_.odom_depth_timeout_=%f", + ros::Time::now().toSec(), md_.last_occ_update_time_.toSec(), mp_.odom_depth_timeout_); + md_.flag_depth_odom_timeout_ = true; + } + return; + } + + // std::cout << "updateOccupancyCallback 1!" << std::endl; + md_.last_occ_update_time_ = ros::Time::now(); + + /* update occupancy */ + // ros::Time t1, t2, t3, t4; + // t1 = ros::Time::now(); + + projectDepthImage(); + // t2 = ros::Time::now(); + raycastProcess(); + // t3 = ros::Time::now(); + + if (md_.local_updated_) + clearAndInflateLocalMap(); + + // t4 = ros::Time::now(); + + // cout << setprecision(7); + // cout << "t2=" << (t2-t1).toSec() << " t3=" << (t3-t2).toSec() << " t4=" << (t4-t3).toSec() << endl;; + + // md_.fuse_time_ += (t2 - t1).toSec(); + // md_.max_fuse_time_ = max(md_.max_fuse_time_, (t2 - t1).toSec()); + + // if (mp_.show_occ_time_) + // ROS_WARN("Fusion: cur t = %lf, avg t = %lf, max t = %lf", (t2 - t1).toSec(), + // md_.fuse_time_ / md_.update_num_, md_.max_fuse_time_); + + md_.occ_need_update_ = false; + md_.local_updated_ = false; +} + +void GridMap::depthPoseCallback(const sensor_msgs::ImageConstPtr &img, + const geometry_msgs::PoseStampedConstPtr &pose) +{ + + std::cout << "test 1!" << std::endl; + + /* get depth image */ + cv_bridge::CvImagePtr cv_ptr; + cv_ptr = cv_bridge::toCvCopy(img, img->encoding); + + if (img->encoding == sensor_msgs::image_encodings::TYPE_32FC1) + { + (cv_ptr->image).convertTo(cv_ptr->image, CV_16UC1, mp_.k_depth_scaling_factor_); + } + cv_ptr->image.copyTo(md_.depth_image_); + + // std::cout << "depth: " << md_.depth_image_.cols << ", " << md_.depth_image_.rows << std::endl; + + /* get pose */ + md_.camera_pos_(0) = pose->pose.position.x; + md_.camera_pos_(1) = pose->pose.position.y; + md_.camera_pos_(2) = pose->pose.position.z; + md_.camera_r_m_ = Eigen::Quaterniond(pose->pose.orientation.w, pose->pose.orientation.x, + pose->pose.orientation.y, pose->pose.orientation.z) + .toRotationMatrix(); + if (isInMap(md_.camera_pos_)) + { + md_.has_odom_ = true; + md_.update_num_ += 1; + md_.occ_need_update_ = true; + } + else + { + md_.occ_need_update_ = false; + } + + md_.flag_use_depth_fusion = true; +} + +void GridMap::odomCallback(const nav_msgs::OdometryConstPtr &odom) +{ + if (md_.has_first_depth_) + return; + + md_.camera_pos_(0) = odom->pose.pose.position.x; + md_.camera_pos_(1) = odom->pose.pose.position.y; + md_.camera_pos_(2) = odom->pose.pose.position.z; + + md_.has_odom_ = true; +} + +void GridMap::cloudCallback(const sensor_msgs::PointCloud2ConstPtr &img) +{ + + pcl::PointCloud latest_cloud; + pcl::fromROSMsg(*img, latest_cloud); + + md_.has_cloud_ = true; + + if (!md_.has_odom_) + { + std::cout << "no odom!" << std::endl; + return; + } + + //ROS_INFO("test 3"); + + if (latest_cloud.points.size() == 0) + return; + + if (isnan(md_.camera_pos_(0)) || isnan(md_.camera_pos_(1)) || isnan(md_.camera_pos_(2))) + return; + + this->resetBuffer(md_.camera_pos_ - mp_.local_update_range_, + md_.camera_pos_ + mp_.local_update_range_); + + pcl::PointXYZ pt; + Eigen::Vector3d p3d, p3d_inf; + + int inf_step = ceil(mp_.obstacles_inflation_ / mp_.resolution_); + int inf_step_z = 1; + + double max_x, max_y, max_z, min_x, min_y, min_z; + + min_x = mp_.map_max_boundary_(0); + min_y = mp_.map_max_boundary_(1); + min_z = mp_.map_max_boundary_(2); + + max_x = mp_.map_min_boundary_(0); + max_y = mp_.map_min_boundary_(1); + max_z = mp_.map_min_boundary_(2); + + for (size_t i = 0; i < latest_cloud.points.size(); ++i) + { + pt = latest_cloud.points[i]; + p3d(0) = pt.x, p3d(1) = pt.y, p3d(2) = pt.z; + + /* point inside update range */ + Eigen::Vector3d devi = p3d - md_.camera_pos_; + Eigen::Vector3i inf_pt; + + if (fabs(devi(0)) < mp_.local_update_range_(0) && fabs(devi(1)) < mp_.local_update_range_(1) && + fabs(devi(2)) < mp_.local_update_range_(2)) + { + + /* inflate the point */ + for (int x = -inf_step; x <= inf_step; ++x) + for (int y = -inf_step; y <= inf_step; ++y) + for (int z = -inf_step_z; z <= inf_step_z; ++z) + { + + p3d_inf(0) = pt.x + x * mp_.resolution_; + p3d_inf(1) = pt.y + y * mp_.resolution_; + p3d_inf(2) = pt.z + z * mp_.resolution_; + + max_x = max(max_x, p3d_inf(0)); + max_y = max(max_y, p3d_inf(1)); + max_z = max(max_z, p3d_inf(2)); + + min_x = min(min_x, p3d_inf(0)); + min_y = min(min_y, p3d_inf(1)); + min_z = min(min_z, p3d_inf(2)); + + posToIndex(p3d_inf, inf_pt); + + if (!isInMap(inf_pt)) + continue; + + int idx_inf = toAddress(inf_pt); + + md_.occupancy_buffer_inflate_[idx_inf] = 1; + } + } + } + + min_x = min(min_x, md_.camera_pos_(0)); + min_y = min(min_y, md_.camera_pos_(1)); + min_z = min(min_z, md_.camera_pos_(2)); + + max_x = max(max_x, md_.camera_pos_(0)); + max_y = max(max_y, md_.camera_pos_(1)); + max_z = max(max_z, md_.camera_pos_(2)); + + max_z = max(max_z, mp_.ground_height_); + + posToIndex(Eigen::Vector3d(max_x, max_y, max_z), md_.local_bound_max_); + posToIndex(Eigen::Vector3d(min_x, min_y, min_z), md_.local_bound_min_); + + boundIndex(md_.local_bound_min_); + boundIndex(md_.local_bound_max_); + + // add virtual ceiling to limit flight height + if (mp_.virtual_ceil_height_ > -0.5) { + int ceil_id = floor((mp_.virtual_ceil_height_ - mp_.map_origin_(2)) * mp_.resolution_inv_) - 1; + for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x) + for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y) { + md_.occupancy_buffer_inflate_[toAddress(x, y, ceil_id)] = 1; + } + } +} + +void GridMap::publishMap() +{ + + if (map_pub_.getNumSubscribers() <= 0) + return; + + pcl::PointXYZ pt; + pcl::PointCloud cloud; + + Eigen::Vector3i min_cut = md_.local_bound_min_; + Eigen::Vector3i max_cut = md_.local_bound_max_; + + int lmm = mp_.local_map_margin_ / 2; + min_cut -= Eigen::Vector3i(lmm, lmm, lmm); + max_cut += Eigen::Vector3i(lmm, lmm, lmm); + + boundIndex(min_cut); + boundIndex(max_cut); + + for (int x = min_cut(0); x <= max_cut(0); ++x) + for (int y = min_cut(1); y <= max_cut(1); ++y) + for (int z = min_cut(2); z <= max_cut(2); ++z) + { + if (md_.occupancy_buffer_[toAddress(x, y, z)] < mp_.min_occupancy_log_) + continue; + + Eigen::Vector3d pos; + indexToPos(Eigen::Vector3i(x, y, z), pos); + if (pos(2) > mp_.visualization_truncate_height_) + continue; + + pt.x = pos(0); + pt.y = pos(1); + pt.z = pos(2); + cloud.push_back(pt); + } + + //border + for(int i = 0; i < mp_.map_voxel_num_(0); i++) + { + pt.x = mp_.map_min_boundary_(0)+i*mp_.resolution_; + pt.y = mp_.map_min_boundary_(1); + pt.z = mp_.map_min_boundary_(2); + cloud.push_back(pt); + + pt.x = mp_.map_min_boundary_(0)+i*mp_.resolution_; + pt.y = mp_.map_max_boundary_(1); + pt.z = mp_.map_min_boundary_(2); + cloud.push_back(pt); + } + + for(int i = 0; i < mp_.map_voxel_num_(1); i++) + { + pt.x = mp_.map_min_boundary_(0); + pt.y = mp_.map_min_boundary_(1)+i*mp_.resolution_; + pt.z = mp_.map_min_boundary_(2); + cloud.push_back(pt); + + pt.x = mp_.map_max_boundary_(0); + pt.y = mp_.map_min_boundary_(1)+i*mp_.resolution_; + pt.z = mp_.map_min_boundary_(2); + cloud.push_back(pt); + } + + cloud.width = cloud.points.size(); + cloud.height = 1; + cloud.is_dense = true; + cloud.header.frame_id = mp_.frame_id_; + + sensor_msgs::PointCloud2 cloud_msg; + + pcl::toROSMsg(cloud, cloud_msg); + map_pub_.publish(cloud_msg); +} + +void GridMap::publishMapInflate(bool all_info) +{ + + if (map_inf_pub_.getNumSubscribers() <= 0) + return; + + pcl::PointXYZ pt; + pcl::PointCloud cloud; + + Eigen::Vector3i min_cut = md_.local_bound_min_; + Eigen::Vector3i max_cut = md_.local_bound_max_; + + if (all_info) + { + int lmm = mp_.local_map_margin_; + min_cut -= Eigen::Vector3i(lmm, lmm, lmm); + max_cut += Eigen::Vector3i(lmm, lmm, lmm); + } + + boundIndex(min_cut); + boundIndex(max_cut); + + for (int x = min_cut(0); x <= max_cut(0); ++x) + for (int y = min_cut(1); y <= max_cut(1); ++y) + for (int z = min_cut(2); z <= max_cut(2); ++z) + { + if (md_.occupancy_buffer_inflate_[toAddress(x, y, z)] == 0) + continue; + + Eigen::Vector3d pos; + indexToPos(Eigen::Vector3i(x, y, z), pos); + if (pos(2) > mp_.visualization_truncate_height_) + continue; + + pt.x = pos(0); + pt.y = pos(1); + pt.z = pos(2); + cloud.push_back(pt); + } + + //border x + for(int i = 0; i < mp_.map_voxel_num_(0); i++) + { + pt.x = mp_.map_min_boundary_(0)+i*mp_.resolution_; + pt.y = mp_.map_min_boundary_(1); + pt.z = mp_.map_min_boundary_(2); + cloud.push_back(pt); + + pt.x = mp_.map_min_boundary_(0)+i*mp_.resolution_; + pt.y = mp_.map_max_boundary_(1); + pt.z = mp_.map_min_boundary_(2); + cloud.push_back(pt); + } + //border y + for(int i = 0; i < mp_.map_voxel_num_(1); i++) + { + pt.x = mp_.map_min_boundary_(0); + pt.y = mp_.map_min_boundary_(1)+i*mp_.resolution_; + pt.z = mp_.map_min_boundary_(2); + cloud.push_back(pt); + + pt.x = mp_.map_max_boundary_(0); + pt.y = mp_.map_min_boundary_(1)+i*mp_.resolution_; + pt.z = mp_.map_min_boundary_(2); + cloud.push_back(pt); + } + + cloud.width = cloud.points.size(); + cloud.height = 1; + cloud.is_dense = true; + cloud.header.frame_id = mp_.frame_id_; + sensor_msgs::PointCloud2 cloud_msg; + + pcl::toROSMsg(cloud, cloud_msg); + map_inf_pub_.publish(cloud_msg); + + // ROS_INFO("pub map"); +} + +bool GridMap::odomValid() { return md_.has_odom_; } + +bool GridMap::hasDepthObservation() { return md_.has_first_depth_; } + +Eigen::Vector3d GridMap::getOrigin() { return mp_.map_origin_; } + +// int GridMap::getVoxelNum() { +// return mp_.map_voxel_num_[0] * mp_.map_voxel_num_[1] * mp_.map_voxel_num_[2]; +// } + +void GridMap::getRegion(Eigen::Vector3d &ori, Eigen::Vector3d &size) +{ + ori = mp_.map_origin_, size = mp_.map_size_; +} + +void GridMap::extrinsicCallback(const nav_msgs::OdometryConstPtr &odom) +{ + Eigen::Quaterniond cam2body_q = Eigen::Quaterniond(odom->pose.pose.orientation.w, + odom->pose.pose.orientation.x, + odom->pose.pose.orientation.y, + odom->pose.pose.orientation.z); + Eigen::Matrix3d cam2body_r_m = cam2body_q.toRotationMatrix(); + md_.cam2body_.block<3, 3>(0, 0) = cam2body_r_m; + md_.cam2body_(0, 3) = odom->pose.pose.position.x; + md_.cam2body_(1, 3) = odom->pose.pose.position.y; + md_.cam2body_(2, 3) = odom->pose.pose.position.z; + md_.cam2body_(3, 3) = 1.0; +} + +void GridMap::depthOdomCallback(const sensor_msgs::ImageConstPtr &img, + const nav_msgs::OdometryConstPtr &odom) +{ + /* get pose */ + Eigen::Quaterniond body_q = Eigen::Quaterniond(odom->pose.pose.orientation.w, + odom->pose.pose.orientation.x, + odom->pose.pose.orientation.y, + odom->pose.pose.orientation.z); + Eigen::Matrix3d body_r_m = body_q.toRotationMatrix(); + Eigen::Matrix4d body2world; + body2world.block<3, 3>(0, 0) = body_r_m; + body2world(0, 3) = odom->pose.pose.position.x; + body2world(1, 3) = odom->pose.pose.position.y; + body2world(2, 3) = odom->pose.pose.position.z; + body2world(3, 3) = 1.0; + + Eigen::Matrix4d cam_T = body2world * md_.cam2body_; + md_.camera_pos_(0) = cam_T(0, 3); + md_.camera_pos_(1) = cam_T(1, 3); + md_.camera_pos_(2) = cam_T(2, 3); + md_.camera_r_m_ = cam_T.block<3, 3>(0, 0); + + /* get depth image */ + cv_bridge::CvImagePtr cv_ptr; + cv_ptr = cv_bridge::toCvCopy(img, img->encoding); + if (img->encoding == sensor_msgs::image_encodings::TYPE_32FC1) + { + (cv_ptr->image).convertTo(cv_ptr->image, CV_16UC1, mp_.k_depth_scaling_factor_); + } + cv_ptr->image.copyTo(md_.depth_image_); + + md_.occ_need_update_ = true; + md_.flag_use_depth_fusion = true; + + // reset depth lost flag + if(md_.flag_depth_odom_timeout_ == true) md_.flag_depth_odom_timeout_ = false; +} diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_env/src/obj_generator.cpp b/src/Prometheus/Modules/ego_planner_swarm/plan_env/src/obj_generator.cpp new file mode 100644 index 00000000..20b07011 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_env/src/obj_generator.cpp @@ -0,0 +1,238 @@ +/** +* This file is part of Fast-Planner. +* +* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, +* Developed by Boyu Zhou , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* Fast-Planner is free software: you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Fast-Planner is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with Fast-Planner. If not, see . +*/ + + + +#include "visualization_msgs/Marker.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +using namespace std; + +int obj_num, _input_type; +double _x_size, _y_size, _h_size, _vel, _yaw_dot, _acc_r1, _acc_r2, _acc_z, _scale1, _scale2, _interval; + + +ros::Publisher obj_pub; // visualize marker +vector pose_pubs; // obj pose (from optitrack) +vector obj_models; + +random_device rd; +default_random_engine eng(rd()); +uniform_real_distribution rand_pos_x; +uniform_real_distribution rand_pos_y; +uniform_real_distribution rand_h; +uniform_real_distribution rand_vel; +uniform_real_distribution rand_acc_r; +uniform_real_distribution rand_acc_t; +uniform_real_distribution rand_acc_z; +uniform_real_distribution rand_color; +uniform_real_distribution rand_scale; +uniform_real_distribution rand_yaw_dot; +uniform_real_distribution rand_yaw; + +ros::Time time_update, time_change; + +void updateCallback(const ros::TimerEvent& e); +void visualizeObj(int id); + +int main(int argc, char** argv) { + ros::init(argc, argv, "dynamic_obj"); + ros::NodeHandle node("~"); + + /* ---------- initialize ---------- */ + node.param("obj_generator/obj_num", obj_num, 20); + node.param("obj_generator/x_size", _x_size, 10.0); + node.param("obj_generator/y_size", _y_size, 10.0); + node.param("obj_generator/h_size", _h_size, 2.0); + node.param("obj_generator/vel", _vel, 2.0); + node.param("obj_generator/yaw_dot", _yaw_dot, 2.0); + node.param("obj_generator/acc_r1", _acc_r1, 2.0); + node.param("obj_generator/acc_r2", _acc_r2, 2.0); + node.param("obj_generator/acc_z", _acc_z, 0.0); + node.param("obj_generator/scale1", _scale1, 0.5); + node.param("obj_generator/scale2", _scale2, 1.0); + node.param("obj_generator/interval", _interval, 100.0); + node.param("obj_generator/input_type", _input_type, 1); + + obj_pub = node.advertise("/dynamic/obj", 10); + for (int i = 0; i < obj_num; ++i) { + ros::Publisher pose_pub = + node.advertise("/dynamic/pose_" + to_string(i), 10); + pose_pubs.push_back(pose_pub); + } + + ros::Timer update_timer = node.createTimer(ros::Duration(1 / 30.0), updateCallback); + cout << "[dynamic]: initialize with " + to_string(obj_num) << " moving obj." << endl; + ros::Duration(1.0).sleep(); + + rand_color = uniform_real_distribution(0.0, 1.0); + rand_pos_x = uniform_real_distribution(-_x_size/2, _x_size/2); + rand_pos_y = uniform_real_distribution(-_y_size/2, _y_size/2); + rand_h = uniform_real_distribution(0.0, _h_size); + rand_vel = uniform_real_distribution(-_vel, _vel); + rand_acc_t = uniform_real_distribution(0.0, 6.28); + rand_acc_r = uniform_real_distribution(_acc_r1, _acc_r2); + rand_acc_z = uniform_real_distribution(-_acc_z, _acc_z); + rand_scale = uniform_real_distribution(_scale1, _scale2); + rand_yaw = uniform_real_distribution(0.0, 2 * 3.141592); + rand_yaw_dot = uniform_real_distribution(-_yaw_dot, _yaw_dot); + + /* ---------- give initial value of each obj ---------- */ + for (int i = 0; i < obj_num; ++i) { + LinearObjModel model; + Eigen::Vector3d pos(rand_pos_x(eng), rand_pos_y(eng), rand_h(eng)); + Eigen::Vector3d vel(rand_vel(eng), rand_vel(eng), 0.0); + Eigen::Vector3d color(rand_color(eng), rand_color(eng), rand_color(eng)); + Eigen::Vector3d scale(rand_scale(eng), 1.5 * rand_scale(eng), 5.0*rand_scale(eng)); + double yaw = rand_yaw(eng); + double yaw_dot = rand_yaw_dot(eng); + + double r, t, z; + r = rand_acc_r(eng); + t = rand_acc_t(eng); + z = rand_acc_z(eng); + Eigen::Vector3d acc(r * cos(t), r * sin(t), z); + + if ( _input_type == 1 ) + { + model.initialize(pos, vel, acc, yaw, yaw_dot, color, scale, _input_type); // Vel input + } + else + { + model.initialize(pos, Eigen::Vector3d(0,0,0), acc, yaw, yaw_dot, color, scale, _input_type); // Acc input + } + model.setLimits(Eigen::Vector3d(_x_size/2, _y_size/2, _h_size), Eigen::Vector2d(0.0, _vel), + Eigen::Vector2d(0, 0)); + obj_models.push_back(model); + } + + time_update = ros::Time::now(); + time_change = ros::Time::now(); + + /* ---------- start loop ---------- */ + ros::spin(); + + return 0; +} + +void updateCallback(const ros::TimerEvent& e) { + ros::Time time_now = ros::Time::now(); + + /* ---------- change input ---------- */ + // double dtc = (time_now - time_change).toSec(); + // if (dtc > _interval) { + // for (int i = 0; i < obj_num; ++i) { + // /* ---------- use acc input ---------- */ + // // double r, t, z; + // // r = rand_acc_r(eng); + // // t = rand_acc_t(eng); + // // z = rand_acc_z(eng); + // // Eigen::Vector3d acc(r * cos(t), r * sin(t), z); + // // obj_models[i].setInput(acc); + + // /* ---------- use vel input ---------- */ + // double vx, vy, vz, yd; + // vx = rand_vel(eng); + // vy = rand_vel(eng); + // vz = 0.0; + // yd = rand_yaw_dot(eng); + + // obj_models[i].setInput(Eigen::Vector3d(vx, vy, vz)); + // obj_models[i].setYawDot(yd); + // } + // time_change = time_now; + // } + + /* ---------- update obj state ---------- */ + double dt = (time_now - time_update).toSec(); + time_update = time_now; + for (int i = 0; i < obj_num; ++i) { + obj_models[i].update(dt); + visualizeObj(i); + ros::Duration(0.000001).sleep(); + } + + /* ---------- collision ---------- */ + // for (int i = 0; i < obj_num; ++i) + // for (int j = i + 1; j < obj_num; ++j) { + // bool collision = LinearObjModel::collide(obj_models[i], obj_models[j]); + // if (collision) { + // double yd1 = rand_yaw_dot(eng); + // double yd2 = rand_yaw_dot(eng); + // obj_models[i].setYawDot(yd1); + // obj_models[j].setYawDot(yd2); + // } + // } +} + +void visualizeObj(int id) { + Eigen::Vector3d pos, color, scale; + pos = obj_models[id].getPosition(); + color = obj_models[id].getColor(); + scale = obj_models[id].getScale(); + double yaw = obj_models[id].getYaw(); + + Eigen::Matrix3d rot; + rot << cos(yaw), -sin(yaw), 0.0, sin(yaw), cos(yaw), 0.0, 0.0, 0.0, 1.0; + + Eigen::Quaterniond qua; + qua = rot; + + /* ---------- rviz ---------- */ + visualization_msgs::Marker mk; + mk.header.frame_id = "world"; + mk.header.stamp = ros::Time::now(); + mk.type = visualization_msgs::Marker::CUBE; + mk.action = visualization_msgs::Marker::ADD; + mk.id = id; + + mk.scale.x = scale(0), mk.scale.y = scale(1), mk.scale.z = scale(2); + mk.color.a = 1.0, mk.color.r = color(0), mk.color.g = color(1), mk.color.b = color(2); + + mk.pose.orientation.w = qua.w(); + mk.pose.orientation.x = qua.x(); + mk.pose.orientation.y = qua.y(); + mk.pose.orientation.z = qua.z(); + + mk.pose.position.x = pos(0), mk.pose.position.y = pos(1), mk.pose.position.z = pos(2); + + obj_pub.publish(mk); + + /* ---------- pose ---------- */ + geometry_msgs::PoseStamped pose; + pose.header.frame_id = "world"; + pose.header.seq = id; + pose.pose.position.x = pos(0), pose.pose.position.y = pos(1), pose.pose.position.z = pos(2); + pose.pose.orientation.w = 1.0; + pose_pubs[id].publish(pose); +} diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_env/src/obj_predictor.cpp b/src/Prometheus/Modules/ego_planner_swarm/plan_env/src/obj_predictor.cpp new file mode 100644 index 00000000..d960cda4 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_env/src/obj_predictor.cpp @@ -0,0 +1,289 @@ +/** +* This file is part of Fast-Planner. +* +* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, +* Developed by Boyu Zhou , +* for more information see . +* If you use this code, please cite the respective publications as +* listed on the above website. +* +* Fast-Planner is free software: you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Fast-Planner is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with Fast-Planner. If not, see . +*/ + + + +#include +#include + +namespace fast_planner { +/* ============================== obj history_ ============================== */ + +// int ObjHistory::queue_size_; +// int ObjHistory::skip_num_; +// ros::Time ObjHistory::global_start_time_; + +void ObjHistory::init(int id, int skip_num, int queue_size, ros::Time global_start_time) { + clear(); + skip_ = 0; + obj_idx_ = id; + skip_num_ = skip_num; + queue_size_ = queue_size; + global_start_time_ = global_start_time; +} + +void ObjHistory::poseCallback(const geometry_msgs::PoseStampedConstPtr& msg) { + ++skip_; + if (skip_ < skip_num_) return; + + Eigen::Vector4d pos_t; + pos_t(0) = msg->pose.position.x, pos_t(1) = msg->pose.position.y, pos_t(2) = msg->pose.position.z; + pos_t(3) = (ros::Time::now() - global_start_time_).toSec(); + + history_.push_back(pos_t); + // cout << "idx: " << obj_idx_ << "pos_t: " << pos_t.transpose() << endl; + + if (history_.size() > queue_size_) history_.pop_front(); + + skip_ = 0; +} + +// ObjHistory:: +/* ============================== obj predictor ============================== + */ +ObjPredictor::ObjPredictor(/* args */) { +} + +ObjPredictor::ObjPredictor(ros::NodeHandle& node) { + this->node_handle_ = node; +} + +ObjPredictor::~ObjPredictor() { +} + +void ObjPredictor::init() { + /* get param */ + int queue_size, skip_nums; + + node_handle_.param("prediction/obj_num", obj_num_, 0); + node_handle_.param("prediction/lambda", lambda_, 1.0); + node_handle_.param("prediction/predict_rate", predict_rate_, 1.0); + node_handle_.param("prediction/queue_size", queue_size, 10); + node_handle_.param("prediction/skip_nums", skip_nums, 1); + + predict_trajs_.reset(new vector); + predict_trajs_->resize(obj_num_); + + obj_scale_.reset(new vector); + obj_scale_->resize(obj_num_); + scale_init_.resize(obj_num_); + for (int i = 0; i < obj_num_; i++) + scale_init_[i] = false; + + /* subscribe to pose */ + ros::Time t_now = ros::Time::now(); + for (int i = 0; i < obj_num_; i++) { + shared_ptr obj_his(new ObjHistory); + + obj_his->init(i, skip_nums, queue_size, t_now); + obj_histories_.push_back(obj_his); + + ros::Subscriber pose_sub = node_handle_.subscribe( + "/dynamic/pose_" + std::to_string(i), 10, &ObjHistory::poseCallback, obj_his.get()); + + pose_subs_.push_back(pose_sub); + + predict_trajs_->at(i).setGlobalStartTime(t_now); + } + + marker_sub_ = node_handle_.subscribe("/dynamic/obj", 10, + &ObjPredictor::markerCallback, this); + + /* update prediction */ + predict_timer_ = + node_handle_.createTimer(ros::Duration(1 / predict_rate_), &ObjPredictor::predictCallback, this); +} + +ObjPrediction ObjPredictor::getPredictionTraj() { + return this->predict_trajs_; +} + +ObjScale ObjPredictor::getObjScale() { + return this->obj_scale_; +} + +void ObjPredictor::predictPolyFit() { + /* iterate all obj */ + for (int i = 0; i < obj_num_; i++) { + /* ---------- write A and b ---------- */ + Eigen::Matrix A; + Eigen::Matrix temp; + Eigen::Matrix bm[3]; // poly coefficent + vector> pm(3); + + A.setZero(); + for (int i = 0; i < 3; ++i) + bm[i].setZero(); + + /* ---------- estimation error ---------- */ + list his; + obj_histories_[i]->getHistory(his); + for (list::iterator it = his.begin(); it != his.end(); ++it) { + Eigen::Vector3d qi = (*it).head(3); + double ti = (*it)(3); + + /* A */ + temp << 1.0, ti, pow(ti, 2), pow(ti, 3), pow(ti, 4), pow(ti, 5); + for (int j = 0; j < 6; ++j) + A.row(j) += 2.0 * pow(ti, j) * temp.transpose(); + + /* b */ + for (int dim = 0; dim < 3; ++dim) + bm[dim] += 2.0 * qi(dim) * temp; + } + + /* ---------- acceleration regulator ---------- */ + double t1 = his.front()(3); + double t2 = his.back()(3); + + temp << 0.0, 0.0, 2 * t1 - 2 * t2, 3 * pow(t1, 2) - 3 * pow(t2, 2), 4 * pow(t1, 3) - 4 * pow(t2, 3), + 5 * pow(t1, 4) - 5 * pow(t2, 4); + A.row(2) += -4 * lambda_ * temp.transpose(); + + temp << 0.0, 0.0, pow(t1, 2) - pow(t2, 2), 2 * pow(t1, 3) - 2 * pow(t2, 3), + 3 * pow(t1, 4) - 3 * pow(t2, 4), 4 * pow(t1, 5) - 4 * pow(t2, 5); + A.row(3) += -12 * lambda_ * temp.transpose(); + + temp << 0.0, 0.0, 20 * pow(t1, 3) - 20 * pow(t2, 3), 45 * pow(t1, 4) - 45 * pow(t2, 4), + 72 * pow(t1, 5) - 72 * pow(t2, 5), 100 * pow(t1, 6) - 100 * pow(t2, 6); + A.row(4) += -4.0 / 5.0 * lambda_ * temp.transpose(); + + temp << 0.0, 0.0, 35 * pow(t1, 4) - 35 * pow(t2, 4), 84 * pow(t1, 5) - 84 * pow(t2, 5), + 140 * pow(t1, 6) - 140 * pow(t2, 6), 200 * pow(t1, 7) - 200 * pow(t2, 7); + A.row(5) += -4.0 / 7.0 * lambda_ * temp.transpose(); + + /* ---------- solve ---------- */ + for (int j = 0; j < 3; j++) { + pm[j] = A.colPivHouseholderQr().solve(bm[j]); + } + + /* ---------- update prediction container ---------- */ + predict_trajs_->at(i).setPolynomial(pm); + predict_trajs_->at(i).setTime(t1, t2); + } +} + +void ObjPredictor::predictCallback(const ros::TimerEvent& e) { + // predictPolyFit(); + predictConstVel(); +} + +void ObjPredictor::markerCallback(const visualization_msgs::MarkerConstPtr& msg) { + int idx = msg->id; + (*obj_scale_)[idx](0) = msg->scale.x; + (*obj_scale_)[idx](1) = msg->scale.y; + (*obj_scale_)[idx](2) = msg->scale.z; + + scale_init_[idx] = true; + + int finish_num = 0; + for (int i = 0; i < obj_num_; i++) { + if (scale_init_[i]) finish_num++; + } + + if (finish_num == obj_num_) { + marker_sub_.shutdown(); + } +} + +void ObjPredictor::predictConstVel() { + for (int i = 0; i < obj_num_; i++) { + /* ---------- get the last two point ---------- */ + list his; + obj_histories_[i]->getHistory(his); + // if ( i==0 ) + // { + // cout << "his.size()=" << his.size() << endl; + // for ( auto hi:his ) + // { + // cout << hi.transpose() << endl; + // } + // } + list::iterator list_it = his.end(); + + /* ---------- test iteration ---------- */ + // cout << "----------------------------" << endl; + // for (auto v4d : his) + // cout << "v4d: " << v4d.transpose() << endl; + + Eigen::Vector3d q1, q2; + double t1, t2; + + --list_it; + q2 = (*list_it).head(3); + t2 = (*list_it)(3); + + --list_it; + q1 = (*list_it).head(3); + t1 = (*list_it)(3); + + Eigen::Matrix p01, q12; + q12.row(0) = q1.transpose(); + q12.row(1) = q2.transpose(); + + Eigen::Matrix At12; + At12 << 1, t1, 1, t2; + + p01 = At12.inverse() * q12; + + vector> polys(3); + for (int j = 0; j < 3; ++j) { + polys[j].setZero(); + polys[j].head(2) = p01.col(j); + } + + // if ( i==0 ) + // { + // cout << "q1=" << q1.transpose() << " t1=" << t1 << " q2=" << q2.transpose() << " t2=" << t2 << endl; + // cout << "polys=" << polys[0].transpose() << endl; + // } + + predict_trajs_->at(i).setPolynomial(polys); + predict_trajs_->at(i).setTime(t1, t2); + } +} + +Eigen::Vector3d ObjPredictor::evaluatePoly(int obj_id, double time) +{ + if ( obj_id < obj_num_ ) + { + return predict_trajs_->at(obj_id).evaluate(time); + } + + double MAX = std::numeric_limits::max(); + return Eigen::Vector3d(MAX, MAX, MAX); +} + +Eigen::Vector3d ObjPredictor::evaluateConstVel(int obj_id, double time) +{ + if ( obj_id < obj_num_ ) + { + return predict_trajs_->at(obj_id).evaluateConstVel(time); + } + + double MAX = std::numeric_limits::max(); + return Eigen::Vector3d(MAX, MAX, MAX); +} + +// ObjPredictor:: +} // namespace fast_planner \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_env/src/raycast.cpp b/src/Prometheus/Modules/ego_planner_swarm/plan_env/src/raycast.cpp new file mode 100644 index 00000000..56ffdbf1 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_env/src/raycast.cpp @@ -0,0 +1,321 @@ +#include +#include +#include +#include + +int signum(int x) { + return x == 0 ? 0 : x < 0 ? -1 : 1; +} + +double mod(double value, double modulus) { + return fmod(fmod(value, modulus) + modulus, modulus); +} + +double intbound(double s, double ds) { + // Find the smallest positive t such that s+t*ds is an integer. + if (ds < 0) { + return intbound(-s, -ds); + } else { + s = mod(s, 1); + // problem is now s+t*ds = 1 + return (1 - s) / ds; + } +} + +void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min, + const Eigen::Vector3d& max, int& output_points_cnt, Eigen::Vector3d* output) { + // std::cout << start << ' ' << end << std::endl; + // From "A Fast Voxel Traversal Algorithm for Ray Tracing" + // by John Amanatides and Andrew Woo, 1987 + // + // + // Extensions to the described algorithm: + // • Imposed a distance limit. + // • The face passed through to reach the current cube is provided to + // the callback. + + // The foundation of this algorithm is a parameterized representation of + // the provided ray, + // origin + t * direction, + // except that t is not actually stored; rather, at any given point in the + // traversal, we keep track of the *greater* t values which we would have + // if we took a step sufficient to cross a cube boundary along that axis + // (i.e. change the integer part of the coordinate) in the variables + // tMaxX, tMaxY, and tMaxZ. + + // Cube containing origin point. + int x = (int)std::floor(start.x()); + int y = (int)std::floor(start.y()); + int z = (int)std::floor(start.z()); + int endX = (int)std::floor(end.x()); + int endY = (int)std::floor(end.y()); + int endZ = (int)std::floor(end.z()); + Eigen::Vector3d direction = (end - start); + double maxDist = direction.squaredNorm(); + + // Break out direction vector. + double dx = endX - x; + double dy = endY - y; + double dz = endZ - z; + + // Direction to increment x,y,z when stepping. + int stepX = (int)signum((int)dx); + int stepY = (int)signum((int)dy); + int stepZ = (int)signum((int)dz); + + // See description above. The initial values depend on the fractional + // part of the origin. + double tMaxX = intbound(start.x(), dx); + double tMaxY = intbound(start.y(), dy); + double tMaxZ = intbound(start.z(), dz); + + // The change in t when taking a step (always positive). + double tDeltaX = ((double)stepX) / dx; + double tDeltaY = ((double)stepY) / dy; + double tDeltaZ = ((double)stepZ) / dz; + + // Avoids an infinite loop. + if (stepX == 0 && stepY == 0 && stepZ == 0) return; + + double dist = 0; + while (true) { + if (x >= min.x() && x < max.x() && y >= min.y() && y < max.y() && z >= min.z() && z < max.z()) { + output[output_points_cnt](0) = x; + output[output_points_cnt](1) = y; + output[output_points_cnt](2) = z; + + output_points_cnt++; + dist = sqrt((x - start(0)) * (x - start(0)) + (y - start(1)) * (y - start(1)) + + (z - start(2)) * (z - start(2))); + + if (dist > maxDist) return; + + /* if (output_points_cnt > 1500) { + std::cerr << "Error, too many racyast voxels." << + std::endl; + throw std::out_of_range("Too many raycast voxels"); + }*/ + } + + if (x == endX && y == endY && z == endZ) break; + + // tMaxX stores the t-value at which we cross a cube boundary along the + // X axis, and similarly for Y and Z. Therefore, choosing the least tMax + // chooses the closest cube boundary. Only the first case of the four + // has been commented in detail. + if (tMaxX < tMaxY) { + if (tMaxX < tMaxZ) { + // Update which cube we are now in. + x += stepX; + // Adjust tMaxX to the next X-oriented boundary crossing. + tMaxX += tDeltaX; + } else { + z += stepZ; + tMaxZ += tDeltaZ; + } + } else { + if (tMaxY < tMaxZ) { + y += stepY; + tMaxY += tDeltaY; + } else { + z += stepZ; + tMaxZ += tDeltaZ; + } + } + } +} + +void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min, + const Eigen::Vector3d& max, std::vector* output) { + // std::cout << start << ' ' << end << std::endl; + // From "A Fast Voxel Traversal Algorithm for Ray Tracing" + // by John Amanatides and Andrew Woo, 1987 + // + // + // Extensions to the described algorithm: + // • Imposed a distance limit. + // • The face passed through to reach the current cube is provided to + // the callback. + + // The foundation of this algorithm is a parameterized representation of + // the provided ray, + // origin + t * direction, + // except that t is not actually stored; rather, at any given point in the + // traversal, we keep track of the *greater* t values which we would have + // if we took a step sufficient to cross a cube boundary along that axis + // (i.e. change the integer part of the coordinate) in the variables + // tMaxX, tMaxY, and tMaxZ. + + // Cube containing origin point. + int x = (int)std::floor(start.x()); + int y = (int)std::floor(start.y()); + int z = (int)std::floor(start.z()); + int endX = (int)std::floor(end.x()); + int endY = (int)std::floor(end.y()); + int endZ = (int)std::floor(end.z()); + Eigen::Vector3d direction = (end - start); + double maxDist = direction.squaredNorm(); + + // Break out direction vector. + double dx = endX - x; + double dy = endY - y; + double dz = endZ - z; + + // Direction to increment x,y,z when stepping. + int stepX = (int)signum((int)dx); + int stepY = (int)signum((int)dy); + int stepZ = (int)signum((int)dz); + + // See description above. The initial values depend on the fractional + // part of the origin. + double tMaxX = intbound(start.x(), dx); + double tMaxY = intbound(start.y(), dy); + double tMaxZ = intbound(start.z(), dz); + + // The change in t when taking a step (always positive). + double tDeltaX = ((double)stepX) / dx; + double tDeltaY = ((double)stepY) / dy; + double tDeltaZ = ((double)stepZ) / dz; + + output->clear(); + + // Avoids an infinite loop. + if (stepX == 0 && stepY == 0 && stepZ == 0) return; + + double dist = 0; + while (true) { + if (x >= min.x() && x < max.x() && y >= min.y() && y < max.y() && z >= min.z() && z < max.z()) { + output->push_back(Eigen::Vector3d(x, y, z)); + + dist = (Eigen::Vector3d(x, y, z) - start).squaredNorm(); + + if (dist > maxDist) return; + + if (output->size() > 1500) { + std::cerr << "Error, too many racyast voxels." << std::endl; + throw std::out_of_range("Too many raycast voxels"); + } + } + + if (x == endX && y == endY && z == endZ) break; + + // tMaxX stores the t-value at which we cross a cube boundary along the + // X axis, and similarly for Y and Z. Therefore, choosing the least tMax + // chooses the closest cube boundary. Only the first case of the four + // has been commented in detail. + if (tMaxX < tMaxY) { + if (tMaxX < tMaxZ) { + // Update which cube we are now in. + x += stepX; + // Adjust tMaxX to the next X-oriented boundary crossing. + tMaxX += tDeltaX; + } else { + z += stepZ; + tMaxZ += tDeltaZ; + } + } else { + if (tMaxY < tMaxZ) { + y += stepY; + tMaxY += tDeltaY; + } else { + z += stepZ; + tMaxZ += tDeltaZ; + } + } + } +} + +bool RayCaster::setInput(const Eigen::Vector3d& start, + const Eigen::Vector3d& end /* , const Eigen::Vector3d& min, + const Eigen::Vector3d& max */) { + start_ = start; + end_ = end; + // max_ = max; + // min_ = min; + + x_ = (int)std::floor(start_.x()); + y_ = (int)std::floor(start_.y()); + z_ = (int)std::floor(start_.z()); + endX_ = (int)std::floor(end_.x()); + endY_ = (int)std::floor(end_.y()); + endZ_ = (int)std::floor(end_.z()); + direction_ = (end_ - start_); + maxDist_ = direction_.squaredNorm(); + + // Break out direction vector. + dx_ = endX_ - x_; + dy_ = endY_ - y_; + dz_ = endZ_ - z_; + + // Direction to increment x,y,z when stepping. + stepX_ = (int)signum((int)dx_); + stepY_ = (int)signum((int)dy_); + stepZ_ = (int)signum((int)dz_); + + // See description above. The initial values depend on the fractional + // part of the origin. + tMaxX_ = intbound(start_.x(), dx_); + tMaxY_ = intbound(start_.y(), dy_); + tMaxZ_ = intbound(start_.z(), dz_); + + // The change in t when taking a step (always positive). + tDeltaX_ = ((double)stepX_) / dx_; + tDeltaY_ = ((double)stepY_) / dy_; + tDeltaZ_ = ((double)stepZ_) / dz_; + + dist_ = 0; + + step_num_ = 0; + + // Avoids an infinite loop. + if (stepX_ == 0 && stepY_ == 0 && stepZ_ == 0) + return false; + else + return true; +} + +bool RayCaster::step(Eigen::Vector3d& ray_pt) { + // if (x_ >= min_.x() && x_ < max_.x() && y_ >= min_.y() && y_ < max_.y() && + // z_ >= min_.z() && z_ < + // max_.z()) + ray_pt = Eigen::Vector3d(x_, y_, z_); + + // step_num_++; + + // dist_ = (Eigen::Vector3d(x_, y_, z_) - start_).squaredNorm(); + + if (x_ == endX_ && y_ == endY_ && z_ == endZ_) { + return false; + } + + // if (dist_ > maxDist_) + // { + // return false; + // } + + // tMaxX stores the t-value at which we cross a cube boundary along the + // X axis, and similarly for Y and Z. Therefore, choosing the least tMax + // chooses the closest cube boundary. Only the first case of the four + // has been commented in detail. + if (tMaxX_ < tMaxY_) { + if (tMaxX_ < tMaxZ_) { + // Update which cube we are now in. + x_ += stepX_; + // Adjust tMaxX to the next X-oriented boundary crossing. + tMaxX_ += tDeltaX_; + } else { + z_ += stepZ_; + tMaxZ_ += tDeltaZ_; + } + } else { + if (tMaxY_ < tMaxZ_) { + y_ += stepY_; + tMaxY_ += tDeltaY_; + } else { + z_ += stepZ_; + tMaxZ_ += tDeltaZ_; + } + } + + return true; +} \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/CMakeLists.txt b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/CMakeLists.txt new file mode 100755 index 00000000..0d182e6a --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ego_planner) + +set(CMAKE_BUILD_TYPE "Release") +ADD_COMPILE_OPTIONS(-std=c++11 ) +ADD_COMPILE_OPTIONS(-std=c++14 ) +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") + +find_package(Eigen3 REQUIRED) +find_package(PCL 1.7 REQUIRED) + +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + geometry_msgs + quadrotor_msgs + plan_env + path_searching + bspline_opt + traj_utils + message_generation + cv_bridge + prometheus_msgs +) + +# catkin_package(CATKIN_DEPENDS message_runtime) +catkin_package( + INCLUDE_DIRS include + LIBRARIES ego_planner + CATKIN_DEPENDS plan_env path_searching bspline_opt traj_utils +# DEPENDS system_lib +) + +include_directories( + include + SYSTEM + ${catkin_INCLUDE_DIRS} ${PROJECT_SOURCE_DIR}/include + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} + ${PROJECT_SOURCE_DIR}/../../uav_control/include + ${PROJECT_SOURCE_DIR}/../../common/include +) + + +add_executable(ego_planner_node + src/ego_planner_node.cpp + src/ego_replan_fsm.cpp + src/planner_manager.cpp + ) +target_link_libraries(ego_planner_node + ${catkin_LIBRARIES} + ) +#add_dependencies(ego_planner_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) + +add_executable(traj_server src/traj_server.cpp) +target_link_libraries(traj_server ${catkin_LIBRARIES}) + + +add_executable(traj_server_for_prometheus src_for_prometheus/traj_server_for_prometheus.cpp) +target_link_libraries(traj_server_for_prometheus ${catkin_LIBRARIES}) + +add_executable(ego_goal_pub src_for_prometheus/ego_goal_pub.cpp) +target_link_libraries(ego_goal_pub ${catkin_LIBRARIES}) \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/include/plan_manage/ego_replan_fsm.h b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/include/plan_manage/ego_replan_fsm.h new file mode 100644 index 00000000..35c17217 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/include/plan_manage/ego_replan_fsm.h @@ -0,0 +1,133 @@ +#ifndef _REBO_REPLAN_FSM_H_ +#define _REBO_REPLAN_FSM_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using std::vector; + +namespace ego_planner +{ + + class EGOReplanFSM + { + + private: + /* ---------- flag ---------- */ + //规划器状态 + enum FSM_EXEC_STATE + { + INIT, + WAIT_TARGET, + GEN_NEW_TRAJ, + REPLAN_TRAJ, + EXEC_TRAJ, + EMERGENCY_STOP, + SEQUENTIAL_START + }; + // 目标点类型 + enum TARGET_TYPE + { + MANUAL_TARGET = 1, + PRESET_TARGET = 2, + REFENCE_PATH = 3 + }; + + /* planning utils */ + EGOPlannerManager::Ptr planner_manager_; + PlanningVisualization::Ptr visualization_; + traj_utils::DataDisp data_disp_; + traj_utils::MultiBsplines multi_bspline_msgs_buf_; + + /* parameters */ + int target_type_; // 1 mannual select, 2 hard code + double no_replan_thresh_, replan_thresh_; + double waypoints_[10][3]; + + int waypoint_num_, wp_id_; + double planning_horizen_, planning_horizen_time_; + double emergency_time_; + bool flag_realworld_experiment_; + bool enable_fail_safe_; + + /* planning data */ + bool have_trigger_, have_target_, have_odom_, have_new_target_, have_recv_pre_agent_; + FSM_EXEC_STATE exec_state_; + int continously_called_times_{0}; + + Eigen::Vector3d odom_pos_, odom_vel_, odom_acc_; // odometry state + Eigen::Quaterniond odom_orient_; + + Eigen::Vector3d init_pt_, start_pt_, start_vel_, start_acc_, start_yaw_; // start state + Eigen::Vector3d end_pt_, end_vel_; // goal state + Eigen::Vector3d local_target_pt_, local_target_vel_; // local target state + std::vector wps_; + int current_wp_; + + bool flag_escape_emergency_; + + /* ROS utils */ + ros::NodeHandle node_; + ros::Timer exec_timer_, safety_timer_; + ros::Subscriber waypoint_sub_, odom_sub_, swarm_trajs_sub_, broadcast_bspline_sub_, trigger_sub_; + ros::Publisher replan_pub_, new_pub_, bspline_pub_, data_disp_pub_, swarm_trajs_pub_, broadcast_bspline_pub_; + + /* helper functions */ + bool callReboundReplan(bool flag_use_poly_init, bool flag_randomPolyTraj); // front-end and back-end method + bool callEmergencyStop(Eigen::Vector3d stop_pos); // front-end and back-end method + bool planFromGlobalTraj(const int trial_times = 1); + bool planFromCurrentTraj(const int trial_times = 1); + + /* return value: std::pair< Times of the same state be continuously called, current continuously called state > */ + void changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call); + std::pair timesOfConsecutiveStateCalls(); + void printFSMExecState(); + + void readGivenWps(); + void planNextWaypoint(const Eigen::Vector3d next_wp); + void getLocalTarget(); + + /* ROS functions */ + void execFSMCallback(const ros::TimerEvent &e); + void checkCollisionCallback(const ros::TimerEvent &e); + void waypointCallback(const geometry_msgs::PoseStampedPtr &msg); + void triggerCallback(const geometry_msgs::PoseStampedPtr &msg); + void odometryCallback(const nav_msgs::OdometryConstPtr &msg); + void swarmTrajsCallback(const traj_utils::MultiBsplinesPtr &msg); + void BroadcastBsplineCallback(const traj_utils::BsplinePtr &msg); + + bool checkCollision(); + void publishSwarmTrajs(bool startup_pub); + + public: + EGOReplanFSM(/* args */) + { + } + ~EGOReplanFSM() + { + } + + void init(ros::NodeHandle &nh); + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + +} // namespace ego_planner + +#endif \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/include/plan_manage/planner_manager.h b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/include/plan_manage/planner_manager.h new file mode 100644 index 00000000..ce546421 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/include/plan_manage/planner_manager.h @@ -0,0 +1,85 @@ +#ifndef _PLANNER_MANAGER_H_ +#define _PLANNER_MANAGER_H_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ego_planner +{ + + // Fast Planner Manager + // Key algorithms of mapping and planning are called + + class EGOPlannerManager + { + // SECTION stable + public: + EGOPlannerManager(); + ~EGOPlannerManager(); + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /* main planning interface */ + bool reboundReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel, Eigen::Vector3d start_acc, + Eigen::Vector3d end_pt, Eigen::Vector3d end_vel, bool flag_polyInit, bool flag_randomPolyTraj); + bool EmergencyStop(Eigen::Vector3d stop_pos); + bool planGlobalTraj(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc, + const Eigen::Vector3d &end_pos, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc); + bool planGlobalTrajWaypoints(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc, + const std::vector &waypoints, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc); + + void initPlanModules(ros::NodeHandle &nh, PlanningVisualization::Ptr vis = NULL); + + void deliverTrajToOptimizer(void) { bspline_optimizer_->setSwarmTrajs(&swarm_trajs_buf_); }; + + void setDroneIdtoOpt(void) { bspline_optimizer_->setDroneId(pp_.drone_id); } + + double getSwarmClearance(void) { return bspline_optimizer_->getSwarmClearance(); } + + bool checkCollision(int drone_id); + + + PlanParameters pp_; + LocalTrajData local_data_; + GlobalTrajData global_data_; + GridMap::Ptr grid_map_; + fast_planner::ObjPredictor::Ptr obj_predictor_; + SwarmTrajData swarm_trajs_buf_; + + private: + /* main planning algorithms & modules */ + PlanningVisualization::Ptr visualization_; + + // ros::Publisher obj_pub_; //zx-todo + + BsplineOptimizer::Ptr bspline_optimizer_; + + int continous_failures_count_{0}; + + void updateTrajInfo(const UniformBspline &position_traj, const ros::Time time_now); + + void reparamBspline(UniformBspline &bspline, vector &start_end_derivative, double ratio, Eigen::MatrixXd &ctrl_pts, double &dt, + double &time_inc); + + bool refineTrajAlgo(UniformBspline &traj, vector &start_end_derivative, double ratio, double &ts, Eigen::MatrixXd &optimal_control_points); + + // !SECTION stable + + // SECTION developing + + public: + typedef unique_ptr Ptr; + + // !SECTION + }; +} // namespace ego_planner + +#endif \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/advanced_param.xml b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/advanced_param.xml new file mode 100644 index 00000000..7f7b438a --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/advanced_param.xml @@ -0,0 +1,160 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/default.rviz b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/default.rviz new file mode 100644 index 00000000..5a1a1969 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/default.rviz @@ -0,0 +1,3055 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /simulation_map1/Autocompute Value Bounds1 + - /drone01/Planning1 + - /drone01/Planning1/drone_path1 + - /drone01/Planning1/drone_path1/Offset1 + - /drone01/Mapping1 + - /drone01/Mapping1/map inflate1 + - /drone11/Planning1 + - /drone11/Planning1/drone_path1 + - /drone11/Mapping1 + - /drone11/Mapping1/map inflate1 + - /drone11/Simulation1 + - /drone21/Planning1 + - /drone21/Planning1/drone_path1 + - /drone21/Mapping1 + - /drone21/Mapping1/map inflate1 + - /drone31/Planning1 + - /drone31/Planning1/drone_path1 + - /drone31/Mapping1 + - /drone31/Mapping1/map inflate1 + - /drone41/Planning1 + - /drone41/Planning1/optimal_traj1 + - /drone41/Planning1/drone_path1 + - /drone41/Mapping1 + - /drone41/Mapping1/map inflate1 + - /drone51/Planning1 + - /drone51/Planning1/drone_path1 + - /drone51/Mapping1 + - /drone51/Mapping1/map inflate1 + - /drone61/Planning1 + - /drone61/Planning1/drone_path1 + - /drone61/Mapping1 + - /drone61/Mapping1/map inflate1 + - /drone71/Planning1 + - /drone71/Planning1/InitTraj1 + - /drone71/Planning1/drone_path1 + - /drone71/Mapping1 + - /drone71/Mapping1/map inflate1 + - /drone71/Simulation1 + - /drone71/Simulation1/robot1 + - /drone81/Planning1 + - /drone81/Planning1/drone_path1 + - /drone81/Mapping1 + - /drone81/Mapping1/map inflate1 + - /drone81/Simulation1/robot1 + - /drone91/Planning1 + - /drone91/Planning1/drone_path1 + - /drone91/Mapping1 + - /drone91/Mapping1/map inflate1 + - /drone91/Simulation1 + - /drone91/Simulation1/robot1 + - /drone101/Planning1 + - /drone101/Planning1/optimal_traj1 + - /drone101/Planning1/drone_path1 + - /drone101/Simulation1 + - /drone101/Simulation1/robot1 + - /drone111/Planning1 + - /drone111/Planning1/optimal_traj1 + - /drone111/Simulation1 + - /drone111/Simulation1/robot1 + - /drone121/Planning1 + - /drone121/Planning1/optimal_traj1 + - /drone121/Planning1/drone_path1 + - /drone121/Simulation1 + - /drone121/Simulation1/robot1 + - /drone131/Planning1 + - /drone131/Planning1/optimal_traj1 + - /drone131/Planning1/drone_path1 + - /drone131/Simulation1/robot1 + - /drone141/Planning1 + - /drone141/Planning1/drone_path1 + - /drone141/Simulation1/robot1 + - /drone151/Planning1 + - /drone151/Planning1/optimal_traj1 + - /drone151/Planning1/drone_path1 + - /drone151/Simulation1 + - /drone151/Simulation1/robot1 + - /drone161/Planning1 + - /drone161/Planning1/optimal_traj1 + - /drone161/Planning1/drone_path1 + - /drone161/Simulation1 + - /drone161/Simulation1/robot1 + - /drone171/Planning1 + - /drone171/Planning1/optimal_traj1 + - /drone171/Planning1/drone_path1 + - /drone171/Simulation1 + - /drone171/Simulation1/robot1 + - /drone181/Planning1 + - /drone181/Planning1/optimal_traj1 + - /drone181/Planning1/drone_path1 + - /drone181/Simulation1 + - /drone181/Simulation1/robot1 + - /drone191/Planning1 + - /drone191/Planning1/goal_point1 + - /drone191/Planning1/optimal_traj1 + - /drone191/Planning1/drone_path1 + - /drone191/Simulation1 + - /drone191/Simulation1/robot1 + - /drone201/Planning1 + - /drone201/Planning1/drone_path1 + - /drone201/Simulation1 + - /drone201/Simulation1/robot1 + Splitter Ratio: 0.43611112236976624 + Tree Height: 441 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: simulation_map +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Class: rviz/Axes + Enabled: false + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Value: false + - 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: 1000 + Reference Frame: + Value: true + - Alpha: 0.15000000596046448 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.9000000953674316 + Min Value: 0 + 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 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: simulation_map + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Boxes + Topic: /map_generator/global_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_0_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_0_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_0_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 29; 108; 212 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_0_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.8399999141693115 + Min Value: 0.03999999910593033 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 29; 108; 212 + 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_0_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_0_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_0_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone0 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_1_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_1_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_1_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 170; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_1_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_1_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_1_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_1_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone1 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_2_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_2_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_2_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 170; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_2_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_2_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_2_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_2_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone2 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_3_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_3_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_3_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 0; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_3_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_3_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_3_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_3_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone3 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_4_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_4_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_4_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 170; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_4_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 0; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_4_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_4_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_4_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone4 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_5_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_5_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_5_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_5_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_5_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_5_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone5 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_6_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_6_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_6_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 190; 120; 121 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_6_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 190; 120; 121 + 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_6_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_6_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_6_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone6 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_7_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_7_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_7_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 170; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_7_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 0; 170; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_5_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_7_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone7 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_8_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_8_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_8_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 226; 0; 158 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_8_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 226; 0; 158 + 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_8_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_8_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone8 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_9_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 85; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_9_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_9_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone9 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_10_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 29; 108; 212 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_10_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_10_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone10 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_11_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 170; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_11_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_11_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone11 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_12_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 170; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_12_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_12_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone12 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_13_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 0; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_13_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_13_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone13 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_14_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 170; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_14_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_14_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone14 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_15_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_15_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_15_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone15 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_16_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 190; 120; 121 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_16_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_16_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone16 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_17_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 170; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_17_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_17_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone17 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_18_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 226; 0; 158 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_18_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_18_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone18 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_19_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_19_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 85; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_19_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_19_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone19 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_20_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 20; 106; 172 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + 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: /drone_20_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_20_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone20 + - Class: rviz/Image + Enabled: false + Image Topic: /drone_0_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/ThirdPersonFollower + Distance: 52.77322769165039 + 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: -10.308119773864746 + Y: -0.9097803831100464 + Z: -9.5367431640625e-07 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Yaw: 3.1415886878967285 + Saved: + - Class: rviz/FPS + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: FPS + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4000000059604645 + Position: + X: -11 + Y: 0 + Z: 8 + Roll: 0 + Target Frame: my_view + Yaw: 0 + - Class: rviz/FPS + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: FPS + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5 + Position: + X: -10 + Y: 0 + Z: 10 + Roll: 0 + Target Frame: my_view + Yaw: 0 + - Class: rviz/FPS + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: FPS + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6000000238418579 + Position: + X: -10 + Y: 0 + Z: 10 + Roll: 0 + Target Frame: my_view + Yaw: 0 +Window Geometry: + Displays: + collapsed: true + Height: 1241 + Hide Left Dock: true + Hide Right Dock: false + Image: + collapsed: true + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1102 + X: 3339 + Y: 75 + depth: + collapsed: false diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/run_in_sim.launch b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/run_in_sim.launch new file mode 100644 index 00000000..0eb4ad79 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/run_in_sim.launch @@ -0,0 +1,135 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + > + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/rviz.launch b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/rviz.launch new file mode 100644 index 00000000..667873df --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/rviz.launch @@ -0,0 +1,3 @@ + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/simple_run.launch b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/simple_run.launch new file mode 100644 index 00000000..44ab7106 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/simple_run.launch @@ -0,0 +1,4 @@ + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/simulator.xml b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/simulator.xml new file mode 100755 index 00000000..9950ea0d --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/simulator.xml @@ -0,0 +1,141 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/single_run_in_sim.launch b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/single_run_in_sim.launch new file mode 100644 index 00000000..14639cc9 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/single_run_in_sim.launch @@ -0,0 +1,117 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/swarm.launch b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/swarm.launch new file mode 100644 index 00000000..1cc2db55 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/swarm.launch @@ -0,0 +1,205 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/swarm_large.launch b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/swarm_large.launch new file mode 100644 index 00000000..d3794d1f --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch/swarm_large.launch @@ -0,0 +1,412 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/advanced_param.xml b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/advanced_param.xml new file mode 100644 index 00000000..d0b67654 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/advanced_param.xml @@ -0,0 +1,156 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/ego.rviz b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/ego.rviz new file mode 100644 index 00000000..e06b824b --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/ego.rviz @@ -0,0 +1,608 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /simulation_map1/Autocompute Value Bounds1 + - /drone11 + - /drone11/Planning1 + - /drone11/Mapping1 + - /drone11/Simulation1 + - /drone11/Simulation1/Odometry1/Shape1 + - /drone21 + - /drone21/Planning1/drone_path1 + - /drone21/Mapping1 + - /drone21/Mapping1/map_generator_local1/Status1 + - /drone21/Simulation1/Odometry1/Shape1 + - /drone21/Simulation1/Odometry1/Covariance1 + - /drone21/Simulation1/robot1 + Splitter Ratio: 0.43611112236976624 + Tree Height: 922 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: map_generator_local +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Class: rviz/Axes + Enabled: false + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Value: false + - 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: 1000 + Reference Frame: + Value: true + - Alpha: 0.15000000596046448 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 3 + Min Value: 0.009999999776482582 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 85; 170; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: simulation_map + Position Transformer: XYZ + Queue Size: 1 + 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 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav1_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav1_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav1_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav1_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav1_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 204; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav1/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.20000000298023224 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /uav1/prometheus/drone_odom + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav1/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav1_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone1 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav2_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav2_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav2_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 170; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav2_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 115; 210; 22 + 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.03999999910593033 + Style: Flat Squares + Topic: /uav2_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 92; 53; 102 + 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav2/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.20000000298023224 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /uav2/prometheus/drone_odom + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav2/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav2_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone2 + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/ThirdPersonFollower + Distance: 19.938554763793945 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.7258191108703613 + Y: -0.35111796855926514 + Z: -3.442609158810228e-5 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9197959899902344 + Target Frame: + Value: ThirdPersonFollower (rviz) + Yaw: 3.2434911727905273 + Saved: + - Class: rviz/FPS + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: FPS + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4000000059604645 + Position: + X: -11 + Y: 0 + Z: 8 + Target Frame: my_view + Value: FPS (rviz) + Yaw: 0 + - Class: rviz/FPS + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: FPS + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5 + Position: + X: -10 + Y: 0 + Z: 10 + Target Frame: my_view + Value: FPS (rviz) + Yaw: 0 + - Class: rviz/FPS + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: FPS + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6000000238418579 + Position: + X: -10 + Y: 0 + Z: 10 + Target Frame: my_view + Value: FPS (rviz) + Yaw: 0 +Window Geometry: + Displays: + collapsed: false + Height: 1476 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2223 + X: 2724 + Y: 74 + depth: + collapsed: false diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/ego_control_config.yaml b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/ego_control_config.yaml new file mode 100755 index 00000000..df314e3a --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/ego_control_config.yaml @@ -0,0 +1,40 @@ + +# 质量，单位：kg +quad_mass : 1.5 +# hov_percent会影响最大推力 +hov_percent : 0.47 + +Limit: + # 位置比例控制 + pxy_int_max: 5.0 + pz_int_max: 10.0 + +hover_gain: + # 位置比例控制 + Kp_xy: 2.0 + Kp_z: 2.0 + # 速度比例控制 + Kv_xy: 2.0 + Kv_z: 2.0 + # 速度积分控制? + Kvi_xy: 0.3 + Kvi_z: 0.3 + # 加速度前馈系数 + Ka_xy: 1.0 + Ka_z: 1.0 + tilt_angle_max : 10.0 + +track_gain: + # 位置比例控制 + Kp_xy: 2.0 + Kp_z: 2.0 + # 速度比例控制 + Kv_xy: 2.0 + Kv_z: 2.0 + # 速度积分控制? + Kvi_xy: 0.1 + Kvi_z: 0.1 + # 加速度前馈系数 + Ka_xy: 1.0 + Ka_z: 1.0 + tilt_angle_max : 10.0 \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/real_ego_planner_basic.launch b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/real_ego_planner_basic.launch new file mode 100755 index 00000000..b262ba66 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/real_ego_planner_basic.launch @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/rviz_cxy_case2.rviz b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/rviz_cxy_case2.rviz new file mode 100644 index 00000000..a8de0048 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/rviz_cxy_case2.rviz @@ -0,0 +1,7575 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /simulation_map1/Autocompute Value Bounds1 + - /drone21/Planning1 + - /drone21/Planning1/drone_path1 + - /drone21/Mapping1 + - /drone31/Planning1 + - /drone41/Planning1/optimal_traj1 + - /drone41/Planning1/drone_path1 + - /drone41/Mapping1 + - /drone41/Mapping1/map inflate1 + - /drone41/Simulation1 + - /drone51/Planning1 + - /drone51/Planning1/drone_path1 + - /drone51/Mapping1 + - /drone51/Mapping1/map inflate1 + - /drone51/Simulation1 + - /drone51/Simulation1/robot1 + - /drone61/Planning1 + - /drone61/Planning1/drone_path1 + - /drone61/Mapping1 + - /drone61/Mapping1/map inflate1 + - /drone61/Simulation1/robot1 + - /drone71/Planning1 + - /drone71/Mapping1/map inflate1 + - /drone71/Simulation1 + - /drone71/Simulation1/robot1 + - /drone81/Planning1 + - /drone81/Planning1/drone_path1 + - /drone81/Mapping1 + - /drone81/Mapping1/map inflate1 + - /drone81/Simulation1 + - /drone81/Simulation1/robot1 + - /drone91/Planning1 + - /drone91/Planning1/goal_point1/Status1 + - /drone91/Simulation1/robot1 + - /drone101/Planning1 + - /drone101/Planning1/goal_point1 + - /drone101/Planning1/optimal_traj1 + - /drone101/Simulation1 + - /drone101/Simulation1/robot1 + - /drone111/Planning1 + - /drone111/Planning1/goal_point1 + - /drone111/Planning1/optimal_traj1 + - /drone111/Planning1/drone_path1 + - /drone111/Mapping1 + - /drone111/Mapping1/map inflate1 + - /drone111/Simulation1 + - /drone111/Simulation1/robot1 + - /drone121/Planning1 + - /drone121/Planning1/goal_point1 + - /drone121/Planning1/optimal_traj1 + - /drone121/Planning1/drone_path1 + - /drone121/Simulation1 + - /drone121/Simulation1/robot1 + - /drone131/Planning1 + - /drone131/Planning1/goal_point1 + - /drone131/Planning1/optimal_traj1 + - /drone131/Planning1/drone_path1 + - /drone131/Simulation1 + - /drone131/Simulation1/robot1 + - /drone141/Planning1 + - /drone141/Planning1/goal_point1 + - /drone141/Planning1/drone_path1 + - /drone141/Simulation1/robot1 + - /drone151/Planning1 + - /drone151/Planning1/goal_point1 + - /drone151/Planning1/optimal_traj1 + - /drone151/Planning1/drone_path1 + - /drone151/Simulation1 + - /drone151/Simulation1/robot1 + - /drone161/Planning1 + - /drone161/Planning1/goal_point1 + - /drone161/Planning1/optimal_traj1 + - /drone161/Planning1/drone_path1 + - /drone161/Simulation1 + - /drone161/Simulation1/robot1 + - /drone171/Planning1/goal_point1 + - /drone171/Planning1/optimal_traj1 + - /drone171/Planning1/drone_path1 + - /drone171/Mapping1 + - /drone171/Simulation1 + - /drone171/Simulation1/robot1 + - /drone181/Planning1 + - /drone181/Planning1/goal_point1 + - /drone181/Planning1/optimal_traj1 + - /drone181/Planning1/drone_path1 + - /drone181/Simulation1 + - /drone181/Simulation1/robot1 + - /drone191/Planning1 + - /drone191/Planning1/goal_point1 + - /drone191/Planning1/optimal_traj1 + - /drone191/Planning1/drone_path1 + - /drone191/Simulation1 + - /drone191/Simulation1/robot1 + - /drone201/Planning1 + - /drone201/Planning1/goal_point1 + - /drone201/Planning1/drone_path1 + - /drone201/Mapping1 + - /drone201/Simulation1 + - /drone201/Simulation1/robot1 + - /drone231/map inflate1 + - /drone241/map inflate1 + - /ugv31/path_cmd1 + - /ugv101/Marker1 + - /ugv151/lpcl1/Status1 + Splitter Ratio: 0.4421416223049164 + Tree Height: 441 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: simulation_map +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Class: rviz/Axes + Enabled: false + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Value: false + - 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: 1000 + Reference Frame: + Value: true + - Alpha: 0.15000000596046448 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 3 + Min Value: 0.009999999776482582 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 85; 170; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: simulation_map + Position Transformer: XYZ + Queue Size: 1 + 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 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav1_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav1_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav1/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav1_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav1/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav1/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone1 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav2_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav2_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav2_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 170; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav2/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 115; 210; 22 + 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav2_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 115; 210; 22 + 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav2/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav2/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav2_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone2 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav3_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav3_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav3_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 0; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav3/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav3_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav3/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav3/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav3_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone3 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav4_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav4_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav4_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 170; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav4/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 0; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav4_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 0; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav4/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav4/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav4_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone4 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav5_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav5_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav5_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav5/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav5_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav5/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav5/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone5 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav6_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav6_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav6_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 190; 120; 121 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav6/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 190; 120; 121 + 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav6_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav6/prometheus/drone_mesh + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav6_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone6 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav7_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav7_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav7_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 170; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav7/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 0; 170; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav7_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav7/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone7 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav8_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav8_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav8_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 226; 0; 158 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav8/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 226; 0; 158 + 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav8_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav8/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone8 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav9_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 85; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav9/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav9/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone9 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav10_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav10_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 29; 108; 212 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav10/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav10_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav10/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone10 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav11_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav11_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 170; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav11/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav11_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav11/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone11 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav12_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav12_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 170; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav12/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav12_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav12/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone12 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav13_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav13_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 0; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav13/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav13_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav13/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone13 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav14_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav14_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 170; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav14/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav14_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav14/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone14 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav15_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav15_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav15/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav15_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav15/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone15 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav16_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav16_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 190; 120; 121 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav16/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav16_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav16/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone16 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav17_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav17_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 170; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav17/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav17_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav17/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone17 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav18_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav18_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 226; 0; 158 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav18/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav18_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav18/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone18 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav19_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav19_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 85; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav19/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav19_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav19/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone19 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav20_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav20_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /uav9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 20; 106; 172 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav20/prometheus/drone_trajectory + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - 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: 170; 85; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav20_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav20/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /uav5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone20 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav21_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav21_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav21/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav21_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav21/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav21/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone21 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav22_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav22_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav22/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav22_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav22/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav22/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone22 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav23_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav23_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav23/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav23_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav23/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav23/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone23 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav24_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav24_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav24/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav24_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav24/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav24/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone24 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav25_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav25_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav25/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav25_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav25/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav25/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone25 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav26_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav26_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav26/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav26_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav26/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav26/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone26 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav27_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav27_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /av27/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav27_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav27/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav27/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone27 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav28_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav28_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav28/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav28_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav28/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav28/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone28 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav29_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav29_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav29/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav29_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav29/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav29/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone29 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav30_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav30_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav30/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav30_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav30/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav30/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone30 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav31_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav31_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav31/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav31_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav31/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav31/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone31 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav32_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav32_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav32/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav32_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav32/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav32/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone32 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav33_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav33_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav33/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav33_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav33/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav33/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone33 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav34_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav34_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav34/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav34_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav34/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav34/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone34 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav35_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav35_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav35/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav35_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav35/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav35/prometheus/drone_mesh + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + Enabled: true + Name: drone35 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav36_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav36_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav36/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav36_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav36/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav36/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone36 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav37_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav37_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav37/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav37_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav37/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav37/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone37 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav38_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav38_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav38/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav38_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav38/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav38/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone38 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav39_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav39_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav39/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav39_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav39/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav39/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone39 + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav40_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav40_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 164; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /uav40/prometheus/drone_trajectory + Unreliable: false + 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; 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: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07000000029802322 + Style: Flat Squares + Topic: /uav40_ego_planner_node/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: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 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: map_generator_local + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav40/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /uav40/prometheus/drone_mesh + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: drone40 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv1/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv1/prometheus/ugv_mesh + Name: Marker + Namespaces: + ugv_mesh: true + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv1/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv1/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 170; 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv1/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv1 + - 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/PointCloud2 + Color: 173; 127; 168 + 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv2/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv2/prometheus/ugv_mesh + Name: Marker + Namespaces: + ugv_mesh: true + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv2/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv2/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 252; 175; 62 + 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv2/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv2 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv3/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv3/prometheus/ugv_mesh + Name: Marker + Namespaces: + ugv_mesh: true + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv3/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv3/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 170; 255; 244 + 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv3/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv3 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv4/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv4/prometheus/ugv_mesh + Name: Marker + Namespaces: + ugv_mesh: true + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv4/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv4/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 55; 44; 220 + 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv4/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv4 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv5/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv5/prometheus/ugv_mesh + Name: Marker + Namespaces: + ugv_mesh: true + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv5/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv5/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 85; 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv5/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv5 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv6/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv6/prometheus/ugv_mesh + Name: Marker + Namespaces: + ugv_mesh: true + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv6/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv6/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 27; 244; 120 + 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv6/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv6 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv7/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv7/prometheus/ugv_mesh + Name: Marker + Namespaces: + ugv_mesh: true + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv7/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv7/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 244; 4; 10 + 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv7/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv7 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv8/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv8/prometheus/ugv_mesh + Name: Marker + Namespaces: + ugv_mesh: true + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv8/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv8/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 24; 4; 101 + 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv8/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv8 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv9/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv9/prometheus/ugv_mesh + Name: Marker + Namespaces: + ugv_mesh: true + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv9/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv9/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 170; 12; 220 + 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv9/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv9 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv10/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv10/prometheus/ugv_mesh + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv10/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv10/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 0; 170; 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv10/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv10 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv11/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv11/prometheus/ugv_mesh + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv11/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv11/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 170; 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv11/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv11 + - 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/PointCloud2 + Color: 173; 127; 168 + 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv12/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv12/prometheus/ugv_mesh + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv12/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv12/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 252; 175; 62 + 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv12/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv12 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv13/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv13/prometheus/ugv_mesh + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv13/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv13/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 170; 255; 244 + 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /uav13/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv13 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv14/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv14/prometheus/ugv_mesh + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv14/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv14/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 55; 44; 220 + 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv14/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv14 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv15/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv15/prometheus/ugv_mesh + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv15/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv15/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 85; 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv15/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv15 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv16/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv16/prometheus/ugv_mesh + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv16/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv16/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 27; 244; 120 + 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv16/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv16 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv17/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv17/prometheus/ugv_mesh + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv17/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv17/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 244; 4; 10 + 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv17/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv17 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv18/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv18/prometheus/ugv_mesh + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv18/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv18/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 24; 4; 101 + 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv18/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv18 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv19/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv19/prometheus/ugv_mesh + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv19/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv19/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 170; 12; 220 + 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv19/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv19 + - 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/PointCloud2 + Color: 255; 25; 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: lpcl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /ugv20/map_generator/local_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /ugv20/prometheus/ugv_mesh + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 233; 79 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: path_cmd + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv20/prometheus/case2/path_cmd + Unreliable: false + 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: Lines + Line Width: 0.029999999329447746 + Name: traj + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ugv20/prometheus/ugv_trajectory + Unreliable: 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/PointCloud2 + Color: 0; 170; 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: gpcl_inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: /ugv20/prometheus/planning/global_inflate_pcl + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: ugv20 + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/ThirdPersonFollower + Distance: 60.05042266845703 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -10.626058578491211 + Y: -1.4291949272155762 + Z: -1.1815851394203492e-5 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.424796462059021 + Target Frame: + Value: ThirdPersonFollower (rviz) + Yaw: 3.1253488063812256 + Saved: + - Class: rviz/FPS + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: FPS + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4000000059604645 + Position: + X: -11 + Y: 0 + Z: 8 + Target Frame: my_view + Value: FPS (rviz) + Yaw: 0 + - Class: rviz/FPS + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: FPS + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5 + Position: + X: -10 + Y: 0 + Z: 10 + Target Frame: my_view + Value: FPS (rviz) + Yaw: 0 + - Class: rviz/FPS + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: FPS + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6000000238418579 + Position: + X: -10 + Y: 0 + Z: 10 + Target Frame: my_view + Value: FPS (rviz) + Yaw: 0 +Window Geometry: + Displays: + collapsed: true + Height: 1383 + Hide Left Dock: true + Hide Right Dock: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1879 + X: 681 + Y: 27 + depth: + collapsed: false diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_4uav.launch b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_4uav.launch new file mode 100755 index 00000000..092b9c49 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_4uav.launch @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_basic.launch b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_basic.launch new file mode 100755 index 00000000..ae8a927c --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_basic.launch @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_pub_goal.launch b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_pub_goal.launch new file mode 100755 index 00000000..658bc875 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/launch_ego_planner/sitl_ego_planner_pub_goal.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/package.xml b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/package.xml new file mode 100755 index 00000000..c3776ea6 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/package.xml @@ -0,0 +1,82 @@ + + + ego_planner + 0.0.0 + The ego_planner package + + + + + iszhouxin + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + + plan_env + path_searching + bspline_opt + traj_utils + + roscpp + std_msgs + + roscpp + std_msgs + message_runtime + + plan_env + path_searching + bspline_opt + traj_utils + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src/ego_planner_node.cpp b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src/ego_planner_node.cpp new file mode 100644 index 00000000..21ef6c36 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src/ego_planner_node.cpp @@ -0,0 +1,56 @@ +#include +#include + +#include + +using namespace ego_planner; + +int main(int argc, char **argv) +{ + + ros::init(argc, argv, "ego_planner_node"); + ros::NodeHandle nh("~"); + + EGOReplanFSM rebo_replan; + + rebo_replan.init(nh); + + // ros::Duration(1.0).sleep(); + ros::spin(); + + return 0; +} + +// #include +// #include +// #include + +// #include + +// using namespace ego_planner; + +// void SignalHandler(int signal) { +// if(ros::isInitialized() && ros::isStarted() && ros::ok() && !ros::isShuttingDown()){ +// ros::shutdown(); +// } +// } + +// int main(int argc, char **argv) { + +// signal(SIGINT, SignalHandler); +// signal(SIGTERM,SignalHandler); + +// ros::init(argc, argv, "ego_planner_node", ros::init_options::NoSigintHandler); +// ros::NodeHandle nh("~"); + +// EGOReplanFSM rebo_replan; + +// rebo_replan.init(nh); + +// // ros::Duration(1.0).sleep(); +// ros::AsyncSpinner async_spinner(4); +// async_spinner.start(); +// ros::waitForShutdown(); + +// return 0; +// } \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src/ego_replan_fsm.cpp b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src/ego_replan_fsm.cpp new file mode 100644 index 00000000..febb17ec --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src/ego_replan_fsm.cpp @@ -0,0 +1,955 @@ + +#include + +namespace ego_planner +{ + + void EGOReplanFSM::init(ros::NodeHandle &nh) + { + current_wp_ = 0; + exec_state_ = FSM_EXEC_STATE::INIT; + have_target_ = false; + have_odom_ = false; + have_recv_pre_agent_ = false; + + /* fsm param */ + // 目标点类型：1，手动设定目标点；2，预设目标点 + nh.param("fsm/flight_type", target_type_, -1); + // 重规划时间间隔 + nh.param("fsm/thresh_replan_time", replan_thresh_, -1.0); + // 与目标距离小于该参数时，停止规划 + nh.param("fsm/thresh_no_replan_meter", no_replan_thresh_, -1.0); + // 规划范围 + nh.param("fsm/planning_horizon", planning_horizen_, -1.0); + // 紧急停止时间 + nh.param("fsm/emergency_time", emergency_time_, 1.0); + // 仿真与实际标志位，实际中需要等待一个起始话题 + nh.param("fsm/realworld_experiment", flag_realworld_experiment_, false); + // 未知 + nh.param("fsm/fail_safe", enable_fail_safe_, true); + // 真实实验中需要等待"/traj_start_trigger"话题 + have_trigger_ = !flag_realworld_experiment_; + // 读取waypoint + nh.param("fsm/waypoint_num", waypoint_num_, -1); + for (int i = 0; i < waypoint_num_; i++) + { + nh.param("fsm/waypoint" + to_string(i) + "_x", waypoints_[i][0], -1.0); + nh.param("fsm/waypoint" + to_string(i) + "_y", waypoints_[i][1], -1.0); + nh.param("fsm/waypoint" + to_string(i) + "_z", waypoints_[i][2], -1.0); + } + + /* initialize main modules */ + // 显示类 + visualization_.reset(new PlanningVisualization(nh)); + // 规划器类 + planner_manager_.reset(new EGOPlannerManager); + planner_manager_->initPlanModules(nh, visualization_); + planner_manager_->deliverTrajToOptimizer(); // store trajectories + planner_manager_->setDroneIdtoOpt(); + + /* callback */ + // 规划状态机定时器 + exec_timer_ = nh.createTimer(ros::Duration(0.01), &EGOReplanFSM::execFSMCallback, this); + // 安全检查定时器 + safety_timer_ = nh.createTimer(ros::Duration(0.05), &EGOReplanFSM::checkCollisionCallback, this); + // 订阅里程计 + odom_sub_ = nh.subscribe("odom_world", 1, &EGOReplanFSM::odometryCallback, this); + + // 订阅其他无人机位置 + // ego默认从0开始，我们默认从1开始，因此这里>2 + if (planner_manager_->pp_.drone_id >= 2) + { + string sub_topic_name = string("/uav") + std::to_string(planner_manager_->pp_.drone_id - 1) + string("_planning/swarm_trajs"); + swarm_trajs_sub_ = nh.subscribe(sub_topic_name.c_str(), 10, &EGOReplanFSM::swarmTrajsCallback, this, ros::TransportHints().tcpNoDelay()); + } + string pub_topic_name = string("/uav") + std::to_string(planner_manager_->pp_.drone_id) + string("_planning/swarm_trajs"); + swarm_trajs_pub_ = nh.advertise(pub_topic_name.c_str(), 10); + + broadcast_bspline_pub_ = nh.advertise("planning/broadcast_bspline_from_planner", 10); + broadcast_bspline_sub_ = nh.subscribe("planning/broadcast_bspline_to_planner", 100, &EGOReplanFSM::BroadcastBsplineCallback, this, ros::TransportHints().tcpNoDelay()); + + bspline_pub_ = nh.advertise("planning/bspline", 10); + data_disp_pub_ = nh.advertise("planning/data_display", 100); + + if (target_type_ == TARGET_TYPE::MANUAL_TARGET) + { + string waypoint_topic_name = string("/uav") + std::to_string(planner_manager_->pp_.drone_id) + string("/prometheus/motion_planning/goal"); + waypoint_sub_ = nh.subscribe(waypoint_topic_name.c_str(), 1, &EGOReplanFSM::waypointCallback, this); + } + else if (target_type_ == TARGET_TYPE::PRESET_TARGET) + { + trigger_sub_ = nh.subscribe("/uav" + std::to_string(planner_manager_->pp_.drone_id) + "/traj_start_trigger", 1, &EGOReplanFSM::triggerCallback, this); + + ROS_INFO("Wait for 1 second."); + int count = 0; + while (ros::ok() && count++ < 1000) + { + ros::spinOnce(); + ros::Duration(0.001).sleep(); + } + + ROS_WARN("Waiting for trigger from [n3ctrl] from RC"); + + while (ros::ok() && (!have_odom_ || !have_trigger_)) + { + ros::spinOnce(); + ros::Duration(0.001).sleep(); + } + + readGivenWps(); + } + else + cout << "Wrong target_type_ value! target_type_=" << target_type_ << endl; + } + + void EGOReplanFSM::readGivenWps() + { + if (waypoint_num_ <= 0) + { + ROS_ERROR("Wrong waypoint_num_ = %d", waypoint_num_); + return; + } + + wps_.resize(waypoint_num_); + for (int i = 0; i < waypoint_num_; i++) + { + wps_[i](0) = waypoints_[i][0]; + wps_[i](1) = waypoints_[i][1]; + wps_[i](2) = waypoints_[i][2]; + } + + for (size_t i = 0; i < (size_t)waypoint_num_; i++) + { + // 发布目标点用于显示 "/drone_x_ego_planner_node/goal_point" - [目标点,颜色,大小,id] + visualization_->displayGoalPoint(wps_[i], Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, i); + ros::Duration(0.001).sleep(); + } + + // 执行第一个路径点 + wp_id_ = 0; + planNextWaypoint(wps_[wp_id_]); + } + + void EGOReplanFSM::planNextWaypoint(const Eigen::Vector3d next_wp) + { + bool success = false; + // planGlobalTraj(起始位置\速度\加速度,终点位置\速度\加速度) + success = planner_manager_->planGlobalTraj(odom_pos_, odom_vel_, Eigen::Vector3d::Zero(), next_wp, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); + + if (success) + { + end_pt_ = next_wp; + + constexpr double step_size_t = 0.1; + // planner_manager_->global_data_.global_duration_是整段轨迹的预估时间 + int i_end = floor(planner_manager_->global_data_.global_duration_ / step_size_t); + vector gloabl_traj(i_end); + for (int i = 0; i < i_end; i++) + { + // 按照step_size_t提取路径点,按照时间分布 + gloabl_traj[i] = planner_manager_->global_data_.global_traj_.evaluate(i * step_size_t); + } + + end_vel_.setZero(); + have_target_ = true; + have_new_target_ = true; + + /*** FSM ***/ + if (exec_state_ == WAIT_TARGET) + changeFSMExecState(GEN_NEW_TRAJ, "TRIG"); + else + { + while (exec_state_ != EXEC_TRAJ) + { + ros::spinOnce(); + ros::Duration(0.001).sleep(); + } + changeFSMExecState(REPLAN_TRAJ, "TRIG"); + } + + // 发布GlobalPath用于显示 "/drone_x_ego_planner_node/global_list" - [GlobalPath,大小,id] + visualization_->displayGlobalPathList(gloabl_traj, 0.1, 0); + } + else + { + ROS_ERROR("Unable to generate global trajectory!"); + } + } + + void EGOReplanFSM::triggerCallback(const geometry_msgs::PoseStampedPtr &msg) + { + have_trigger_ = true; + cout << "Triggered!" << endl; + init_pt_ = odom_pos_; + } + + void EGOReplanFSM::waypointCallback(const geometry_msgs::PoseStampedPtr &msg) + { + if (msg->pose.position.z < -0.1) + return; + + if(msg->pose.position.x == 99.99 && msg->pose.position.y == 99.99) + { + callEmergencyStop(odom_pos_); + have_target_ = false; + exec_state_ = WAIT_TARGET; + return; + } + + callEmergencyStop(odom_pos_); + sleep(2.0); + + cout << "EGO: Get goal!" << endl; + init_pt_ = odom_pos_; + + // 此处定高1米 + Eigen::Vector3d end_wp(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z); + + // 发布目标点用于显示 - [目标点,颜色,大小,id] + visualization_->displayGoalPoint(end_wp, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 1); + + planNextWaypoint(end_wp); + } + + void EGOReplanFSM::odometryCallback(const nav_msgs::OdometryConstPtr &msg) + { + odom_pos_(0) = msg->pose.pose.position.x; + odom_pos_(1) = msg->pose.pose.position.y; + odom_pos_(2) = msg->pose.pose.position.z; + + odom_vel_(0) = msg->twist.twist.linear.x; + odom_vel_(1) = msg->twist.twist.linear.y; + odom_vel_(2) = msg->twist.twist.linear.z; + + //odom_acc_ = estimateAcc( msg ); + + odom_orient_.w() = msg->pose.pose.orientation.w; + odom_orient_.x() = msg->pose.pose.orientation.x; + odom_orient_.y() = msg->pose.pose.orientation.y; + odom_orient_.z() = msg->pose.pose.orientation.z; + + have_odom_ = true; + } + + void EGOReplanFSM::BroadcastBsplineCallback(const traj_utils::BsplinePtr &msg) + { + // printf("BroadcastBsplineCallback!\n"); + size_t id = msg->drone_id; + if ((int)id == planner_manager_->pp_.drone_id) + return; + + if (abs((ros::Time::now() - msg->start_time).toSec()) > 0.25) + { + ROS_ERROR("Time difference is too large! Local - Remote Agent %d = %fs", + msg->drone_id, (ros::Time::now() - msg->start_time).toSec()); + return; + } + + /* Fill up the buffer */ + if (planner_manager_->swarm_trajs_buf_.size() <= id) + { + for (size_t i = planner_manager_->swarm_trajs_buf_.size(); i <= id; i++) + { + OneTrajDataOfSwarm blank; + blank.drone_id = -1; + planner_manager_->swarm_trajs_buf_.push_back(blank); + } + } + + /* Test distance to the agent */ + Eigen::Vector3d cp0(msg->pos_pts[0].x, msg->pos_pts[0].y, msg->pos_pts[0].z); + Eigen::Vector3d cp1(msg->pos_pts[1].x, msg->pos_pts[1].y, msg->pos_pts[1].z); + Eigen::Vector3d cp2(msg->pos_pts[2].x, msg->pos_pts[2].y, msg->pos_pts[2].z); + Eigen::Vector3d swarm_start_pt = (cp0 + 4 * cp1 + cp2) / 6; + if ((swarm_start_pt - odom_pos_).norm() > planning_horizen_ * 4.0f / 3.0f) + { + planner_manager_->swarm_trajs_buf_[id].drone_id = -1; + return; // if the current drone is too far to the received agent. + } + + /* Store data */ + Eigen::MatrixXd pos_pts(3, msg->pos_pts.size()); + Eigen::VectorXd knots(msg->knots.size()); + for (size_t j = 0; j < msg->knots.size(); ++j) + { + knots(j) = msg->knots[j]; + } + for (size_t j = 0; j < msg->pos_pts.size(); ++j) + { + pos_pts(0, j) = msg->pos_pts[j].x; + pos_pts(1, j) = msg->pos_pts[j].y; + pos_pts(2, j) = msg->pos_pts[j].z; + } + + planner_manager_->swarm_trajs_buf_[id].drone_id = id; + + if (msg->order % 2) + { + double cutback = (double)msg->order / 2 + 1.5; + planner_manager_->swarm_trajs_buf_[id].duration_ = msg->knots[msg->knots.size() - ceil(cutback)]; + } + else + { + double cutback = (double)msg->order / 2 + 1.5; + planner_manager_->swarm_trajs_buf_[id].duration_ = (msg->knots[msg->knots.size() - floor(cutback)] + msg->knots[msg->knots.size() - ceil(cutback)]) / 2; + } + + UniformBspline pos_traj(pos_pts, msg->order, msg->knots[1] - msg->knots[0]); + pos_traj.setKnot(knots); + planner_manager_->swarm_trajs_buf_[id].position_traj_ = pos_traj; + + planner_manager_->swarm_trajs_buf_[id].start_pos_ = planner_manager_->swarm_trajs_buf_[id].position_traj_.evaluateDeBoorT(0); + + planner_manager_->swarm_trajs_buf_[id].start_time_ = msg->start_time; + // planner_manager_->swarm_trajs_buf_[id].start_time_ = ros::Time::now(); // Un-reliable time sync + + /* Check Collision */ + // printf("Check Collision\n"); + if (planner_manager_->checkCollision(id)) + { + changeFSMExecState(REPLAN_TRAJ, "TRAJ_CHECK"); + } + } + + void EGOReplanFSM::swarmTrajsCallback(const traj_utils::MultiBsplinesPtr &msg) + { + + multi_bspline_msgs_buf_.traj.clear(); + multi_bspline_msgs_buf_ = *msg; + + // cout << "\033[45;33mmulti_bspline_msgs_buf.drone_id_from=" << multi_bspline_msgs_buf_.drone_id_from << " multi_bspline_msgs_buf_.traj.size()=" << multi_bspline_msgs_buf_.traj.size() << "\033[0m" << endl; + + if (!have_odom_) + { + ROS_ERROR("swarmTrajsCallback(): no odom!, return."); + return; + } + + if ((int)msg->traj.size() != msg->drone_id_from + 1) // drone_id must start from 0 + { + ROS_ERROR("Wrong trajectory size! msg->traj.size()=%d, msg->drone_id_from+1=%d", (int)msg->traj.size(), msg->drone_id_from + 1); + return; + } + + if (msg->traj[0].order != 3) // only support B-spline order equals 3. + { + ROS_ERROR("Only support B-spline order equals 3."); + return; + } + + // Step 1. receive the trajectories + planner_manager_->swarm_trajs_buf_.clear(); + planner_manager_->swarm_trajs_buf_.resize(msg->traj.size()); + + for (size_t i = 0; i < msg->traj.size(); i++) + { + + Eigen::Vector3d cp0(msg->traj[i].pos_pts[0].x, msg->traj[i].pos_pts[0].y, msg->traj[i].pos_pts[0].z); + Eigen::Vector3d cp1(msg->traj[i].pos_pts[1].x, msg->traj[i].pos_pts[1].y, msg->traj[i].pos_pts[1].z); + Eigen::Vector3d cp2(msg->traj[i].pos_pts[2].x, msg->traj[i].pos_pts[2].y, msg->traj[i].pos_pts[2].z); + Eigen::Vector3d swarm_start_pt = (cp0 + 4 * cp1 + cp2) / 6; + if ((swarm_start_pt - odom_pos_).norm() > planning_horizen_ * 4.0f / 3.0f) + { + planner_manager_->swarm_trajs_buf_[i].drone_id = -1; + continue; + } + + Eigen::MatrixXd pos_pts(3, msg->traj[i].pos_pts.size()); + Eigen::VectorXd knots(msg->traj[i].knots.size()); + for (size_t j = 0; j < msg->traj[i].knots.size(); ++j) + { + knots(j) = msg->traj[i].knots[j]; + } + for (size_t j = 0; j < msg->traj[i].pos_pts.size(); ++j) + { + pos_pts(0, j) = msg->traj[i].pos_pts[j].x; + pos_pts(1, j) = msg->traj[i].pos_pts[j].y; + pos_pts(2, j) = msg->traj[i].pos_pts[j].z; + } + + planner_manager_->swarm_trajs_buf_[i].drone_id = i; + + if (msg->traj[i].order % 2) + { + double cutback = (double)msg->traj[i].order / 2 + 1.5; + planner_manager_->swarm_trajs_buf_[i].duration_ = msg->traj[i].knots[msg->traj[i].knots.size() - ceil(cutback)]; + } + else + { + double cutback = (double)msg->traj[i].order / 2 + 1.5; + planner_manager_->swarm_trajs_buf_[i].duration_ = (msg->traj[i].knots[msg->traj[i].knots.size() - floor(cutback)] + msg->traj[i].knots[msg->traj[i].knots.size() - ceil(cutback)]) / 2; + } + + // planner_manager_->swarm_trajs_buf_[i].position_traj_ = + UniformBspline pos_traj(pos_pts, msg->traj[i].order, msg->traj[i].knots[1] - msg->traj[i].knots[0]); + pos_traj.setKnot(knots); + planner_manager_->swarm_trajs_buf_[i].position_traj_ = pos_traj; + + planner_manager_->swarm_trajs_buf_[i].start_pos_ = planner_manager_->swarm_trajs_buf_[i].position_traj_.evaluateDeBoorT(0); + + planner_manager_->swarm_trajs_buf_[i].start_time_ = msg->traj[i].start_time; + } + + have_recv_pre_agent_ = true; + + printf("have_recv_pre_agent_==true\n"); + } + + void EGOReplanFSM::changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call) + { + + if (new_state == exec_state_) + continously_called_times_++; + else + continously_called_times_ = 1; + + static string state_str[8] = {"INIT", "WAIT_TARGET", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ", "EMERGENCY_STOP", "SEQUENTIAL_START"}; + int pre_s = int(exec_state_); + exec_state_ = new_state; + // COMMENT + cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << endl; + } + + std::pair EGOReplanFSM::timesOfConsecutiveStateCalls() + { + return std::pair(continously_called_times_, exec_state_); + } + + void EGOReplanFSM::printFSMExecState() + { + static string state_str[8] = {"INIT", "WAIT_TARGET", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ", "EMERGENCY_STOP", "SEQUENTIAL_START"}; + + cout << "/uav"<< planner_manager_->pp_.drone_id <<" planner state: " + state_str[int(exec_state_)] << endl; + } + + void EGOReplanFSM::execFSMCallback(const ros::TimerEvent &e) + { + // 暂停计时 + exec_timer_.stop(); // To avoid blockage (阻塞) + + static int fsm_num = 0; + fsm_num++; + if (fsm_num == 1000) + { + printFSMExecState(); + if (!have_odom_) + cout << "no odom." << endl; + if (!have_target_) + // cout << "wait for goal or trigger." << endl; + fsm_num = 0; + } + + switch (exec_state_) + { + case INIT: + { + if (!have_odom_) + { + goto force_return; + // return; + } + changeFSMExecState(WAIT_TARGET, "FSM"); + break; + } + + case WAIT_TARGET: + { + if (!have_target_ || !have_trigger_) + goto force_return; + // return; + else + { + changeFSMExecState(SEQUENTIAL_START, "FSM"); + } + break; + } + + // 按顺序启动 + case SEQUENTIAL_START: // for swarm + { + // swarmTrajsCallback回调后,have_recv_pre_agent_会被设置为true + // ego默认从0开始，我们默认从1开始，因此这里>2 + if (planner_manager_->pp_.drone_id <= 1 || (planner_manager_->pp_.drone_id >= 2&& have_recv_pre_agent_)) + { + if (have_odom_ && have_target_ && have_trigger_) + { + bool success = planFromGlobalTraj(10); // zx-todo + if (success) + { + changeFSMExecState(EXEC_TRAJ, "FSM"); + + publishSwarmTrajs(true); + } + else + { + ROS_ERROR("Failed to generate the first trajectory!!!"); + changeFSMExecState(SEQUENTIAL_START, "FSM"); + } + } + else + { + ROS_ERROR("No odom or no target! have_odom_=%d, have_target_=%d", have_odom_, have_target_); + } + } + + break; + } + + case GEN_NEW_TRAJ: + { + + // Eigen::Vector3d rot_x = odom_orient_.toRotationMatrix().block(0, 0, 3, 1); + // start_yaw_(0) = atan2(rot_x(1), rot_x(0)); + // start_yaw_(1) = start_yaw_(2) = 0.0; + + bool success = planFromGlobalTraj(10); // zx-todo + if (success) + { + changeFSMExecState(EXEC_TRAJ, "FSM"); + flag_escape_emergency_ = true; + publishSwarmTrajs(false); + } + else + { + changeFSMExecState(GEN_NEW_TRAJ, "FSM"); + } + break; + } + + case REPLAN_TRAJ: + { + + if (planFromCurrentTraj(1)) + { + changeFSMExecState(EXEC_TRAJ, "FSM"); + publishSwarmTrajs(false); + } + else + { + changeFSMExecState(REPLAN_TRAJ, "FSM"); + } + + break; + } + + case EXEC_TRAJ: + { + /* determine if need to replan */ + LocalTrajData *info = &planner_manager_->local_data_; + ros::Time time_now = ros::Time::now(); + double t_cur = (time_now - info->start_time_).toSec(); + t_cur = min(info->duration_, t_cur); + + // 当前位置从轨迹中读取,可改为从odom中读取 + Eigen::Vector3d pos = info->position_traj_.evaluateDeBoorT(t_cur); + + /* && (end_pt_ - pos).norm() < 0.5 */ + if ((target_type_ == TARGET_TYPE::PRESET_TARGET) && + (wp_id_ < waypoint_num_ - 1) && + (end_pt_ - pos).norm() < no_replan_thresh_) + { + wp_id_++; + planNextWaypoint(wps_[wp_id_]); + } + else if ((local_target_pt_ - end_pt_).norm() < 1e-3) // end_pt_是目标点， + { + if (t_cur > info->duration_ - 1e-2) + { + have_target_ = false; + have_trigger_ = false; + + if (target_type_ == TARGET_TYPE::PRESET_TARGET) + { + wp_id_ = 0; + planNextWaypoint(wps_[wp_id_]); + } + + changeFSMExecState(WAIT_TARGET, "FSM"); + goto force_return; + // return; + } + else if ((end_pt_ - pos).norm() > no_replan_thresh_ && t_cur > replan_thresh_) + { + changeFSMExecState(REPLAN_TRAJ, "FSM"); + } + } + else if (t_cur > replan_thresh_) + { + changeFSMExecState(REPLAN_TRAJ, "FSM"); + } + + break; + } + + case EMERGENCY_STOP: + { + + if (flag_escape_emergency_) // Avoiding repeated calls + { + callEmergencyStop(odom_pos_); + } + else + { + if (enable_fail_safe_ && odom_vel_.norm() < 0.1) + changeFSMExecState(GEN_NEW_TRAJ, "FSM"); + } + + flag_escape_emergency_ = false; + break; + } + } + + data_disp_.header.stamp = ros::Time::now(); + data_disp_pub_.publish(data_disp_); + + force_return:; + exec_timer_.start(); + } + + bool EGOReplanFSM::planFromGlobalTraj(const int trial_times /*=1*/) //zx-todo + { + start_pt_ = odom_pos_; + start_vel_ = odom_vel_; + start_acc_.setZero(); + + bool flag_random_poly_init; + // 如果切换了exec状态,timesOfConsecutiveStateCalls().first=1 + if (timesOfConsecutiveStateCalls().first == 1) + flag_random_poly_init = false; + else + flag_random_poly_init = true; + + for (int i = 0; i < trial_times; i++) + { + if (callReboundReplan(true, flag_random_poly_init)) + { + return true; + } + } + return false; + } + + bool EGOReplanFSM::planFromCurrentTraj(const int trial_times /*=1*/) + { + + LocalTrajData *info = &planner_manager_->local_data_; + ros::Time time_now = ros::Time::now(); + double t_cur = (time_now - info->start_time_).toSec(); + + //cout << "info->velocity_traj_=" << info->velocity_traj_.get_control_points() << endl; + + // 将start_pt改为当前位置? + start_pt_ = info->position_traj_.evaluateDeBoorT(t_cur); + start_vel_ = info->velocity_traj_.evaluateDeBoorT(t_cur); + start_acc_ = info->acceleration_traj_.evaluateDeBoorT(t_cur); + + bool success = callReboundReplan(false, false); + + if (!success) + { + success = callReboundReplan(true, false); + //changeFSMExecState(EXEC_TRAJ, "FSM"); + if (!success) + { + for (int i = 0; i < trial_times; i++) + { + success = callReboundReplan(true, true); + if (success) + break; + } + if (!success) + { + return false; + } + } + } + + return true; + } + + void EGOReplanFSM::checkCollisionCallback(const ros::TimerEvent &e) + { + + LocalTrajData *info = &planner_manager_->local_data_; + auto map = planner_manager_->grid_map_; + + if (exec_state_ == WAIT_TARGET || info->start_time_.toSec() < 1e-5) + return; + + /* ---------- check lost of depth ---------- */ + if(map->getOdomDepthTimeout()) + { + ROS_ERROR("Depth Lost! EMERGENCY_STOP"); + enable_fail_safe_ = false; + changeFSMExecState(EMERGENCY_STOP, "SAFETY"); + } + else + { + if(enable_fail_safe_ == false) + { + ROS_INFO("Depth Get! GEN_NEW_TRAJ"); + enable_fail_safe_ = true; + } + } + + /* ---------- check trajectory ---------- */ + constexpr double time_step = 0.01; + double t_cur = (ros::Time::now() - info->start_time_).toSec(); + Eigen::Vector3d p_cur = info->position_traj_.evaluateDeBoorT(t_cur); + const double CLEARANCE = 1.0 * planner_manager_->getSwarmClearance(); + double t_cur_global = ros::Time::now().toSec(); + double t_2_3 = info->duration_ * 2 / 3; + for (double t = t_cur; t < info->duration_; t += time_step) + { + if (t_cur < t_2_3 && t >= t_2_3) // If t_cur < t_2_3, only the first 2/3 partition of the trajectory is considered valid and will get checked. + break; + + bool occ = false; + occ |= map->getInflateOccupancy(info->position_traj_.evaluateDeBoorT(t)); + + for (size_t id = 0; id < planner_manager_->swarm_trajs_buf_.size(); id++) + { + if ((planner_manager_->swarm_trajs_buf_.at(id).drone_id != (int)id) || (planner_manager_->swarm_trajs_buf_.at(id).drone_id == planner_manager_->pp_.drone_id)) + { + continue; + } + + double t_X = t_cur_global - planner_manager_->swarm_trajs_buf_.at(id).start_time_.toSec(); + Eigen::Vector3d swarm_pridicted = planner_manager_->swarm_trajs_buf_.at(id).position_traj_.evaluateDeBoorT(t_X); + double dist = (p_cur - swarm_pridicted).norm(); + + if (dist < CLEARANCE) + { + occ = true; + break; + } + } + + if (occ) + { + + if (planFromCurrentTraj()) // Make a chance + { + changeFSMExecState(EXEC_TRAJ, "SAFETY"); + publishSwarmTrajs(false); + return; + } + else + { + if (t - t_cur < emergency_time_) // 0.8s of emergency time + { + ROS_WARN("Suddenly discovered obstacles. emergency stop! time=%f", t - t_cur); + changeFSMExecState(EMERGENCY_STOP, "SAFETY"); + } + else + { + //ROS_WARN("current traj in collision, replan."); + changeFSMExecState(REPLAN_TRAJ, "SAFETY"); + } + return; + } + break; + } + } + } + + bool EGOReplanFSM::callReboundReplan(bool flag_use_poly_init, bool flag_randomPolyTraj) + { + // 根据planning_horizen_来确定局部目标点 local_target_pt_,local_target_vel_ + getLocalTarget(); + + // + bool plan_and_refine_success = + planner_manager_->reboundReplan(start_pt_, start_vel_, start_acc_, local_target_pt_, local_target_vel_, (have_new_target_ || flag_use_poly_init), flag_randomPolyTraj); + have_new_target_ = false; + + // comment + // cout << "refine_success=" << plan_and_refine_success << endl; + + if (plan_and_refine_success) + { + + auto info = &planner_manager_->local_data_; + + traj_utils::Bspline bspline; + bspline.order = 3; + bspline.start_time = info->start_time_; + bspline.traj_id = info->traj_id_; + + Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint(); + bspline.pos_pts.reserve(pos_pts.cols()); + for (int i = 0; i < pos_pts.cols(); ++i) + { + geometry_msgs::Point pt; + pt.x = pos_pts(0, i); + pt.y = pos_pts(1, i); + pt.z = pos_pts(2, i); + bspline.pos_pts.push_back(pt); + } + + Eigen::VectorXd knots = info->position_traj_.getKnot(); + // cout << knots.transpose() << endl; + bspline.knots.reserve(knots.rows()); + for (int i = 0; i < knots.rows(); ++i) + { + bspline.knots.push_back(knots(i)); + } + + /* 1. publish traj to traj_server */ + bspline_pub_.publish(bspline); + + /* 2. publish traj to the next drone of swarm */ + + /* 3. publish traj for visualization */ + // 发布优化后的轨迹 "/optimal_list" , 颜色和scale已经内置 + visualization_->displayOptimalList(info->position_traj_.get_control_points(), 0); + } + + return plan_and_refine_success; + } + + void EGOReplanFSM::publishSwarmTrajs(bool startup_pub) + { + auto info = &planner_manager_->local_data_; + + traj_utils::Bspline bspline; + bspline.order = 3; + bspline.start_time = info->start_time_; + bspline.drone_id = planner_manager_->pp_.drone_id; + bspline.traj_id = info->traj_id_; + + Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint(); + bspline.pos_pts.reserve(pos_pts.cols()); + for (int i = 0; i < pos_pts.cols(); ++i) + { + geometry_msgs::Point pt; + pt.x = pos_pts(0, i); + pt.y = pos_pts(1, i); + pt.z = pos_pts(2, i); + bspline.pos_pts.push_back(pt); + } + + Eigen::VectorXd knots = info->position_traj_.getKnot(); + // cout << knots.transpose() << endl; + bspline.knots.reserve(knots.rows()); + for (int i = 0; i < knots.rows(); ++i) + { + bspline.knots.push_back(knots(i)); + } + + if (startup_pub) + { + multi_bspline_msgs_buf_.drone_id_from = planner_manager_->pp_.drone_id; // zx-todo + if ((int)multi_bspline_msgs_buf_.traj.size() == planner_manager_->pp_.drone_id + 1) + { + multi_bspline_msgs_buf_.traj.back() = bspline; + } + else if ((int)multi_bspline_msgs_buf_.traj.size() == planner_manager_->pp_.drone_id) + { + multi_bspline_msgs_buf_.traj.push_back(bspline); + } + else + { + ROS_ERROR("Wrong traj nums and drone_id pair!!! traj.size()=%d, drone_id=%d", (int)multi_bspline_msgs_buf_.traj.size(), planner_manager_->pp_.drone_id); + // return plan_and_refine_success; + } + swarm_trajs_pub_.publish(multi_bspline_msgs_buf_); + } + + broadcast_bspline_pub_.publish(bspline); + } + + bool EGOReplanFSM::callEmergencyStop(Eigen::Vector3d stop_pos) + { + + planner_manager_->EmergencyStop(stop_pos); + + auto info = &planner_manager_->local_data_; + + /* publish traj */ + traj_utils::Bspline bspline; + bspline.order = 3; + bspline.start_time = info->start_time_; + bspline.traj_id = info->traj_id_; + + Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint(); + bspline.pos_pts.reserve(pos_pts.cols()); + for (int i = 0; i < pos_pts.cols(); ++i) + { + geometry_msgs::Point pt; + pt.x = pos_pts(0, i); + pt.y = pos_pts(1, i); + pt.z = pos_pts(2, i); + bspline.pos_pts.push_back(pt); + } + + Eigen::VectorXd knots = info->position_traj_.getKnot(); + bspline.knots.reserve(knots.rows()); + for (int i = 0; i < knots.rows(); ++i) + { + bspline.knots.push_back(knots(i)); + } + + bspline_pub_.publish(bspline); + + return true; + } + + void EGOReplanFSM::getLocalTarget() + { + double t; + + double t_step = planning_horizen_ / 20 / planner_manager_->pp_.max_vel_; + double dist_min = 9999, dist_min_t = 0.0; + for (t = planner_manager_->global_data_.last_progress_time_; t < planner_manager_->global_data_.global_duration_; t += t_step) + { + Eigen::Vector3d pos_t = planner_manager_->global_data_.getPosition(t); + double dist = (pos_t - start_pt_).norm(); + + if (t < planner_manager_->global_data_.last_progress_time_ + 1e-5 && dist > planning_horizen_) + { + // Important conor case! + for (; t < planner_manager_->global_data_.global_duration_; t += t_step) + { + Eigen::Vector3d pos_t_temp = planner_manager_->global_data_.getPosition(t); + double dist_temp = (pos_t_temp - start_pt_).norm(); + if (dist_temp < planning_horizen_) + { + pos_t = pos_t_temp; + dist = (pos_t - start_pt_).norm(); + cout << "Escape conor case \"getLocalTarget\"" << endl; + break; + } + } + } + + if (dist < dist_min) + { + dist_min = dist; + dist_min_t = t; + } + + if (dist >= planning_horizen_) + { + local_target_pt_ = pos_t; + planner_manager_->global_data_.last_progress_time_ = dist_min_t; + break; + } + } + if (t > planner_manager_->global_data_.global_duration_) // Last global point + { + local_target_pt_ = end_pt_; + planner_manager_->global_data_.last_progress_time_ = planner_manager_->global_data_.global_duration_; + } + + if ((end_pt_ - local_target_pt_).norm() < (planner_manager_->pp_.max_vel_ * planner_manager_->pp_.max_vel_) / (2 * planner_manager_->pp_.max_acc_)) + { + local_target_vel_ = Eigen::Vector3d::Zero(); + } + else + { + local_target_vel_ = planner_manager_->global_data_.getVelocity(t); + } + } + +} // namespace ego_planner diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src/ego_traj_to_cmd.cpp b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src/ego_traj_to_cmd.cpp new file mode 100644 index 00000000..1dd8e29b --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src/ego_traj_to_cmd.cpp @@ -0,0 +1,103 @@ +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "prometheus_msgs/DroneState.h" +#include "prometheus_msgs/SwarmCommand.h" +#include "quadrotor_msgs/PositionCommand.h" +#include "std_msgs/Empty.h" +#include "uav_utils/geometry_utils.h" +#include "printf_utils.h" + +// 无人机名字 +string uav_name; +// 无人机编号 +int uav_id; +int control_flag; +// ego输出 +quadrotor_msgs::PositionCommand ego_traj_cmd; +bool get_ego_traj; +// 发布的控制指令 +prometheus_msgs::SwarmCommand Command_Now; + +ros::Subscriber ego_ouput_sub; +ros::Publisher command_pub; + +void ego_ouput_cb(const quadrotor_msgs::PositionCommand::ConstPtr& msg) +{ + ego_traj_cmd = *msg; + get_ego_traj = true; + + if(ego_traj_cmd.velocity.x == 0) + { + get_ego_traj = false; + // cout << YELLOW << " Arrive the goal " << TAIL <>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< +int main(int argc, char **argv) +{ + ros::init(argc, argv, "ego_traj_to_cmd"); + ros::NodeHandle nh("~"); + ros::Rate rate(100.0); + + // 无人机编号 1号无人机则为1 + nh.param("uav_id", uav_id, 0); + nh.param("control_flag", control_flag, 0); + + uav_name = "/uav" + std::to_string(uav_id); + + // 【订阅】EGO的轨迹输出(traj_server的输出) + ego_ouput_sub = nh.subscribe(uav_name + "/prometheus/ego/traj_cmd", 1, ego_ouput_cb); + + // 【发布】 路径指令 （发送至swarm_controller.cpp） + command_pub = nh.advertise(uav_name + "/prometheus/swarm_command", 1); + + while(ros::ok()) + { + ros::spinOnce(); + rate.sleep(); + } +} + diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src/planner_manager.cpp b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src/planner_manager.cpp new file mode 100644 index 00000000..bd09892d --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src/planner_manager.cpp @@ -0,0 +1,600 @@ +// #include +#include +#include +#include "visualization_msgs/Marker.h" // zx-todo + +namespace ego_planner +{ + + // SECTION interfaces for setup and query + + EGOPlannerManager::EGOPlannerManager() {} + + EGOPlannerManager::~EGOPlannerManager() {} + + void EGOPlannerManager::initPlanModules(ros::NodeHandle &nh, PlanningVisualization::Ptr vis) + { + /* read algorithm parameters */ + // 最大速度 + nh.param("manager/max_vel", pp_.max_vel_, -1.0); + // 最大加速度 + nh.param("manager/max_acc", pp_.max_acc_, -1.0); + // 最大加加速度 + nh.param("manager/max_jerk", pp_.max_jerk_, -1.0); + // 未知，限制参数 + nh.param("manager/feasibility_tolerance", pp_.feasibility_tolerance_, 0.0); + // 控制点距离 + nh.param("manager/control_points_distance", pp_.ctrl_pt_dist, -1.0); + // 规划范围 + nh.param("manager/planning_horizon", pp_.planning_horizen_, 5.0); + // ？ + nh.param("manager/use_distinctive_trajs", pp_.use_distinctive_trajs, false); + // 无人机ID + nh.param("manager/drone_id", pp_.drone_id, 0); + + local_data_.traj_id_ = 0; + // 地图类 + grid_map_.reset(new GridMap); + grid_map_->initMap(nh); + + // 默认没有使用 + // obj_predictor_.reset(new fast_planner::ObjPredictor(nh)); + // obj_predictor_->init(); + // obj_pub_ = nh.advertise("/dynamic/obj_prdi", 10); // zx-todo + + // bspline优化器 + bspline_optimizer_.reset(new BsplineOptimizer); + bspline_optimizer_->setParam(nh); + bspline_optimizer_->setEnvironment(grid_map_, obj_predictor_); + bspline_optimizer_->a_star_.reset(new AStar); + bspline_optimizer_->a_star_->initGridMap(grid_map_, Eigen::Vector3i(100, 100, 100)); + + visualization_ = vis; + } + + // !SECTION + + // SECTION rebond replanning + + bool EGOPlannerManager::reboundReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel, + Eigen::Vector3d start_acc, Eigen::Vector3d local_target_pt, + Eigen::Vector3d local_target_vel, bool flag_polyInit, bool flag_randomPolyTraj) + { + static int count = 0; + // comment + // printf("\033[47;30m\n[drone no.%d , replan %d times]====================================\033[0m\n", pp_.drone_id, count++); + // cout.precision(3); + // cout << "start: " << start_pt.transpose() << ", " << start_vel.transpose() << "\ngoal:" << local_target_pt.transpose() << ", " << local_target_vel.transpose() + // << endl; + + if ((start_pt - local_target_pt).norm() < 0.2) + { + cout << "The start point is too Close to goal" << endl; + continous_failures_count_++; + return false; + } + + bspline_optimizer_->setLocalTargetPt(local_target_pt); + + ros::Time t_start = ros::Time::now(); + ros::Duration t_init, t_opt, t_refine; + + /*** STEP 1: INIT ***/ + double ts = (start_pt - local_target_pt).norm() > 0.1 ? pp_.ctrl_pt_dist / pp_.max_vel_ * 1.5 : pp_.ctrl_pt_dist / pp_.max_vel_ * 5; // pp_.ctrl_pt_dist / pp_.max_vel_ is too tense, and will surely exceed the acc/vel limits + vector point_set, start_end_derivatives; + static bool flag_first_call = true, flag_force_polynomial = false; + bool flag_regenerate = false; + do + { + point_set.clear(); + start_end_derivatives.clear(); + flag_regenerate = false; + + if (flag_first_call || flag_polyInit || flag_force_polynomial /*|| ( start_pt - local_target_pt ).norm() < 1.0*/) // Initial path generated from a min-snap traj by order. + { + flag_first_call = false; + flag_force_polynomial = false; + + PolynomialTraj gl_traj; + + double dist = (start_pt - local_target_pt).norm(); + double time = pow(pp_.max_vel_, 2) / pp_.max_acc_ > dist ? sqrt(dist / pp_.max_acc_) : (dist - pow(pp_.max_vel_, 2) / pp_.max_acc_) / pp_.max_vel_ + 2 * pp_.max_vel_ / pp_.max_acc_; + + if (!flag_randomPolyTraj) + { + gl_traj = PolynomialTraj::one_segment_traj_gen(start_pt, start_vel, start_acc, local_target_pt, local_target_vel, Eigen::Vector3d::Zero(), time); + } + else + { + Eigen::Vector3d horizen_dir = ((start_pt - local_target_pt).cross(Eigen::Vector3d(0, 0, 1))).normalized(); + Eigen::Vector3d vertical_dir = ((start_pt - local_target_pt).cross(horizen_dir)).normalized(); + Eigen::Vector3d random_inserted_pt = (start_pt + local_target_pt) / 2 + + (((double)rand()) / RAND_MAX - 0.5) * (start_pt - local_target_pt).norm() * horizen_dir * 0.8 * (-0.978 / (continous_failures_count_ + 0.989) + 0.989) + + (((double)rand()) / RAND_MAX - 0.5) * (start_pt - local_target_pt).norm() * vertical_dir * 0.4 * (-0.978 / (continous_failures_count_ + 0.989) + 0.989); + Eigen::MatrixXd pos(3, 3); + pos.col(0) = start_pt; + pos.col(1) = random_inserted_pt; + pos.col(2) = local_target_pt; + Eigen::VectorXd t(2); + t(0) = t(1) = time / 2; + gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, local_target_vel, start_acc, Eigen::Vector3d::Zero(), t); + } + + double t; + bool flag_too_far; + ts *= 1.5; // ts will be divided by 1.5 in the next + do + { + ts /= 1.5; + point_set.clear(); + flag_too_far = false; + Eigen::Vector3d last_pt = gl_traj.evaluate(0); + for (t = 0; t < time; t += ts) + { + Eigen::Vector3d pt = gl_traj.evaluate(t); + if ((last_pt - pt).norm() > pp_.ctrl_pt_dist * 1.5) + { + flag_too_far = true; + break; + } + last_pt = pt; + point_set.push_back(pt); + } + } while (flag_too_far || point_set.size() < 7); // To make sure the initial path has enough points. + t -= ts; + start_end_derivatives.push_back(gl_traj.evaluateVel(0)); + start_end_derivatives.push_back(local_target_vel); + start_end_derivatives.push_back(gl_traj.evaluateAcc(0)); + start_end_derivatives.push_back(gl_traj.evaluateAcc(t)); + } + else // Initial path generated from previous trajectory. + { + + double t; + double t_cur = (ros::Time::now() - local_data_.start_time_).toSec(); + + vector pseudo_arc_length; + vector segment_point; + pseudo_arc_length.push_back(0.0); + for (t = t_cur; t < local_data_.duration_ + 1e-3; t += ts) + { + segment_point.push_back(local_data_.position_traj_.evaluateDeBoorT(t)); + if (t > t_cur) + { + pseudo_arc_length.push_back((segment_point.back() - segment_point[segment_point.size() - 2]).norm() + pseudo_arc_length.back()); + } + } + t -= ts; + + double poly_time = (local_data_.position_traj_.evaluateDeBoorT(t) - local_target_pt).norm() / pp_.max_vel_ * 2; + if (poly_time > ts) + { + PolynomialTraj gl_traj = PolynomialTraj::one_segment_traj_gen(local_data_.position_traj_.evaluateDeBoorT(t), + local_data_.velocity_traj_.evaluateDeBoorT(t), + local_data_.acceleration_traj_.evaluateDeBoorT(t), + local_target_pt, local_target_vel, Eigen::Vector3d::Zero(), poly_time); + + for (t = ts; t < poly_time; t += ts) + { + if (!pseudo_arc_length.empty()) + { + segment_point.push_back(gl_traj.evaluate(t)); + pseudo_arc_length.push_back((segment_point.back() - segment_point[segment_point.size() - 2]).norm() + pseudo_arc_length.back()); + } + else + { + ROS_ERROR("pseudo_arc_length is empty, return!"); + continous_failures_count_++; + return false; + } + } + } + + double sample_length = 0; + double cps_dist = pp_.ctrl_pt_dist * 1.5; // cps_dist will be divided by 1.5 in the next + size_t id = 0; + do + { + cps_dist /= 1.5; + point_set.clear(); + sample_length = 0; + id = 0; + while ((id <= pseudo_arc_length.size() - 2) && sample_length <= pseudo_arc_length.back()) + { + if (sample_length >= pseudo_arc_length[id] && sample_length < pseudo_arc_length[id + 1]) + { + point_set.push_back((sample_length - pseudo_arc_length[id]) / (pseudo_arc_length[id + 1] - pseudo_arc_length[id]) * segment_point[id + 1] + + (pseudo_arc_length[id + 1] - sample_length) / (pseudo_arc_length[id + 1] - pseudo_arc_length[id]) * segment_point[id]); + sample_length += cps_dist; + } + else + id++; + } + point_set.push_back(local_target_pt); + } while (point_set.size() < 7); // If the start point is very close to end point, this will help + + start_end_derivatives.push_back(local_data_.velocity_traj_.evaluateDeBoorT(t_cur)); + start_end_derivatives.push_back(local_target_vel); + start_end_derivatives.push_back(local_data_.acceleration_traj_.evaluateDeBoorT(t_cur)); + start_end_derivatives.push_back(Eigen::Vector3d::Zero()); + + if (point_set.size() > pp_.planning_horizen_ / pp_.ctrl_pt_dist * 3) // The initial path is unnormally too long! + { + flag_force_polynomial = true; + flag_regenerate = true; + } + } + } while (flag_regenerate); + + Eigen::MatrixXd ctrl_pts, ctrl_pts_temp; + UniformBspline::parameterizeToBspline(ts, point_set, start_end_derivatives, ctrl_pts); + + vector> segments; + segments = bspline_optimizer_->initControlPoints(ctrl_pts, true); + + t_init = ros::Time::now() - t_start; + t_start = ros::Time::now(); + + /*** STEP 2: OPTIMIZE ***/ + bool flag_step_1_success = false; + vector> vis_trajs; + + if (pp_.use_distinctive_trajs) + { + // cout << "enter" << endl; + std::vector trajs = bspline_optimizer_->distinctiveTrajs(segments); + // comment + // cout << "\033[1;33m"<< "multi-trajs=" << trajs.size() << "\033[1;0m" << endl; + + double final_cost, min_cost = 999999.0; + for (int i = trajs.size() - 1; i >= 0; i--) + { + if (bspline_optimizer_->BsplineOptimizeTrajRebound(ctrl_pts_temp, final_cost, trajs[i], ts)) + { + //comment + // cout << "traj " << trajs.size() - i << " success." << endl; + + flag_step_1_success = true; + if (final_cost < min_cost) + { + min_cost = final_cost; + ctrl_pts = ctrl_pts_temp; + } + + // visualization + point_set.clear(); + for (int j = 0; j < ctrl_pts_temp.cols(); j++) + { + point_set.push_back(ctrl_pts_temp.col(j)); + } + vis_trajs.push_back(point_set); + } + else + { + cout << "traj " << trajs.size() - i << " failed." << endl; + } + } + + t_opt = ros::Time::now() - t_start; + + // 显示 "/init_list" + visualization_->displayMultiInitPathList(vis_trajs, 0.2); // This visuallization will take up several milliseconds. + } + else + { + flag_step_1_success = bspline_optimizer_->BsplineOptimizeTrajRebound(ctrl_pts, ts); + t_opt = ros::Time::now() - t_start; + //static int vis_id = 0; + visualization_->displayInitPathList(point_set, 0.2, 0); + } + // comment + // cout << "/uav"<< pp_.drone_id<< " plan_success=" << flag_step_1_success << endl; + if (!flag_step_1_success) + { + visualization_->displayOptimalList(ctrl_pts, 0); + continous_failures_count_++; + return false; + } + + t_start = ros::Time::now(); + + UniformBspline pos = UniformBspline(ctrl_pts, 3, ts); + pos.setPhysicalLimits(pp_.max_vel_, pp_.max_acc_, pp_.feasibility_tolerance_); + + /*** STEP 3: REFINE(RE-ALLOCATE TIME) IF NECESSARY ***/ + // Note: Only adjust time in single drone mode. But we still allow drone_0 to adjust its time profile. + // ego默认从0开始，我们默认从1开始，因此这里<=1 + if (pp_.drone_id <= 1) + { + + double ratio; + bool flag_step_2_success = true; + if (!pos.checkFeasibility(ratio, false)) + { + cout << "Need to reallocate time." << endl; + + Eigen::MatrixXd optimal_control_points; + flag_step_2_success = refineTrajAlgo(pos, start_end_derivatives, ratio, ts, optimal_control_points); + if (flag_step_2_success) + pos = UniformBspline(optimal_control_points, 3, ts); + } + + if (!flag_step_2_success) + { + printf("\033[34mThis refined trajectory hits obstacles. It doesn't matter if appeares occasionally. But if continously appearing, Increase parameter \"lambda_fitness\".\n\033[0m"); + continous_failures_count_++; + return false; + } + } + else + { + static bool print_once = true; + if (print_once) + { + print_once = false; + ROS_ERROR("IN SWARM MODE, REFINE DISABLED!"); + } + } + + t_refine = ros::Time::now() - t_start; + + // save planned results + updateTrajInfo(pos, ros::Time::now()); + + static double sum_time = 0; + static int count_success = 0; + sum_time += (t_init + t_opt + t_refine).toSec(); + count_success++; + // comment + // cout << "total time:\033[42m" << (t_init + t_opt + t_refine).toSec() << "\033[0m,optimize:" << (t_init + t_opt).toSec() << ",refine:" << t_refine.toSec() << ",avg_time=" << sum_time / count_success << endl; + + // success. YoY + continous_failures_count_ = 0; + return true; + } + + bool EGOPlannerManager::EmergencyStop(Eigen::Vector3d stop_pos) + { + Eigen::MatrixXd control_points(3, 6); + for (int i = 0; i < 6; i++) + { + control_points.col(i) = stop_pos; + } + + updateTrajInfo(UniformBspline(control_points, 3, 1.0), ros::Time::now()); + + return true; + } + + bool EGOPlannerManager::checkCollision(int drone_id) + { + if (local_data_.start_time_.toSec() < 1e9) // It means my first planning has not started + return false; + + double my_traj_start_time = local_data_.start_time_.toSec(); + double other_traj_start_time = swarm_trajs_buf_[drone_id].start_time_.toSec(); + + double t_start = max(my_traj_start_time, other_traj_start_time); + double t_end = min(my_traj_start_time + local_data_.duration_ * 2 / 3, other_traj_start_time + swarm_trajs_buf_[drone_id].duration_); + + for (double t = t_start; t < t_end; t += 0.03) + { + if ((local_data_.position_traj_.evaluateDeBoorT(t - my_traj_start_time) - swarm_trajs_buf_[drone_id].position_traj_.evaluateDeBoorT(t - other_traj_start_time)).norm() < bspline_optimizer_->getSwarmClearance()) + { + return true; + } + } + + return false; + } + + bool EGOPlannerManager::planGlobalTrajWaypoints(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc, + const std::vector &waypoints, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc) + { + + // generate global reference trajectory + + vector points; + points.push_back(start_pos); + + for (size_t wp_i = 0; wp_i < waypoints.size(); wp_i++) + { + points.push_back(waypoints[wp_i]); + } + + double total_len = 0; + total_len += (start_pos - waypoints[0]).norm(); + for (size_t i = 0; i < waypoints.size() - 1; i++) + { + total_len += (waypoints[i + 1] - waypoints[i]).norm(); + } + + // insert intermediate points if too far + vector inter_points; + double dist_thresh = max(total_len / 8, 4.0); + + for (size_t i = 0; i < points.size() - 1; ++i) + { + inter_points.push_back(points.at(i)); + double dist = (points.at(i + 1) - points.at(i)).norm(); + + if (dist > dist_thresh) + { + int id_num = floor(dist / dist_thresh) + 1; + + for (int j = 1; j < id_num; ++j) + { + Eigen::Vector3d inter_pt = + points.at(i) * (1.0 - double(j) / id_num) + points.at(i + 1) * double(j) / id_num; + inter_points.push_back(inter_pt); + } + } + } + + inter_points.push_back(points.back()); + + // for ( int i=0; i= 3) + gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, end_vel, start_acc, end_acc, time); + else if (pos.cols() == 2) + gl_traj = PolynomialTraj::one_segment_traj_gen(start_pos, start_vel, start_acc, pos.col(1), end_vel, end_acc, time(0)); + else + return false; + + auto time_now = ros::Time::now(); + global_data_.setGlobalTraj(gl_traj, time_now); + + return true; + } + + bool EGOPlannerManager::planGlobalTraj(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc, + const Eigen::Vector3d &end_pos, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc) + { + + // generate global reference trajectory,生成全局参考轨迹 + vector points; + points.push_back(start_pos); + points.push_back(end_pos); + + // insert intermediate points if too far + vector inter_points; + const double dist_thresh = 4.0; + + // points.size() = 2, 这个for循环只执行一次? + for (size_t i = 0; i < points.size() - 1; ++i) + { + // push_back初始点 + inter_points.push_back(points.at(i)); + // 目标点与起始点的距离 + double dist = (points.at(i + 1) - points.at(i)).norm(); + + // 距离大于4米 + if (dist > dist_thresh) + { + // 以4米为一个单位计算增加的点数 + int id_num = floor(dist / dist_thresh) + 1; + + for (int j = 1; j < id_num; ++j) + { + Eigen::Vector3d inter_pt = + points.at(i) * (1.0 - double(j) / id_num) + points.at(i + 1) * double(j) / id_num; + inter_points.push_back(inter_pt); + } + } + } + + // push_back目标点 + inter_points.push_back(points.back()); + + // write position matrix + int pt_num = inter_points.size(); + // 航点矩阵 + Eigen::MatrixXd pos(3, pt_num); + // 赋值 + for (int i = 0; i < pt_num; ++i) + pos.col(i) = inter_points[i]; + + Eigen::Vector3d zero(0, 0, 0); + Eigen::VectorXd time(pt_num - 1); + for (int i = 0; i < pt_num - 1; ++i) + { + // 预估时间 + time(i) = (pos.col(i + 1) - pos.col(i)).norm() / (pp_.max_vel_); + } + + // 第一段时间扩大2倍 + time(0) *= 2.0; + // 最后一段时间扩大2倍 + time(time.rows() - 1) *= 2.0; + + PolynomialTraj gl_traj; + // 如果仅有一段,或有多段 + if (pos.cols() >= 3) + gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, end_vel, start_acc, end_acc, time); + else if (pos.cols() == 2) + gl_traj = PolynomialTraj::one_segment_traj_gen(start_pos, start_vel, start_acc, end_pos, end_vel, end_acc, time(0)); + else + return false; + + auto time_now = ros::Time::now(); + // 存入global_data_,等待下一步处理 + global_data_.setGlobalTraj(gl_traj, time_now); + + return true; + } + + bool EGOPlannerManager::refineTrajAlgo(UniformBspline &traj, vector &start_end_derivative, double ratio, double &ts, Eigen::MatrixXd &optimal_control_points) + { + double t_inc; + + Eigen::MatrixXd ctrl_pts; // = traj.getControlPoint() + + // std::cout << "ratio: " << ratio << std::endl; + reparamBspline(traj, start_end_derivative, ratio, ctrl_pts, ts, t_inc); + + traj = UniformBspline(ctrl_pts, 3, ts); + + double t_step = traj.getTimeSum() / (ctrl_pts.cols() - 3); + bspline_optimizer_->ref_pts_.clear(); + for (double t = 0; t < traj.getTimeSum() + 1e-4; t += t_step) + bspline_optimizer_->ref_pts_.push_back(traj.evaluateDeBoorT(t)); + + bool success = bspline_optimizer_->BsplineOptimizeTrajRefine(ctrl_pts, ts, optimal_control_points); + + return success; + } + + void EGOPlannerManager::updateTrajInfo(const UniformBspline &position_traj, const ros::Time time_now) + { + local_data_.start_time_ = time_now; + local_data_.position_traj_ = position_traj; + local_data_.velocity_traj_ = local_data_.position_traj_.getDerivative(); + local_data_.acceleration_traj_ = local_data_.velocity_traj_.getDerivative(); + local_data_.start_pos_ = local_data_.position_traj_.evaluateDeBoorT(0.0); + local_data_.duration_ = local_data_.position_traj_.getTimeSum(); + local_data_.traj_id_ += 1; + } + + void EGOPlannerManager::reparamBspline(UniformBspline &bspline, vector &start_end_derivative, double ratio, + Eigen::MatrixXd &ctrl_pts, double &dt, double &time_inc) + { + double time_origin = bspline.getTimeSum(); + int seg_num = bspline.getControlPoint().cols() - 3; + // double length = bspline.getLength(0.1); + // int seg_num = ceil(length / pp_.ctrl_pt_dist); + + bspline.lengthenTime(ratio); + double duration = bspline.getTimeSum(); + dt = duration / double(seg_num); + time_inc = duration - time_origin; + + vector point_set; + for (double time = 0.0; time <= duration + 1e-4; time += dt) + { + point_set.push_back(bspline.evaluateDeBoorT(time)); + } + UniformBspline::parameterizeToBspline(dt, point_set, start_end_derivative, ctrl_pts); + } + +} // namespace ego_planner diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src/traj_server.cpp b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src/traj_server.cpp new file mode 100644 index 00000000..92d68fdd --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src/traj_server.cpp @@ -0,0 +1,264 @@ +#include "bspline_opt/uniform_bspline.h" +#include "nav_msgs/Odometry.h" +#include "traj_utils/Bspline.h" +#include "quadrotor_msgs/PositionCommand.h" +#include "std_msgs/Empty.h" +#include "visualization_msgs/Marker.h" +#include + +ros::Publisher pos_cmd_pub; + +quadrotor_msgs::PositionCommand cmd; +double pos_gain[3] = {0, 0, 0}; +double vel_gain[3] = {0, 0, 0}; + +using ego_planner::UniformBspline; + +bool receive_traj_ = false; +vector traj_; +double traj_duration_; +ros::Time start_time_; +int traj_id_; + +// yaw control +double last_yaw_, last_yaw_dot_; +double time_forward_; + +void bsplineCallback(traj_utils::BsplineConstPtr msg) +{ + // parse pos traj + + Eigen::MatrixXd pos_pts(3, msg->pos_pts.size()); + + Eigen::VectorXd knots(msg->knots.size()); + for (size_t i = 0; i < msg->knots.size(); ++i) + { + knots(i) = msg->knots[i]; + } + + for (size_t i = 0; i < msg->pos_pts.size(); ++i) + { + pos_pts(0, i) = msg->pos_pts[i].x; + pos_pts(1, i) = msg->pos_pts[i].y; + pos_pts(2, i) = msg->pos_pts[i].z; + } + + UniformBspline pos_traj(pos_pts, msg->order, 0.1); + pos_traj.setKnot(knots); + + // parse yaw traj + + // Eigen::MatrixXd yaw_pts(msg->yaw_pts.size(), 1); + // for (int i = 0; i < msg->yaw_pts.size(); ++i) { + // yaw_pts(i, 0) = msg->yaw_pts[i]; + // } + + //UniformBspline yaw_traj(yaw_pts, msg->order, msg->yaw_dt); + + start_time_ = msg->start_time; + traj_id_ = msg->traj_id; + + traj_.clear(); + traj_.push_back(pos_traj); + traj_.push_back(traj_[0].getDerivative()); + traj_.push_back(traj_[1].getDerivative()); + + traj_duration_ = traj_[0].getTimeSum(); + + receive_traj_ = true; +} + +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; + 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 cmdCallback(const ros::TimerEvent &e) +{ + /* no publishing before receive traj_ */ + if (!receive_traj_) + return; + + ros::Time time_now = ros::Time::now(); + double t_cur = (time_now - start_time_).toSec(); + + Eigen::Vector3d pos(Eigen::Vector3d::Zero()), vel(Eigen::Vector3d::Zero()), acc(Eigen::Vector3d::Zero()), pos_f; + std::pair yaw_yawdot(0, 0); + + static ros::Time time_last = ros::Time::now(); + if (t_cur < traj_duration_ && t_cur >= 0.0) + { + pos = traj_[0].evaluateDeBoorT(t_cur); + vel = traj_[1].evaluateDeBoorT(t_cur); + acc = traj_[2].evaluateDeBoorT(t_cur); + + /*** calculate yaw ***/ + yaw_yawdot = calculate_yaw(t_cur, pos, time_now, time_last); + /*** calculate yaw ***/ + + double tf = min(traj_duration_, t_cur + 2.0); + pos_f = traj_[0].evaluateDeBoorT(tf); + } + else if (t_cur >= traj_duration_) + { + /* hover when finish traj_ */ + pos = traj_[0].evaluateDeBoorT(traj_duration_); + vel.setZero(); + acc.setZero(); + + yaw_yawdot.first = last_yaw_; + yaw_yawdot.second = 0; + + pos_f = pos; + } + else + { + cout << "[Traj server]: invalid time." << endl; + } + time_last = time_now; + + cmd.header.stamp = time_now; + cmd.header.frame_id = "world"; + cmd.trajectory_flag = quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_READY; + cmd.trajectory_id = traj_id_; + + cmd.position.x = pos(0); + cmd.position.y = pos(1); + cmd.position.z = pos(2); + + cmd.velocity.x = vel(0); + cmd.velocity.y = vel(1); + cmd.velocity.z = vel(2); + + cmd.acceleration.x = acc(0); + cmd.acceleration.y = acc(1); + cmd.acceleration.z = acc(2); + + cmd.yaw = yaw_yawdot.first; + cmd.yaw_dot = yaw_yawdot.second; + + last_yaw_ = cmd.yaw; + + pos_cmd_pub.publish(cmd); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "traj_server"); + // ros::NodeHandle node; + ros::NodeHandle nh("~"); + + ros::Subscriber bspline_sub = nh.subscribe("planning/bspline", 10, bsplineCallback); + + pos_cmd_pub = nh.advertise("/position_cmd", 50); + + 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]; + + nh.param("traj_server/time_forward", time_forward_, -1.0); + nh.param("traj_server/last_yaw", last_yaw_, 0.0); + last_yaw_dot_ = 0.0; + + ros::Duration(1.0).sleep(); + + ROS_WARN("[Traj server]: ready."); + + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src_for_prometheus/ego_goal_pub.cpp b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src_for_prometheus/ego_goal_pub.cpp new file mode 100644 index 00000000..894ac802 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src_for_prometheus/ego_goal_pub.cpp @@ -0,0 +1,59 @@ +#include + +#include +#include "printf_utils.h" + +#define MAX_SWRAM_NUM 41 +ros::Publisher planner_goal_pub[MAX_SWRAM_NUM]; +ros::Publisher planner_start_pub[MAX_SWRAM_NUM]; + +double target[10][3]; + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "ego_goal_pub"); + ros::NodeHandle nh("~"); + + int swarm_num; + bool sim_mode; + // 无人机编号 1号无人机则为1 + nh.param("swarm_num", swarm_num, 1); + nh.param("sim_mode", sim_mode, true); + + string uav_name; + for (int i = 1; i <= swarm_num; i++) + { + nh.param("uav" + to_string(i) + "/target_x", target[i][0], -1.0); + nh.param("uav" + to_string(i) + "/target_y", target[i][1], -1.0); + nh.param("uav" + to_string(i) + "/target_z", target[i][2], -1.0); + + cout << GREEN << "uav_"<< i <<"/target: [" << target[i][0] << "," << target[i][1] << "," << target[i][2] << "]" << TAIL << endl; + + uav_name = "/uav" + std::to_string(i); + // 【发布】目标点至EGO-planner-swarm + planner_goal_pub[i] = nh.advertise(uav_name + "/prometheus/motion_planning/goal", 1); + // 【发布】start_trigger + planner_start_pub[i] = nh.advertise(uav_name + "/traj_start_trigger", 1); + } + + // 【订阅】EGO的轨迹输出(traj_server的输出) + // ego_ouput_sub = nh.subscribe(uav_name + "/prometheus/ego/traj_cmd", 1, &Case3FSM::ego_ouput_cb, this); + + bool flag; + cout << GREEN << "input 1 to pub target:" << TAIL << endl; + cin >> flag; + + geometry_msgs::PoseStamped target_point; + geometry_msgs::PoseStamped start_trigger; + for (int i = 1; i <= swarm_num; i++) + { + target_point.pose.position.x = target[i][0]; + target_point.pose.position.y = target[i][1]; + target_point.pose.position.z = target[i][2]; + + planner_goal_pub[i].publish(target_point); + // planner_start_pub[i].publish(start_trigger); + } + + return 0; +} \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src_for_prometheus/traj_server_for_prometheus.cpp b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src_for_prometheus/traj_server_for_prometheus.cpp new file mode 100644 index 00000000..d8e537b9 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/plan_manage/src_for_prometheus/traj_server_for_prometheus.cpp @@ -0,0 +1,310 @@ +#include +#include "bspline_opt/uniform_bspline.h" +#include "nav_msgs/Odometry.h" +#include "traj_utils/Bspline.h" +#include "quadrotor_msgs/PositionCommand.h" +#include +#include "std_msgs/Empty.h" +#include "visualization_msgs/Marker.h" + +#include "geometry_utils.h" + +// 无人机编号 +int uav_id; +int control_flag; + +ros::Publisher pos_cmd_pub, uav_cmd_pub; + +quadrotor_msgs::PositionCommand cmd; +double pos_gain[3] = {0, 0, 0}; +double vel_gain[3] = {0, 0, 0}; + +using ego_planner::UniformBspline; + +bool receive_traj_ = false; +vector traj_; +double traj_duration_; +ros::Time start_time_; +int traj_id_; + +// yaw control +double last_yaw_, last_yaw_dot_; +double time_forward_; + +void pub_prometheus_command(quadrotor_msgs::PositionCommand ego_traj_cmd); +void bsplineCallback(traj_utils::BsplineConstPtr msg); +void cmdCallback(const ros::TimerEvent &e); +std::pair calculate_yaw(double t_cur, Eigen::Vector3d &pos, ros::Time &time_now, ros::Time &time_last); + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "traj_server_for_prometheus"); + ros::NodeHandle nh("~"); + + // 无人机编号 1号无人机则为1 + nh.param("uav_id", uav_id, 0); + nh.param("control_flag", control_flag, 0); + nh.param("traj_server/time_forward", time_forward_, 1.0); + nh.param("traj_server/last_yaw", last_yaw_, 0.0); + string uav_name = "/uav" + std::to_string(uav_id); + + // [订阅] EGO规划结果 + ros::Subscriber bspline_sub = nh.subscribe(uav_name +"/planning/bspline", 10, bsplineCallback); + + // [发布] EGO规划结果 + pos_cmd_pub = nh.advertise(uav_name + "/position_cmd", 50); + + // [订阅] EGO规划结果 - to Prometheus + uav_cmd_pub = nh.advertise(uav_name + "/prometheus/command", 50); + + // [定时器] EGO规划结果发布定时器 + 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_dot_ = 0.0; + + ros::Duration(1.0).sleep(); + + ROS_WARN("[Traj server]: ready."); + + ros::spin(); + + return 0; +} + +void pub_prometheus_command(quadrotor_msgs::PositionCommand ego_traj_cmd) +{ + prometheus_msgs::UAVCommand uav_command; + uav_command.header.stamp = ros::Time::now(); + uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move; + uav_command.Command_ID = uav_command.Command_ID + 1; + if (control_flag == 0) + { + // pos_controller + uav_command.Move_mode = prometheus_msgs::UAVCommand::TRAJECTORY; + } + else if (control_flag == 1) + { + // px4_sender + uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS; + } + uav_command.position_ref[0] = ego_traj_cmd.position.x; + uav_command.position_ref[1] = ego_traj_cmd.position.y; + uav_command.position_ref[2] = ego_traj_cmd.position.z; + uav_command.velocity_ref[0] = ego_traj_cmd.velocity.x; + uav_command.velocity_ref[1] = ego_traj_cmd.velocity.y; + uav_command.velocity_ref[2] = ego_traj_cmd.velocity.z; + uav_command.acceleration_ref[0] = ego_traj_cmd.acceleration.x; + uav_command.acceleration_ref[1] = ego_traj_cmd.acceleration.y; + uav_command.acceleration_ref[2] = ego_traj_cmd.acceleration.z; + uav_command.yaw_ref = geometry_utils::normalize_angle(ego_traj_cmd.yaw); + // uav_command.yaw_rate_ref = ego_traj_cmd.yaw_dot; + + uav_cmd_pub.publish(uav_command); +} + +void cmdCallback(const ros::TimerEvent &e) +{ + /* no publishing before receive traj_ */ + if (!receive_traj_) + return; + + ros::Time time_now = ros::Time::now(); + double t_cur = (time_now - start_time_).toSec(); + + Eigen::Vector3d pos(Eigen::Vector3d::Zero()), vel(Eigen::Vector3d::Zero()), acc(Eigen::Vector3d::Zero()), pos_f; + std::pair yaw_yawdot(0, 0); + + static ros::Time time_last = ros::Time::now(); + if (t_cur < traj_duration_ && t_cur >= 0.0) + { + pos = traj_[0].evaluateDeBoorT(t_cur); + vel = traj_[1].evaluateDeBoorT(t_cur); + acc = traj_[2].evaluateDeBoorT(t_cur); + + /*** calculate yaw ***/ + yaw_yawdot = calculate_yaw(t_cur, pos, time_now, time_last); + /*** calculate yaw ***/ + + double tf = min(traj_duration_, t_cur + 2.0); + pos_f = traj_[0].evaluateDeBoorT(tf); + } + else if (t_cur >= traj_duration_) + { + /* hover when finish traj_ */ + pos = traj_[0].evaluateDeBoorT(traj_duration_); + vel.setZero(); + acc.setZero(); + + yaw_yawdot.first = last_yaw_; + yaw_yawdot.second = 0; + + pos_f = pos; + } + else + { + cout << "[Traj server]: invalid time." << endl; + } + time_last = time_now; + + cmd.header.stamp = time_now; + cmd.header.frame_id = "world"; + cmd.trajectory_flag = quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_READY; + cmd.trajectory_id = traj_id_; + + cmd.position.x = pos(0); + cmd.position.y = pos(1); + cmd.position.z = pos(2); + + cmd.velocity.x = vel(0); + cmd.velocity.y = vel(1); + cmd.velocity.z = vel(2); + + cmd.acceleration.x = acc(0); + cmd.acceleration.y = acc(1); + cmd.acceleration.z = acc(2); + + cmd.yaw = yaw_yawdot.first; + cmd.yaw_dot = yaw_yawdot.second; + + last_yaw_ = cmd.yaw; + + pos_cmd_pub.publish(cmd); + + pub_prometheus_command(cmd); +} + +void bsplineCallback(traj_utils::BsplineConstPtr msg) +{ + // parse pos traj + + Eigen::MatrixXd pos_pts(3, msg->pos_pts.size()); + + Eigen::VectorXd knots(msg->knots.size()); + for (size_t i = 0; i < msg->knots.size(); ++i) + { + knots(i) = msg->knots[i]; + } + + for (size_t i = 0; i < msg->pos_pts.size(); ++i) + { + pos_pts(0, i) = msg->pos_pts[i].x; + pos_pts(1, i) = msg->pos_pts[i].y; + pos_pts(2, i) = msg->pos_pts[i].z; + } + + UniformBspline pos_traj(pos_pts, msg->order, 0.1); + pos_traj.setKnot(knots); + + start_time_ = msg->start_time; + traj_id_ = msg->traj_id; + + traj_.clear(); + traj_.push_back(pos_traj); + traj_.push_back(traj_[0].getDerivative()); + traj_.push_back(traj_[1].getDerivative()); + + traj_duration_ = traj_[0].getTimeSum(); + + receive_traj_ = true; +} + +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; + 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; +} diff --git a/src/Prometheus/Modules/ego_planner_swarm/rosmsg_tcp_bridge/CMakeLists.txt b/src/Prometheus/Modules/ego_planner_swarm/rosmsg_tcp_bridge/CMakeLists.txt new file mode 100755 index 00000000..30ec4192 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/rosmsg_tcp_bridge/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rosmsg_tcp_bridge) + +set(CMAKE_BUILD_TYPE "Release") +ADD_COMPILE_OPTIONS(-std=c++11 ) +ADD_COMPILE_OPTIONS(-std=c++14 ) +set(CMAKE_CXX_FLAGS_RELEASE "-O2 -Wall -g") + +find_package(Eigen3 REQUIRED) + +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + geometry_msgs + traj_utils +) + +catkin_package( + CATKIN_DEPENDS traj_utils +) + +include_directories( + SYSTEM + ${catkin_INCLUDE_DIRS} ${PROJECT_SOURCE_DIR}/include + ${EIGEN3_INCLUDE_DIR} +) + +add_executable(bridge_node + src/bridge_node.cpp + ) +target_link_libraries(bridge_node + ${catkin_LIBRARIES} + ) + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/rosmsg_tcp_bridge/launch/bridge.launch b/src/Prometheus/Modules/ego_planner_swarm/rosmsg_tcp_bridge/launch/bridge.launch new file mode 100644 index 00000000..babaa016 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/rosmsg_tcp_bridge/launch/bridge.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/rosmsg_tcp_bridge/package.xml b/src/Prometheus/Modules/ego_planner_swarm/rosmsg_tcp_bridge/package.xml new file mode 100755 index 00000000..677652f7 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/rosmsg_tcp_bridge/package.xml @@ -0,0 +1,67 @@ + + + rosmsg_tcp_bridge + 0.0.0 + The rosmsg_tcp_bridge package + + + + + iszhouxin + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + traj_utils + + roscpp + std_msgs + traj_utils + + + + + + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/rosmsg_tcp_bridge/src/bridge_node.cpp b/src/Prometheus/Modules/ego_planner_swarm/rosmsg_tcp_bridge/src/bridge_node.cpp new file mode 100644 index 00000000..04d7e304 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/rosmsg_tcp_bridge/src/bridge_node.cpp @@ -0,0 +1,828 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#define PORT 8080 +#define UDP_PORT 8081 +#define BUF_LEN 1048576 // 1MB +#define BUF_LEN_SHORT 1024 // 1KB + +using namespace std; + +int send_sock_, server_fd_, recv_sock_, udp_server_fd_, udp_send_fd_; +ros::Subscriber swarm_trajs_sub_, other_odoms_sub_, emergency_stop_sub_, one_traj_sub_; +ros::Publisher swarm_trajs_pub_, other_odoms_pub_, emergency_stop_pub_, one_traj_pub_; +string tcp_ip_, udp_ip_; +int drone_id_; +double odom_broadcast_freq_; +char send_buf_[BUF_LEN], recv_buf_[BUF_LEN], udp_recv_buf_[BUF_LEN], udp_send_buf_[BUF_LEN]; +struct sockaddr_in addr_udp_send_; +traj_utils::MultiBsplinesPtr bsplines_msg_; +nav_msgs::OdometryPtr odom_msg_; +std_msgs::EmptyPtr stop_msg_; +traj_utils::BsplinePtr bspline_msg_; + +enum MESSAGE_TYPE +{ + ODOM = 888, + MULTI_TRAJ, + ONE_TRAJ, + STOP +} massage_type_; + +int connect_to_next_drone(const char *ip, const int port) +{ + /* Connect */ + int sock = 0; + struct sockaddr_in serv_addr; + if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) + { + printf("\n Socket creation error \n"); + return -1; + } + + serv_addr.sin_family = AF_INET; + serv_addr.sin_port = htons(port); + + // Convert IPv4 and IPv6 addresses from text to binary form + if (inet_pton(AF_INET, ip, &serv_addr.sin_addr) <= 0) + { + printf("\nInvalid address/ Address not supported \n"); + return -1; + } + + if (connect(sock, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0) + { + ROS_WARN("Tcp connection to drone_%d Failed", drone_id_+1); + return -1; + } + + char str[INET_ADDRSTRLEN]; + ROS_INFO("Connect to %s success!", inet_ntop(AF_INET, &serv_addr.sin_addr, str, sizeof(str))); + + return sock; +} + +int init_broadcast(const char *ip, const int port) +{ + int fd; + + if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) <= 0) + { + ROS_ERROR("[bridge_node]Socket sender creation error!"); + exit(EXIT_FAILURE); + } + + int so_broadcast = 1; + if (setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &so_broadcast, sizeof(so_broadcast)) < 0) + { + cout << "Error in setting Broadcast option"; + exit(EXIT_FAILURE); + } + + addr_udp_send_.sin_family = AF_INET; + addr_udp_send_.sin_port = htons(port); + + if (inet_pton(AF_INET, ip, &addr_udp_send_.sin_addr) <= 0) + { + printf("\nInvalid address/ Address not supported \n"); + return -1; + } + + return fd; +} + +int wait_connection_from_previous_drone(const int port, int &server_fd, int &new_socket) +{ + struct sockaddr_in address; + int opt = 1; + int addrlen = sizeof(address); + + // Creating socket file descriptor + if ((server_fd = socket(AF_INET, SOCK_STREAM, 0)) == 0) + { + perror("socket failed"); + exit(EXIT_FAILURE); + } + + // Forcefully attaching socket to the port + if (setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT, + &opt, sizeof(opt))) + { + perror("setsockopt"); + exit(EXIT_FAILURE); + } + address.sin_family = AF_INET; + address.sin_addr.s_addr = INADDR_ANY; + address.sin_port = htons(PORT); + + // Forcefully attaching socket to the port + if (bind(server_fd, (struct sockaddr *)&address, + sizeof(address)) < 0) + { + perror("bind failed"); + exit(EXIT_FAILURE); + } + if (listen(server_fd, 3) < 0) + { + perror("listen"); + exit(EXIT_FAILURE); + } + if ((new_socket = accept(server_fd, (struct sockaddr *)&address, + (socklen_t *)&addrlen)) < 0) + { + perror("accept"); + exit(EXIT_FAILURE); + } + + char str[INET_ADDRSTRLEN]; + ROS_INFO( "Receive tcp connection from %s", inet_ntop(AF_INET, &address.sin_addr, str, sizeof(str)) ); + + return new_socket; +} + +int udp_bind_to_port(const int port, int &server_fd) +{ + struct sockaddr_in address; + int opt = 1; + + // Creating socket file descriptor + if ((server_fd = socket(AF_INET, SOCK_DGRAM, 0)) == 0) + { + perror("socket failed"); + exit(EXIT_FAILURE); + } + + // Forcefully attaching socket to the port + if (setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT, + &opt, sizeof(opt))) + { + perror("setsockopt"); + exit(EXIT_FAILURE); + } + address.sin_family = AF_INET; + address.sin_addr.s_addr = INADDR_ANY; + address.sin_port = htons(port); + + // Forcefully attaching socket to the port + if (bind(server_fd, (struct sockaddr *)&address, + sizeof(address)) < 0) + { + perror("bind failed"); + exit(EXIT_FAILURE); + } + + return server_fd; +} + +int serializeMultiBsplines(const traj_utils::MultiBsplinesPtr &msg) +{ + char *ptr = send_buf_; + + unsigned long total_len = 0; + total_len += sizeof(MESSAGE_TYPE) + sizeof(int32_t) + sizeof(size_t); + for (size_t i = 0; i < msg->traj.size(); i++) + { + total_len += sizeof(int32_t) + sizeof(int32_t) + sizeof(double) + sizeof(int64_t) + sizeof(double); + total_len += sizeof(size_t) + msg->traj[i].knots.size() * sizeof(double); + total_len += sizeof(size_t) + (3 * msg->traj[i].pos_pts.size()) * sizeof(double); + total_len += sizeof(size_t) + msg->traj[i].yaw_pts.size() * sizeof(double); + } + if (total_len + 1 > BUF_LEN) + { + ROS_ERROR("[bridge_node] Topic is too large, please enlarge BUF_LEN"); + return -1; + } + + *((MESSAGE_TYPE *)ptr) = MESSAGE_TYPE::MULTI_TRAJ; + ptr += sizeof(MESSAGE_TYPE); + + *((int32_t *)ptr) = msg->drone_id_from; + ptr += sizeof(int32_t); + if (ptr - send_buf_ > BUF_LEN) + { + } + *((size_t *)ptr) = msg->traj.size(); + ptr += sizeof(size_t); + for (size_t i = 0; i < msg->traj.size(); i++) + { + *((int32_t *)ptr) = msg->traj[i].drone_id; + ptr += sizeof(int32_t); + *((int32_t *)ptr) = msg->traj[i].order; + ptr += sizeof(int32_t); + *((double *)ptr) = msg->traj[i].start_time.toSec(); + ptr += sizeof(double); + *((int64_t *)ptr) = msg->traj[i].traj_id; + ptr += sizeof(int64_t); + *((double *)ptr) = msg->traj[i].yaw_dt; + ptr += sizeof(double); + + *((size_t *)ptr) = msg->traj[i].knots.size(); + ptr += sizeof(size_t); + for (size_t j = 0; j < msg->traj[i].knots.size(); j++) + { + *((double *)ptr) = msg->traj[i].knots[j]; + ptr += sizeof(double); + } + + *((size_t *)ptr) = msg->traj[i].pos_pts.size(); + ptr += sizeof(size_t); + for (size_t j = 0; j < msg->traj[i].pos_pts.size(); j++) + { + *((double *)ptr) = msg->traj[i].pos_pts[j].x; + ptr += sizeof(double); + *((double *)ptr) = msg->traj[i].pos_pts[j].y; + ptr += sizeof(double); + *((double *)ptr) = msg->traj[i].pos_pts[j].z; + ptr += sizeof(double); + } + + *((size_t *)ptr) = msg->traj[i].yaw_pts.size(); + ptr += sizeof(size_t); + for (size_t j = 0; j < msg->traj[i].yaw_pts.size(); j++) + { + *((double *)ptr) = msg->traj[i].yaw_pts[j]; + ptr += sizeof(double); + } + } + + return ptr - send_buf_; +} + +int serializeOdom(const nav_msgs::OdometryPtr &msg) +{ + char *ptr = udp_send_buf_; + + unsigned long total_len = 0; + total_len = sizeof(size_t) + + msg->child_frame_id.length() * sizeof(char) + + sizeof(size_t) + + msg->header.frame_id.length() * sizeof(char) + + sizeof(uint32_t) + + sizeof(double) + + 7 * sizeof(double) + + 36 * sizeof(double) + + 6 * sizeof(double) + + 36 * sizeof(double); + + if (total_len + 1 > BUF_LEN) + { + ROS_ERROR("[bridge_node] Topic is too large, please enlarge BUF_LEN"); + return -1; + } + + *((MESSAGE_TYPE *)ptr) = MESSAGE_TYPE::ODOM; + ptr += sizeof(MESSAGE_TYPE); + + // child_frame_id + size_t len = msg->child_frame_id.length(); + *((size_t *)ptr) = len; + ptr += sizeof(size_t); + memcpy((void *)ptr, (void *)msg->child_frame_id.c_str(), len * sizeof(char)); + ptr += len * sizeof(char); + + // header + len = msg->header.frame_id.length(); + *((size_t *)ptr) = len; + ptr += sizeof(size_t); + memcpy((void *)ptr, (void *)msg->header.frame_id.c_str(), len * sizeof(char)); + ptr += len * sizeof(char); + *((uint32_t *)ptr) = msg->header.seq; + ptr += sizeof(uint32_t); + *((double *)ptr) = msg->header.stamp.toSec(); + ptr += sizeof(double); + + *((double *)ptr) = msg->pose.pose.position.x; + ptr += sizeof(double); + *((double *)ptr) = msg->pose.pose.position.y; + ptr += sizeof(double); + *((double *)ptr) = msg->pose.pose.position.z; + ptr += sizeof(double); + + *((double *)ptr) = msg->pose.pose.orientation.w; + ptr += sizeof(double); + *((double *)ptr) = msg->pose.pose.orientation.x; + ptr += sizeof(double); + *((double *)ptr) = msg->pose.pose.orientation.y; + ptr += sizeof(double); + *((double *)ptr) = msg->pose.pose.orientation.z; + ptr += sizeof(double); + + for (size_t j = 0; j < 36; j++) + { + *((double *)ptr) = msg->pose.covariance[j]; + ptr += sizeof(double); + } + + *((double *)ptr) = msg->twist.twist.linear.x; + ptr += sizeof(double); + *((double *)ptr) = msg->twist.twist.linear.y; + ptr += sizeof(double); + *((double *)ptr) = msg->twist.twist.linear.z; + ptr += sizeof(double); + *((double *)ptr) = msg->twist.twist.angular.x; + ptr += sizeof(double); + *((double *)ptr) = msg->twist.twist.angular.y; + ptr += sizeof(double); + *((double *)ptr) = msg->twist.twist.angular.z; + ptr += sizeof(double); + + for (size_t j = 0; j < 36; j++) + { + *((double *)ptr) = msg->twist.covariance[j]; + ptr += sizeof(double); + } + + return ptr - udp_send_buf_; +} + +int serializeStop(const std_msgs::EmptyPtr &msg) +{ + char *ptr = udp_send_buf_; + + *((MESSAGE_TYPE *)ptr) = MESSAGE_TYPE::STOP; + ptr += sizeof(MESSAGE_TYPE); + + return ptr - udp_send_buf_; +} + +int serializeOneTraj(const traj_utils::BsplinePtr &msg) +{ + char *ptr = udp_send_buf_; + + unsigned long total_len = 0; + total_len += sizeof(int32_t) + sizeof(int32_t) + sizeof(double) + sizeof(int64_t) + sizeof(double); + total_len += sizeof(size_t) + msg->knots.size() * sizeof(double); + total_len += sizeof(size_t) + (3 * msg->pos_pts.size()) * sizeof(double); + total_len += sizeof(size_t) + msg->yaw_pts.size() * sizeof(double); + if (total_len + 1 > BUF_LEN) + { + ROS_ERROR("[bridge_node] Topic is too large, please enlarge BUF_LEN (2)"); + return -1; + } + + *((MESSAGE_TYPE *)ptr) = MESSAGE_TYPE::ONE_TRAJ; + ptr += sizeof(MESSAGE_TYPE); + + *((int32_t *)ptr) = msg->drone_id; + ptr += sizeof(int32_t); + *((int32_t *)ptr) = msg->order; + ptr += sizeof(int32_t); + *((double *)ptr) = msg->start_time.toSec(); + ptr += sizeof(double); + *((int64_t *)ptr) = msg->traj_id; + ptr += sizeof(int64_t); + *((double *)ptr) = msg->yaw_dt; + ptr += sizeof(double); + + *((size_t *)ptr) = msg->knots.size(); + ptr += sizeof(size_t); + for (size_t j = 0; j < msg->knots.size(); j++) + { + *((double *)ptr) = msg->knots[j]; + ptr += sizeof(double); + } + + *((size_t *)ptr) = msg->pos_pts.size(); + ptr += sizeof(size_t); + for (size_t j = 0; j < msg->pos_pts.size(); j++) + { + *((double *)ptr) = msg->pos_pts[j].x; + ptr += sizeof(double); + *((double *)ptr) = msg->pos_pts[j].y; + ptr += sizeof(double); + *((double *)ptr) = msg->pos_pts[j].z; + ptr += sizeof(double); + } + + *((size_t *)ptr) = msg->yaw_pts.size(); + ptr += sizeof(size_t); + for (size_t j = 0; j < msg->yaw_pts.size(); j++) + { + *((double *)ptr) = msg->yaw_pts[j]; + ptr += sizeof(double); + } + + return ptr - udp_send_buf_; +} + +int deserializeOneTraj(traj_utils::BsplinePtr &msg) +{ + char *ptr = udp_recv_buf_; + + ptr += sizeof(MESSAGE_TYPE); + + msg->drone_id = *((int32_t *)ptr); + ptr += sizeof(int32_t); + msg->order = *((int32_t *)ptr); + ptr += sizeof(int32_t); + msg->start_time.fromSec(*((double *)ptr)); + ptr += sizeof(double); + msg->traj_id = *((int64_t *)ptr); + ptr += sizeof(int64_t); + msg->yaw_dt = *((double *)ptr); + ptr += sizeof(double); + msg->knots.resize(*((size_t *)ptr)); + ptr += sizeof(size_t); + for (size_t j = 0; j < msg->knots.size(); j++) + { + msg->knots[j] = *((double *)ptr); + ptr += sizeof(double); + } + + msg->pos_pts.resize(*((size_t *)ptr)); + ptr += sizeof(size_t); + for (size_t j = 0; j < msg->pos_pts.size(); j++) + { + msg->pos_pts[j].x = *((double *)ptr); + ptr += sizeof(double); + msg->pos_pts[j].y = *((double *)ptr); + ptr += sizeof(double); + msg->pos_pts[j].z = *((double *)ptr); + ptr += sizeof(double); + } + + msg->yaw_pts.resize(*((size_t *)ptr)); + ptr += sizeof(size_t); + for (size_t j = 0; j < msg->yaw_pts.size(); j++) + { + msg->yaw_pts[j] = *((double *)ptr); + ptr += sizeof(double); + } + + return ptr - udp_recv_buf_; +} + +int deserializeStop(std_msgs::EmptyPtr &msg) +{ + char *ptr = udp_recv_buf_; + + return ptr - udp_recv_buf_; +} + +int deserializeOdom(nav_msgs::OdometryPtr &msg) +{ + char *ptr = udp_recv_buf_; + + ptr += sizeof(MESSAGE_TYPE); + + // child_frame_id + size_t len = *((size_t *)ptr); + ptr += sizeof(size_t); + msg->child_frame_id.assign((const char *)ptr, len); + ptr += len * sizeof(char); + + // header + len = *((size_t *)ptr); + ptr += sizeof(size_t); + msg->header.frame_id.assign((const char *)ptr, len); + ptr += len * sizeof(char); + msg->header.seq = *((uint32_t *)ptr); + ptr += sizeof(uint32_t); + msg->header.stamp.fromSec(*((double *)ptr)); + ptr += sizeof(double); + + msg->pose.pose.position.x = *((double *)ptr); + ptr += sizeof(double); + msg->pose.pose.position.y = *((double *)ptr); + ptr += sizeof(double); + msg->pose.pose.position.z = *((double *)ptr); + ptr += sizeof(double); + + msg->pose.pose.orientation.w = *((double *)ptr); + ptr += sizeof(double); + msg->pose.pose.orientation.x = *((double *)ptr); + ptr += sizeof(double); + msg->pose.pose.orientation.y = *((double *)ptr); + ptr += sizeof(double); + msg->pose.pose.orientation.z = *((double *)ptr); + ptr += sizeof(double); + + for (size_t j = 0; j < 36; j++) + { + msg->pose.covariance[j] = *((double *)ptr); + ptr += sizeof(double); + } + + msg->twist.twist.linear.x = *((double *)ptr); + ptr += sizeof(double); + msg->twist.twist.linear.y = *((double *)ptr); + ptr += sizeof(double); + msg->twist.twist.linear.z = *((double *)ptr); + ptr += sizeof(double); + msg->twist.twist.angular.x = *((double *)ptr); + ptr += sizeof(double); + msg->twist.twist.angular.y = *((double *)ptr); + ptr += sizeof(double); + msg->twist.twist.angular.z = *((double *)ptr); + ptr += sizeof(double); + + for (size_t j = 0; j < 36; j++) + { + msg->twist.covariance[j] = *((double *)ptr); + ptr += sizeof(double); + } + + return ptr - udp_recv_buf_; +} + +int deserializeMultiBsplines(traj_utils::MultiBsplinesPtr &msg) +{ + char *ptr = recv_buf_; + + ptr += sizeof(MESSAGE_TYPE); + + msg->drone_id_from = *((int32_t *)ptr); + ptr += sizeof(int32_t); + msg->traj.resize(*((size_t *)ptr)); + ptr += sizeof(size_t); + for (size_t i = 0; i < msg->traj.size(); i++) + { + msg->traj[i].drone_id = *((int32_t *)ptr); + ptr += sizeof(int32_t); + msg->traj[i].order = *((int32_t *)ptr); + ptr += sizeof(int32_t); + msg->traj[i].start_time.fromSec(*((double *)ptr)); + ptr += sizeof(double); + msg->traj[i].traj_id = *((int64_t *)ptr); + ptr += sizeof(int64_t); + msg->traj[i].yaw_dt = *((double *)ptr); + ptr += sizeof(double); + + msg->traj[i].knots.resize(*((size_t *)ptr)); + ptr += sizeof(size_t); + for (size_t j = 0; j < msg->traj[i].knots.size(); j++) + { + msg->traj[i].knots[j] = *((double *)ptr); + ptr += sizeof(double); + } + + msg->traj[i].pos_pts.resize(*((size_t *)ptr)); + ptr += sizeof(size_t); + for (size_t j = 0; j < msg->traj[i].pos_pts.size(); j++) + { + msg->traj[i].pos_pts[j].x = *((double *)ptr); + ptr += sizeof(double); + msg->traj[i].pos_pts[j].y = *((double *)ptr); + ptr += sizeof(double); + msg->traj[i].pos_pts[j].z = *((double *)ptr); + ptr += sizeof(double); + } + + msg->traj[i].yaw_pts.resize(*((size_t *)ptr)); + ptr += sizeof(size_t); + for (size_t j = 0; j < msg->traj[i].yaw_pts.size(); j++) + { + msg->traj[i].yaw_pts[j] = *((double *)ptr); + ptr += sizeof(double); + } + } + + return ptr - recv_buf_; +} + +void multitraj_sub_tcp_cb(const traj_utils::MultiBsplinesPtr &msg) +{ + int len = serializeMultiBsplines(msg); + if (send(send_sock_, send_buf_, len, 0) <= 0) + { + ROS_ERROR("TCP SEND ERROR!!!"); + } +} + +void odom_sub_udp_cb(const nav_msgs::OdometryPtr &msg) +{ + + static ros::Time t_last; + ros::Time t_now = ros::Time::now(); + if ((t_now - t_last).toSec() * odom_broadcast_freq_ < 1.0) + { + return; + } + t_last = t_now; + + msg->child_frame_id = string("drone_") + std::to_string(drone_id_); + + int len = serializeOdom(msg); + + if (sendto(udp_send_fd_, udp_send_buf_, len, 0, (struct sockaddr *)&addr_udp_send_, sizeof(addr_udp_send_)) <= 0) + { + ROS_ERROR("UDP SEND ERROR (1)!!!"); + } +} + +void emergency_stop_sub_udp_cb(const std_msgs::EmptyPtr &msg) +{ + + int len = serializeStop(msg); + + if (sendto(udp_send_fd_, udp_send_buf_, len, 0, (struct sockaddr *)&addr_udp_send_, sizeof(addr_udp_send_)) <= 0) + { + ROS_ERROR("UDP SEND ERROR (2)!!!"); + } +} + +void one_traj_sub_udp_cb(const traj_utils::BsplinePtr &msg) +{ + + int len = serializeOneTraj(msg); + + if (sendto(udp_send_fd_, udp_send_buf_, len, 0, (struct sockaddr *)&addr_udp_send_, sizeof(addr_udp_send_)) <= 0) + { + ROS_ERROR("UDP SEND ERROR (3)!!!"); + } +} + +void server_fun() +{ + int valread; + + // Connect + if (wait_connection_from_previous_drone(PORT, server_fd_, recv_sock_) < 0) + { + ROS_ERROR("[bridge_node]Socket recever creation error!"); + exit(EXIT_FAILURE); + } + + while (true) + { + valread = read(recv_sock_, recv_buf_, BUF_LEN); + + if ( valread <= 0 ) + { + ROS_ERROR("Received message length <= 0, maybe connection has lost"); + close(recv_sock_); + close(server_fd_); + return; + } + + if (valread == deserializeMultiBsplines(bsplines_msg_)) + { + swarm_trajs_pub_.publish(*bsplines_msg_); + } + else + { + ROS_ERROR("Received message length not matches the sent one!!!"); + continue; + } + } +} + +void udp_recv_fun() +{ + int valread; + struct sockaddr_in addr_client; + socklen_t addr_len; + + // Connect + if (udp_bind_to_port(UDP_PORT, udp_server_fd_) < 0) + { + ROS_ERROR("[bridge_node]Socket recever creation error!"); + exit(EXIT_FAILURE); + } + + while (true) + { + if ((valread = recvfrom(udp_server_fd_, udp_recv_buf_, BUF_LEN, 0, (struct sockaddr *)&addr_client, (socklen_t *)&addr_len)) < 0) + { + perror("recvfrom error:"); + exit(EXIT_FAILURE); + } + + char *ptr = udp_recv_buf_; + switch (*((MESSAGE_TYPE *)ptr)) + { + case MESSAGE_TYPE::STOP: + { + + if (valread == sizeof(std_msgs::Empty)) + { + if (valread == deserializeStop(stop_msg_)) + { + emergency_stop_pub_.publish(*stop_msg_); + } + else + { + ROS_ERROR("Received message length not matches the sent one (1)!!!"); + continue; + } + } + + break; + } + + case MESSAGE_TYPE::ODOM: + { + if (valread == deserializeOdom(odom_msg_)) + { + other_odoms_pub_.publish(*odom_msg_); + } + else + { + ROS_ERROR("Received message length not matches the sent one (2)!!!"); + continue; + } + + break; + } + + case MESSAGE_TYPE::ONE_TRAJ: + { + + if ( valread == deserializeOneTraj(bspline_msg_) ) + { + one_traj_pub_.publish(*bspline_msg_); + } + else + { + ROS_ERROR("Received message length not matches the sent one (3)!!!"); + continue; + } + + break; + } + + default: + + //ROS_ERROR("Unknown received message???"); + + break; + } + } +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "rosmsg_tcp_bridge"); + ros::NodeHandle nh("~"); + + nh.param("next_drone_ip", tcp_ip_, string("127.0.0.1")); + nh.param("broadcast_ip", udp_ip_, string("127.0.0.255")); + nh.param("drone_id", drone_id_, -1); + nh.param("odom_max_freq", odom_broadcast_freq_, 1000.0); + + bsplines_msg_.reset(new traj_utils::MultiBsplines); + odom_msg_.reset(new nav_msgs::Odometry); + stop_msg_.reset(new std_msgs::Empty); + bspline_msg_.reset(new traj_utils::Bspline); + + if (drone_id_ == -1) + { + ROS_ERROR("Wrong drone_id!"); + exit(EXIT_FAILURE); + } + + string sub_traj_topic_name = string("/uav") + std::to_string(drone_id_) + string("_planning/swarm_trajs"); + swarm_trajs_sub_ = nh.subscribe(sub_traj_topic_name.c_str(), 10, multitraj_sub_tcp_cb, ros::TransportHints().tcpNoDelay()); + + if ( drone_id_ >= 2 ) + { + string pub_traj_topic_name = string("/uav") + std::to_string(drone_id_ - 1) + string("_planning/swarm_trajs"); + swarm_trajs_pub_ = nh.advertise(pub_traj_topic_name.c_str(), 10); + } + + // other_odoms_sub_ = nh.subscribe("my_odom", 10, odom_sub_udp_cb, ros::TransportHints().tcpNoDelay()); + // other_odoms_pub_ = nh.advertise("/others_odom", 10); + + //emergency_stop_sub_ = nh.subscribe("emergency_stop_broadcast", 10, emergency_stop_sub_udp_cb, ros::TransportHints().tcpNoDelay()); + //emergency_stop_pub_ = nh.advertise("emergency_stop_recv", 10); + + one_traj_sub_ = nh.subscribe("/broadcast_bspline", 100, one_traj_sub_udp_cb, ros::TransportHints().tcpNoDelay()); + one_traj_pub_ = nh.advertise("/broadcast_bspline2", 100); + + boost::thread recv_thd(server_fun); + recv_thd.detach(); + ros::Duration(0.1).sleep(); + boost::thread udp_recv_thd(udp_recv_fun); + udp_recv_thd.detach(); + ros::Duration(0.1).sleep(); + + // TCP connect + send_sock_ = connect_to_next_drone(tcp_ip_.c_str(), PORT); + + // UDP connect + udp_send_fd_ = init_broadcast(udp_ip_.c_str(), UDP_PORT); + + cout << "[rosmsg_tcp_bridge] start running" << endl; + + ros::spin(); + + close(send_sock_); + close(recv_sock_); + close(server_fd_); + close(udp_server_fd_); + close(udp_send_fd_); + + return 0; +} diff --git a/src/Prometheus/Modules/ego_planner_swarm/traj_utils/CMakeLists.txt b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/CMakeLists.txt new file mode 100755 index 00000000..5f1f457f --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/CMakeLists.txt @@ -0,0 +1,59 @@ +cmake_minimum_required(VERSION 2.8.3) +project(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 + #bspline_opt + #path_searching + roscpp + std_msgs + geometry_msgs + message_generation + #cv_bridge +) + +find_package(Eigen3 REQUIRED) +find_package(PCL 1.7 REQUIRED) + +# Generate messages in the 'msg' folder +add_message_files( + FILES + Bspline.msg + DataDisp.msg + MultiBsplines.msg + ) + +# Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES traj_utils + CATKIN_DEPENDS message_runtime +# DEPENDS system_lib +) + +include_directories( + SYSTEM + include + ${catkin_INCLUDE_DIRS} + ${Eigen3_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} +) + +link_directories(${PCL_LIBRARY_DIRS}) + +add_library( traj_utils + src/planning_visualization.cpp + src/polynomial_traj.cpp + ) +target_link_libraries( traj_utils + ${catkin_LIBRARIES} + ) diff --git a/src/Prometheus/Modules/ego_planner_swarm/traj_utils/include/traj_utils/plan_container.hpp b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/include/traj_utils/plan_container.hpp new file mode 100644 index 00000000..4c249df1 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/include/traj_utils/plan_container.hpp @@ -0,0 +1,233 @@ +#ifndef _PLAN_CONTAINER_H_ +#define _PLAN_CONTAINER_H_ + +#include +#include +#include + +#include +#include + +using std::vector; + +namespace ego_planner +{ + + class GlobalTrajData + { + private: + public: + PolynomialTraj global_traj_; + vector local_traj_; + + double global_duration_; + ros::Time global_start_time_; + double local_start_time_, local_end_time_; + double time_increase_; + double last_time_inc_; + double last_progress_time_; + + GlobalTrajData(/* args */) {} + + ~GlobalTrajData() {} + + bool localTrajReachTarget() { return fabs(local_end_time_ - global_duration_) < 0.1; } + + void setGlobalTraj(const PolynomialTraj &traj, const ros::Time &time) + { + global_traj_ = traj; + global_traj_.init(); + global_duration_ = global_traj_.getTimeSum(); + global_start_time_ = time; + + local_traj_.clear(); + local_start_time_ = -1; + local_end_time_ = -1; + time_increase_ = 0.0; + last_time_inc_ = 0.0; + last_progress_time_ = 0.0; + } + + void setLocalTraj(UniformBspline traj, double local_ts, double local_te, double time_inc) + { + local_traj_.resize(3); + local_traj_[0] = traj; + local_traj_[1] = local_traj_[0].getDerivative(); + local_traj_[2] = local_traj_[1].getDerivative(); + + local_start_time_ = local_ts; + local_end_time_ = local_te; + global_duration_ += time_inc; + time_increase_ += time_inc; + last_time_inc_ = time_inc; + } + + Eigen::Vector3d getPosition(double t) + { + if (t >= -1e-3 && t <= local_start_time_) + { + return global_traj_.evaluate(t - time_increase_ + last_time_inc_); + } + else if (t >= local_end_time_ && t <= global_duration_ + 1e-3) + { + return global_traj_.evaluate(t - time_increase_); + } + else + { + double tm, tmp; + local_traj_[0].getTimeSpan(tm, tmp); + return local_traj_[0].evaluateDeBoor(tm + t - local_start_time_); + } + } + + Eigen::Vector3d getVelocity(double t) + { + if (t >= -1e-3 && t <= local_start_time_) + { + return global_traj_.evaluateVel(t); + } + else if (t >= local_end_time_ && t <= global_duration_ + 1e-3) + { + return global_traj_.evaluateVel(t - time_increase_); + } + else + { + double tm, tmp; + local_traj_[0].getTimeSpan(tm, tmp); + return local_traj_[1].evaluateDeBoor(tm + t - local_start_time_); + } + } + + Eigen::Vector3d getAcceleration(double t) + { + if (t >= -1e-3 && t <= local_start_time_) + { + return global_traj_.evaluateAcc(t); + } + else if (t >= local_end_time_ && t <= global_duration_ + 1e-3) + { + return global_traj_.evaluateAcc(t - time_increase_); + } + else + { + double tm, tmp; + local_traj_[0].getTimeSpan(tm, tmp); + return local_traj_[2].evaluateDeBoor(tm + t - local_start_time_); + } + } + + // get Bspline paramterization data of a local trajectory within a sphere + // start_t: start time of the trajectory + // dist_pt: distance between the discretized points + void getTrajByRadius(const double &start_t, const double &des_radius, const double &dist_pt, + vector &point_set, vector &start_end_derivative, + double &dt, double &seg_duration) + { + double seg_length = 0.0; // length of the truncated segment + double seg_time = 0.0; // duration of the truncated segment + double radius = 0.0; // distance to the first point of the segment + + double delt = 0.2; + Eigen::Vector3d first_pt = getPosition(start_t); // first point of the segment + Eigen::Vector3d prev_pt = first_pt; // previous point + Eigen::Vector3d cur_pt; // current point + + // go forward until the traj exceed radius or global time + + while (radius < des_radius && seg_time < global_duration_ - start_t - 1e-3) + { + seg_time += delt; + seg_time = min(seg_time, global_duration_ - start_t); + + cur_pt = getPosition(start_t + seg_time); + seg_length += (cur_pt - prev_pt).norm(); + prev_pt = cur_pt; + radius = (cur_pt - first_pt).norm(); + } + + // get parameterization dt by desired density of points + int seg_num = floor(seg_length / dist_pt); + + // get outputs + + seg_duration = seg_time; // duration of the truncated segment + dt = seg_time / seg_num; // time difference between two points + + for (double tp = 0.0; tp <= seg_time + 1e-4; tp += dt) + { + cur_pt = getPosition(start_t + tp); + point_set.push_back(cur_pt); + } + + start_end_derivative.push_back(getVelocity(start_t)); + start_end_derivative.push_back(getVelocity(start_t + seg_time)); + start_end_derivative.push_back(getAcceleration(start_t)); + start_end_derivative.push_back(getAcceleration(start_t + seg_time)); + } + + // get Bspline paramterization data of a fixed duration local trajectory + // start_t: start time of the trajectory + // duration: time length of the segment + // seg_num: discretized the segment into *seg_num* parts + void getTrajByDuration(double start_t, double duration, int seg_num, + vector &point_set, + vector &start_end_derivative, double &dt) + { + dt = duration / seg_num; + Eigen::Vector3d cur_pt; + for (double tp = 0.0; tp <= duration + 1e-4; tp += dt) + { + cur_pt = getPosition(start_t + tp); + point_set.push_back(cur_pt); + } + + start_end_derivative.push_back(getVelocity(start_t)); + start_end_derivative.push_back(getVelocity(start_t + duration)); + start_end_derivative.push_back(getAcceleration(start_t)); + start_end_derivative.push_back(getAcceleration(start_t + duration)); + } + }; + + struct PlanParameters + { + /* planning algorithm parameters */ + double max_vel_, max_acc_, max_jerk_; // physical limits + double ctrl_pt_dist; // distance between adjacient B-spline control points + double feasibility_tolerance_; // permitted ratio of vel/acc exceeding limits + double planning_horizen_; + bool use_distinctive_trajs; + int drone_id; // single drone: drone_id <= -1, swarm: drone_id >= 0 + + /* processing time */ + double time_search_ = 0.0; + double time_optimize_ = 0.0; + double time_adjust_ = 0.0; + }; + + struct LocalTrajData + { + /* info of generated traj */ + + int traj_id_; + double duration_; + ros::Time start_time_; + Eigen::Vector3d start_pos_; + UniformBspline position_traj_, velocity_traj_, acceleration_traj_; + }; + + struct OneTrajDataOfSwarm + { + /* info of generated traj */ + + int drone_id; + double duration_; + ros::Time start_time_; + Eigen::Vector3d start_pos_; + UniformBspline position_traj_; + }; + + typedef std::vector SwarmTrajData; + +} // namespace ego_planner + +#endif \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/traj_utils/include/traj_utils/planning_visualization.h b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/include/traj_utils/planning_visualization.h new file mode 100644 index 00000000..06d755b4 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/include/traj_utils/planning_visualization.h @@ -0,0 +1,55 @@ +#ifndef _PLANNING_VISUALIZATION_H_ +#define _PLANNING_VISUALIZATION_H_ + +#include +#include +//#include +#include +//#include +#include +#include +#include +#include +#include + +using std::vector; +namespace ego_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); + + typedef std::shared_ptr Ptr; + + void displayMarkerList(ros::Publisher &pub, const vector &list, double scale, + Eigen::Vector4d color, int id, bool show_sphere = true); + 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 displayMultiInitPathList(vector> init_trajs, const double scale); + 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/Prometheus/Modules/ego_planner_swarm/traj_utils/include/traj_utils/polynomial_traj.h b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/include/traj_utils/polynomial_traj.h new file mode 100644 index 00000000..72bac9b4 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/include/traj_utils/polynomial_traj.h @@ -0,0 +1,336 @@ +#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; + } + + /* 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; + } + + 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/Prometheus/Modules/ego_planner_swarm/traj_utils/msg/Bspline.msg b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/msg/Bspline.msg new file mode 100755 index 00000000..df4cfd40 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/msg/Bspline.msg @@ -0,0 +1,12 @@ +int32 drone_id + +int32 order +int64 traj_id +time start_time + +float64[] knots +geometry_msgs/Point[] pos_pts + +float64[] yaw_pts +float64 yaw_dt + diff --git a/src/Prometheus/Modules/ego_planner_swarm/traj_utils/msg/DataDisp.msg b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/msg/DataDisp.msg new file mode 100644 index 00000000..742f28ad --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/msg/DataDisp.msg @@ -0,0 +1,7 @@ +Header header + +float64 a +float64 b +float64 c +float64 d +float64 e \ No newline at end of file diff --git a/src/Prometheus/Modules/ego_planner_swarm/traj_utils/msg/MultiBsplines.msg b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/msg/MultiBsplines.msg new file mode 100644 index 00000000..2f116f21 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/msg/MultiBsplines.msg @@ -0,0 +1,4 @@ +int32 drone_id_from + +Bspline[] traj + diff --git a/src/Prometheus/Modules/ego_planner_swarm/traj_utils/package.xml b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/package.xml new file mode 100755 index 00000000..481ac269 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/package.xml @@ -0,0 +1,77 @@ + + + traj_utils + 0.0.0 + The traj_utils package + + + + + bzhouai + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + + roscpp + std_msgs + message_generation + + catkin + + + + std_msgs + + catkin + + + roscpp + std_msgs + message_runtime + + + + + + + + diff --git a/src/Prometheus/Modules/ego_planner_swarm/traj_utils/src/planning_visualization.cpp b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/src/planning_visualization.cpp new file mode 100644 index 00000000..888320cb --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/src/planning_visualization.cpp @@ -0,0 +1,265 @@ +#include + +using std::cout; +using std::endl; +namespace ego_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); + } + + // // real ids used: {id, id+1000} + void PlanningVisualization::displayMarkerList(ros::Publisher &pub, const vector &list, double scale, + Eigen::Vector4d color, int id, bool show_sphere /* = true */ ) + { + 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); + //if (show_sphere) sphere.points.push_back(pt); + line_strip.points.push_back(pt); + } + //if (show_sphere) 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::displayMultiInitPathList(vector> init_trajs, const double scale) + { + + if (init_list_pub.getNumSubscribers() == 0) + { + return; + } + + static int last_nums = 0; + + for ( int id=0; id blank; + displayMarkerList(init_list_pub, blank, scale, color, id, false); + ros::Duration(0.001).sleep(); + } + last_nums = 0; + + for ( int id=0; id 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 (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/Prometheus/Modules/ego_planner_swarm/traj_utils/src/polynomial_traj.cpp b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/src/polynomial_traj.cpp new file mode 100644 index 00000000..a83b7ae1 --- /dev/null +++ b/src/Prometheus/Modules/ego_planner_swarm/traj_utils/src/polynomial_traj.cpp @@ -0,0 +1,224 @@ +#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/Prometheus/Modules/future_aircraft/CMakeLists.txt b/src/Prometheus/Modules/future_aircraft/CMakeLists.txt new file mode 100644 index 00000000..4898dbc5 --- /dev/null +++ b/src/Prometheus/Modules/future_aircraft/CMakeLists.txt @@ -0,0 +1,145 @@ +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/Prometheus/Modules/future_aircraft/conf/calib.yaml b/src/Prometheus/Modules/future_aircraft/conf/calib.yaml new file mode 100644 index 00000000..eb2442d5 --- /dev/null +++ b/src/Prometheus/Modules/future_aircraft/conf/calib.yaml @@ -0,0 +1,20 @@ +%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/Prometheus/Modules/future_aircraft/conf/calib_sitl.yaml b/src/Prometheus/Modules/future_aircraft/conf/calib_sitl.yaml new file mode 100644 index 00000000..58c2ff5a --- /dev/null +++ b/src/Prometheus/Modules/future_aircraft/conf/calib_sitl.yaml @@ -0,0 +1,17 @@ +%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/Prometheus/Modules/future_aircraft/launch/future_aircraft.launch b/src/Prometheus/Modules/future_aircraft/launch/future_aircraft.launch new file mode 100755 index 00000000..ede28625 --- /dev/null +++ b/src/Prometheus/Modules/future_aircraft/launch/future_aircraft.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/Prometheus/Modules/future_aircraft/launch/test.launch b/src/Prometheus/Modules/future_aircraft/launch/test.launch new file mode 100755 index 00000000..ef4d0877 --- /dev/null +++ b/src/Prometheus/Modules/future_aircraft/launch/test.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/src/Prometheus/Modules/future_aircraft/package.xml b/src/Prometheus/Modules/future_aircraft/package.xml new file mode 100644 index 00000000..e7fa468f --- /dev/null +++ b/src/Prometheus/Modules/future_aircraft/package.xml @@ -0,0 +1,21 @@ + + + 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/Prometheus/Modules/future_aircraft/readme.md b/src/Prometheus/Modules/future_aircraft/readme.md new file mode 100644 index 00000000..34e96243 --- /dev/null +++ b/src/Prometheus/Modules/future_aircraft/readme.md @@ -0,0 +1,286 @@ +# 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/Prometheus/Modules/future_aircraft/scripts/locus.py b/src/Prometheus/Modules/future_aircraft/scripts/locus.py new file mode 100755 index 00000000..cd9f918e --- /dev/null +++ b/src/Prometheus/Modules/future_aircraft/scripts/locus.py @@ -0,0 +1,64 @@ +#!/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/Prometheus/Modules/future_aircraft/src/ellipse_det.cpp b/src/Prometheus/Modules/future_aircraft/src/ellipse_det.cpp new file mode 100644 index 00000000..e9e1b788 --- /dev/null +++ b/src/Prometheus/Modules/future_aircraft/src/ellipse_det.cpp @@ -0,0 +1,168 @@ +#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/Prometheus/Modules/future_aircraft/src/ellipse_detector/ellipse_detector.cpp b/src/Prometheus/Modules/future_aircraft/src/ellipse_detector/ellipse_detector.cpp new file mode 100644 index 00000000..f256e57b --- /dev/null +++ b/src/Prometheus/Modules/future_aircraft/src/ellipse_detector/ellipse_detector.cpp @@ -0,0 +1,3855 @@ +#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/Prometheus/Modules/future_aircraft/src/ellipse_detector/ellipse_detector.h b/src/Prometheus/Modules/future_aircraft/src/ellipse_detector/ellipse_detector.h new file mode 100644 index 00000000..7a8a1f90 --- /dev/null +++ b/src/Prometheus/Modules/future_aircraft/src/ellipse_detector/ellipse_detector.h @@ -0,0 +1,1374 @@ +#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/Prometheus/Modules/future_aircraft/src/future_aircraft.cpp b/src/Prometheus/Modules/future_aircraft/src/future_aircraft.cpp new file mode 100644 index 00000000..a21be801 --- /dev/null +++ b/src/Prometheus/Modules/future_aircraft/src/future_aircraft.cpp @@ -0,0 +1,475 @@ +/****************************************************************************** +*例程简介: 第二届大学生未来飞行器挑战赛的实践类仿真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/Prometheus/Modules/motion_planning/CMakeLists.txt b/src/Prometheus/Modules/motion_planning/CMakeLists.txt new file mode 120000 index 00000000..66dd650a --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/CMakeLists.txt @@ -0,0 +1 @@ +/opt/ros/melodic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/src/Prometheus/Modules/motion_planning/global_planner/CMakeLists.txt b/src/Prometheus/Modules/motion_planning/global_planner/CMakeLists.txt new file mode 100644 index 00000000..02c9edc3 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/global_planner/CMakeLists.txt @@ -0,0 +1,58 @@ +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/Prometheus/Modules/motion_planning/global_planner/global_planner.md b/src/Prometheus/Modules/motion_planning/global_planner/global_planner.md new file mode 100644 index 00000000..71824e7e --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/global_planner/global_planner.md @@ -0,0 +1,78 @@ +## 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/Prometheus/Modules/motion_planning/global_planner/include/A_star.h b/src/Prometheus/Modules/motion_planning/global_planner/include/A_star.h new file mode 100644 index 00000000..87bded62 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/global_planner/include/A_star.h @@ -0,0 +1,181 @@ +#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/Prometheus/Modules/motion_planning/global_planner/include/global_planner.h b/src/Prometheus/Modules/motion_planning/global_planner/include/global_planner.h new file mode 100644 index 00000000..2d21b150 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/global_planner/include/global_planner.h @@ -0,0 +1,121 @@ +#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/Prometheus/Modules/motion_planning/global_planner/include/global_planner_utils.h b/src/Prometheus/Modules/motion_planning/global_planner/include/global_planner_utils.h new file mode 100644 index 00000000..6a4c455f --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/global_planner/include/global_planner_utils.h @@ -0,0 +1,8 @@ +#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/Prometheus/Modules/motion_planning/global_planner/include/occupy_map.h b/src/Prometheus/Modules/motion_planning/global_planner/include/occupy_map.h new file mode 100644 index 00000000..eaff87f9 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/global_planner/include/occupy_map.h @@ -0,0 +1,128 @@ +#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/Prometheus/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_2dlidar.launch b/src/Prometheus/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_2dlidar.launch new file mode 100755 index 00000000..ffb24872 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_2dlidar.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_global_point.launch b/src/Prometheus/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_global_point.launch new file mode 100755 index 00000000..29e7a507 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_global_point.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_local_point.launch b/src/Prometheus/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_local_point.launch new file mode 100755 index 00000000..12b24557 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/global_planner/launch/sitl_global_planner_with_local_point.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/motion_planning/global_planner/package.xml b/src/Prometheus/Modules/motion_planning/global_planner/package.xml new file mode 100644 index 00000000..d3f79c05 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/global_planner/package.xml @@ -0,0 +1,40 @@ + + + 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/Prometheus/Modules/motion_planning/global_planner/src/A_star.cpp b/src/Prometheus/Modules/motion_planning/global_planner/src/A_star.cpp new file mode 100644 index 00000000..d960c503 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/global_planner/src/A_star.cpp @@ -0,0 +1,367 @@ +#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/Prometheus/Modules/motion_planning/global_planner/src/global_planner.cpp b/src/Prometheus/Modules/motion_planning/global_planner/src/global_planner.cpp new file mode 100644 index 00000000..35359111 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/global_planner/src/global_planner.cpp @@ -0,0 +1,459 @@ +#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/Prometheus/Modules/motion_planning/global_planner/src/global_planner_node.cpp b/src/Prometheus/Modules/motion_planning/global_planner/src/global_planner_node.cpp new file mode 100644 index 00000000..106f4ae1 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/global_planner/src/global_planner_node.cpp @@ -0,0 +1,28 @@ +#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/Prometheus/Modules/motion_planning/global_planner/src/occupy_map.cpp b/src/Prometheus/Modules/motion_planning/global_planner/src/occupy_map.cpp new file mode 100644 index 00000000..23b4d591 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/global_planner/src/occupy_map.cpp @@ -0,0 +1,600 @@ +#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/Prometheus/Modules/motion_planning/local_planner/CMakeLists.txt b/src/Prometheus/Modules/motion_planning/local_planner/CMakeLists.txt new file mode 100644 index 00000000..8a1b1474 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/local_planner/CMakeLists.txt @@ -0,0 +1,50 @@ +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/Prometheus/Modules/motion_planning/local_planner/include/apf.h b/src/Prometheus/Modules/motion_planning/local_planner/include/apf.h new file mode 100644 index 00000000..05715914 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/local_planner/include/apf.h @@ -0,0 +1,63 @@ +#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/Prometheus/Modules/motion_planning/local_planner/include/local_planner.h b/src/Prometheus/Modules/motion_planning/local_planner/include/local_planner.h new file mode 100644 index 00000000..b19d747d --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/local_planner/include/local_planner.h @@ -0,0 +1,115 @@ +#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/Prometheus/Modules/motion_planning/local_planner/include/local_planner_alg.h b/src/Prometheus/Modules/motion_planning/local_planner/include/local_planner_alg.h new file mode 100644 index 00000000..7c315d2d --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/local_planner/include/local_planner_alg.h @@ -0,0 +1,37 @@ +#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/Prometheus/Modules/motion_planning/local_planner/include/local_planner_utils.h b/src/Prometheus/Modules/motion_planning/local_planner/include/local_planner_utils.h new file mode 100644 index 00000000..3caf4ff6 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/local_planner/include/local_planner_utils.h @@ -0,0 +1,10 @@ +#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/Prometheus/Modules/motion_planning/local_planner/include/vfh.h b/src/Prometheus/Modules/motion_planning/local_planner/include/vfh.h new file mode 100644 index 00000000..c070b735 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/local_planner/include/vfh.h @@ -0,0 +1,73 @@ +#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/Prometheus/Modules/motion_planning/local_planner/launch/sitl_apf_with_local_point.launch b/src/Prometheus/Modules/motion_planning/local_planner/launch/sitl_apf_with_local_point.launch new file mode 100755 index 00000000..6947902c --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/local_planner/launch/sitl_apf_with_local_point.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/motion_planning/local_planner/launch/sitl_vfh_with_local_point.launch b/src/Prometheus/Modules/motion_planning/local_planner/launch/sitl_vfh_with_local_point.launch new file mode 100755 index 00000000..8fa12651 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/local_planner/launch/sitl_vfh_with_local_point.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/motion_planning/local_planner/local_planner.md b/src/Prometheus/Modules/motion_planning/local_planner/local_planner.md new file mode 100644 index 00000000..1c985930 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/local_planner/local_planner.md @@ -0,0 +1,31 @@ +## 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/Prometheus/Modules/motion_planning/local_planner/package.xml b/src/Prometheus/Modules/motion_planning/local_planner/package.xml new file mode 100644 index 00000000..af2fe83b --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/local_planner/package.xml @@ -0,0 +1,37 @@ + + + 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/Prometheus/Modules/motion_planning/local_planner/src/apf.cpp b/src/Prometheus/Modules/motion_planning/local_planner/src/apf.cpp new file mode 100644 index 00000000..817eb86e --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/local_planner/src/apf.cpp @@ -0,0 +1,183 @@ +#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/Prometheus/Modules/motion_planning/local_planner/src/local_planner.cpp b/src/Prometheus/Modules/motion_planning/local_planner/src/local_planner.cpp new file mode 100644 index 00000000..1a3f3bc9 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/local_planner/src/local_planner.cpp @@ -0,0 +1,432 @@ +#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/Prometheus/Modules/motion_planning/local_planner/src/local_planner_node.cpp b/src/Prometheus/Modules/motion_planning/local_planner/src/local_planner_node.cpp new file mode 100644 index 00000000..7c40663a --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/local_planner/src/local_planner_node.cpp @@ -0,0 +1,27 @@ +#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/Prometheus/Modules/motion_planning/local_planner/src/vfh.cpp b/src/Prometheus/Modules/motion_planning/local_planner/src/vfh.cpp new file mode 100644 index 00000000..fc110b21 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/local_planner/src/vfh.cpp @@ -0,0 +1,286 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap.sh b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap.sh new file mode 100755 index 00000000..24b78d56 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap.sh @@ -0,0 +1,7 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/CMakeLists.txt b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/CMakeLists.txt new file mode 100755 index 00000000..b6b3bb13 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/CMakeLists.txt @@ -0,0 +1,221 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/include/min_snap/min_snap_closeform.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/include/min_snap/min_snap_closeform.h new file mode 100755 index 00000000..95744dd0 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/include/min_snap/min_snap_closeform.h @@ -0,0 +1,42 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/launch/default.rviz b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/launch/default.rviz new file mode 100755 index 00000000..231314b3 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/launch/default.rviz @@ -0,0 +1,312 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/launch/min_snap.launch b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/launch/min_snap.launch new file mode 100755 index 00000000..ba2be9d9 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/launch/min_snap.launch @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/launch/real.launch b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/launch/real.launch new file mode 100755 index 00000000..b4caab8f --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/launch/real.launch @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/launch/rviz.launch b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/launch/rviz.launch new file mode 100755 index 00000000..af5eb8ba --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/launch/rviz.launch @@ -0,0 +1,3 @@ + + + diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/package.xml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/package.xml new file mode 100755 index 00000000..3f7026f2 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/package.xml @@ -0,0 +1,71 @@ + + + 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/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/src/min_snap_closeform.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/src/min_snap_closeform.cpp new file mode 100755 index 00000000..f84fdeeb --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/src/min_snap_closeform.cpp @@ -0,0 +1,237 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/src/min_snap_generator.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/src/min_snap_generator.cpp new file mode 100755 index 00000000..36f9cd26 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/min_snap/src/min_snap_generator.cpp @@ -0,0 +1,137 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/readme.pdf b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/readme.pdf new file mode 100755 index 00000000..fff19f07 Binary files /dev/null and b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/readme.pdf differ diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/CMakeLists.txt b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/CMakeLists.txt new file mode 100755 index 00000000..4dbb93c4 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/CMakeLists.txt @@ -0,0 +1,217 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/launch/my_sim.xml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/launch/my_sim.xml new file mode 100755 index 00000000..bd32ade7 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/launch/my_sim.xml @@ -0,0 +1,140 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/launch/real_traj_server.xml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/launch/real_traj_server.xml new file mode 100755 index 00000000..6c56f0ed --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/launch/real_traj_server.xml @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/package.xml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/package.xml new file mode 100755 index 00000000..45d9012d --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/package.xml @@ -0,0 +1,70 @@ + + + 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/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/src/my_traj_server.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/src/my_traj_server.cpp new file mode 100755 index 00000000..5ef4a4a7 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_server/src/my_traj_server.cpp @@ -0,0 +1,264 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/CMakeLists.txt b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/CMakeLists.txt new file mode 100755 index 00000000..7e57d4b1 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/CMakeLists.txt @@ -0,0 +1,39 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/include/mini_snap_traj_utils/planning_visualization.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/include/mini_snap_traj_utils/planning_visualization.h new file mode 100755 index 00000000..a38f479f --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/include/mini_snap_traj_utils/planning_visualization.h @@ -0,0 +1,54 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/include/mini_snap_traj_utils/polynomial_traj.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/include/mini_snap_traj_utils/polynomial_traj.h new file mode 100755 index 00000000..fe9f1610 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/include/mini_snap_traj_utils/polynomial_traj.h @@ -0,0 +1,368 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/package.xml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/package.xml new file mode 100755 index 00000000..6773176b --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/package.xml @@ -0,0 +1,67 @@ + + + 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/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/src/planning_visualization.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/src/planning_visualization.cpp new file mode 100755 index 00000000..3604f493 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/src/planning_visualization.cpp @@ -0,0 +1,253 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/src/polynomial_traj.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/src/polynomial_traj.cpp new file mode 100755 index 00000000..4960544b --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/traj_utils/src/polynomial_traj.cpp @@ -0,0 +1,224 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/CMakeLists.txt b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/CMakeLists.txt new file mode 100755 index 00000000..a510ed1c --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/CMakeLists.txt @@ -0,0 +1,13 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/arch.cmake b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/arch.cmake new file mode 100755 index 00000000..9f1dcc8f --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/arch.cmake @@ -0,0 +1,126 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/cmake_modules.cmake b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/cmake_modules.cmake new file mode 100755 index 00000000..4370b380 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/cmake_modules.cmake @@ -0,0 +1,2 @@ +list(APPEND CMAKE_MODULE_PATH "${cmake_utils_SOURCE_PREFIX}/cmake_modules") +link_directories( ${cmake_utils_SOURCE_PREFIX}/lib/mosek8 ) diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/color.cmake b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/color.cmake new file mode 100755 index 00000000..bada383c --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake/color.cmake @@ -0,0 +1,17 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindEigen.cmake b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindEigen.cmake new file mode 100755 index 00000000..d45fad7c --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindEigen.cmake @@ -0,0 +1,173 @@ +# 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindGSL.cmake b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindGSL.cmake new file mode 100755 index 00000000..797b2455 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindGSL.cmake @@ -0,0 +1,135 @@ +# 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindmvIMPACT.cmake b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindmvIMPACT.cmake new file mode 100755 index 00000000..d1a61b64 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/cmake_modules/FindmvIMPACT.cmake @@ -0,0 +1,124 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/package.xml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/package.xml new file mode 100755 index 00000000..d745d2d3 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/cmake_utils/package.xml @@ -0,0 +1,18 @@ + + + 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/CMakeLists.txt b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/CMakeLists.txt new file mode 100755 index 00000000..a89dd726 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/CMakeLists.txt @@ -0,0 +1,222 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/Makefile b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/Makefile new file mode 100755 index 00000000..b75b928f --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map2D.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map2D.h new file mode 100755 index 00000000..cee4d65a --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map2D.h @@ -0,0 +1,242 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map3D.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map3D.h new file mode 100755 index 00000000..7f8aa7c0 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map3D.h @@ -0,0 +1,608 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/mainpage.dox b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/mainpage.dox new file mode 100755 index 00000000..3a1fb201 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b multi_map_server is ... + + + + +\section codeapi Code API + + + + +*/ diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/MultiOccupancyGrid.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/MultiOccupancyGrid.msg new file mode 100755 index 00000000..d7b020d3 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/MultiOccupancyGrid.msg @@ -0,0 +1,2 @@ +nav_msgs/OccupancyGrid[] maps +geometry_msgs/Pose[] origins diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/MultiSparseMap3D.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/MultiSparseMap3D.msg new file mode 100755 index 00000000..c7be2d78 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/MultiSparseMap3D.msg @@ -0,0 +1,2 @@ +SparseMap3D[] maps +geometry_msgs/Pose[] origins diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/SparseMap3D.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/SparseMap3D.msg new file mode 100755 index 00000000..777fb861 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/SparseMap3D.msg @@ -0,0 +1,4 @@ +Header header +nav_msgs/MapMetaData info +VerticalOccupancyGridList[] lists + diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/VerticalOccupancyGridList.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/VerticalOccupancyGridList.msg new file mode 100755 index 00000000..379ffaae --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/msg/VerticalOccupancyGridList.msg @@ -0,0 +1,6 @@ +float32 x +float32 y +int32[] upper +int32[] lower +int32[] mass + diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/package.xml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/package.xml new file mode 100755 index 00000000..f037f19b --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/package.xml @@ -0,0 +1,38 @@ + + 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/__init__.py b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/__init__.py new file mode 100755 index 00000000..b23e43d7 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/__init__.py @@ -0,0 +1 @@ +#autogenerated by ROS python message generators \ No newline at end of file diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/_MultiOccupancyGrid.py b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/_MultiOccupancyGrid.py new file mode 100755 index 00000000..bf2de496 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/_MultiOccupancyGrid.py @@ -0,0 +1,404 @@ +"""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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/__init__.py b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/__init__.py new file mode 100755 index 00000000..29e57955 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/__init__.py @@ -0,0 +1,4 @@ +from ._SparseMap3D import * +from ._MultiOccupancyGrid import * +from ._MultiSparseMap3D import * +from ._VerticalOccupancyGridList import * diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_visualization.cc b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_visualization.cc new file mode 100755 index 00000000..7013fefc --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/multi_map_visualization.cc @@ -0,0 +1,90 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/unused/multi_map_server.cc b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/unused/multi_map_server.cc new file mode 100755 index 00000000..a96d953b --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/multi_map_server/src/unused/multi_map_server.cc @@ -0,0 +1,192 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/CMakeLists.txt b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/CMakeLists.txt new file mode 100755 index 00000000..396d4a58 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/CMakeLists.txt @@ -0,0 +1,211 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/Makefile b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/Makefile new file mode 100755 index 00000000..b75b928f --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/mainpage.dox b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/mainpage.dox new file mode 100755 index 00000000..3c09d69e --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b odom_visualization is ... + + + + +\section codeapi Code API + + + + +*/ diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/meshes/hummingbird.mesh b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/meshes/hummingbird.mesh new file mode 100755 index 00000000..dcde17c5 Binary files /dev/null and b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/meshes/hummingbird.mesh differ diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/package.xml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/package.xml new file mode 100755 index 00000000..f8756918 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/package.xml @@ -0,0 +1,31 @@ + + 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp new file mode 100755 index 00000000..67c57fb0 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp @@ -0,0 +1,472 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/CMakeLists.txt b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/CMakeLists.txt new file mode 100755 index 00000000..87beff30 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/CMakeLists.txt @@ -0,0 +1,26 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/Makefile b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/Makefile new file mode 100755 index 00000000..b75b928f --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/include/pose_utils.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/include/pose_utils.h new file mode 100755 index 00000000..326c7c8f --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/include/pose_utils.h @@ -0,0 +1,53 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/package.xml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/package.xml new file mode 100755 index 00000000..df593f9b --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/package.xml @@ -0,0 +1,15 @@ + + pose_utils + 0.0.0 + pose_utils + Shaojie Shen + BSD + catkin + roscpp + roscpp + + + + + diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/src/pose_utils.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/src/pose_utils.cpp new file mode 100755 index 00000000..f2b43657 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/pose_utils/src/pose_utils.cpp @@ -0,0 +1,531 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/CMakeLists.txt b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/CMakeLists.txt new file mode 100755 index 00000000..9838d093 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/CMakeLists.txt @@ -0,0 +1,110 @@ +#-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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/cmake/FindEigen3.cmake b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/cmake/FindEigen3.cmake new file mode 100755 index 00000000..9c546a05 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/cmake/FindEigen3.cmake @@ -0,0 +1,81 @@ +# - 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/comm_types.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/comm_types.h new file mode 100755 index 00000000..42a08aa2 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/comm_types.h @@ -0,0 +1,87 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/decode_msgs.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/decode_msgs.h new file mode 100755 index 00000000..a3076ea7 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/decode_msgs.h @@ -0,0 +1,23 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/encode_msgs.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/encode_msgs.h new file mode 100755 index 00000000..a424b119 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/encode_msgs.h @@ -0,0 +1,22 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/AuxCommand.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/AuxCommand.msg new file mode 100755 index 00000000..f59bf356 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/AuxCommand.msg @@ -0,0 +1,5 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Corrections.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Corrections.msg new file mode 100755 index 00000000..e0f4e888 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Corrections.msg @@ -0,0 +1,2 @@ +float64 kf_correction +float64[2] angle_corrections diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Gains.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Gains.msg new file mode 100755 index 00000000..f5d10a33 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Gains.msg @@ -0,0 +1,4 @@ +float64 Kp +float64 Kd +float64 Kp_yaw +float64 Kd_yaw diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/LQRTrajectory.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/LQRTrajectory.msg new file mode 100755 index 00000000..0a34e9b6 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/LQRTrajectory.msg @@ -0,0 +1,30 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Odometry.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Odometry.msg new file mode 100755 index 00000000..3272d71a --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Odometry.msg @@ -0,0 +1,8 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/OutputData.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/OutputData.msg new file mode 100755 index 00000000..ac958880 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/OutputData.msg @@ -0,0 +1,12 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PPROutputData.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PPROutputData.msg new file mode 100755 index 00000000..70434a02 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PPROutputData.msg @@ -0,0 +1,16 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg new file mode 100755 index 00000000..0aab297a --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg @@ -0,0 +1,28 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg new file mode 100755 index 00000000..49c6fa1d --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg @@ -0,0 +1,22 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Px4ctrlDebug.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Px4ctrlDebug.msg new file mode 100755 index 00000000..f28ba2c3 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Px4ctrlDebug.msg @@ -0,0 +1,37 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/SO3Command.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/SO3Command.msg new file mode 100755 index 00000000..d3868efb --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/SO3Command.msg @@ -0,0 +1,6 @@ +Header header +geometry_msgs/Vector3 force +geometry_msgs/Quaternion orientation +float64[3] kR +float64[3] kOm +quadrotor_msgs/AuxCommand aux diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Serial.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Serial.msg new file mode 100755 index 00000000..5a54cce3 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/Serial.msg @@ -0,0 +1,13 @@ +# 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/StatusData.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/StatusData.msg new file mode 100755 index 00000000..d176e4f0 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/StatusData.msg @@ -0,0 +1,4 @@ +Header header +uint16 loop_rate +float64 voltage +uint8 seq diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/TRPYCommand.msg b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/TRPYCommand.msg new file mode 100755 index 00000000..0d471a62 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/msg/TRPYCommand.msg @@ -0,0 +1,6 @@ +Header header +float32 thrust +float32 roll +float32 pitch +float32 yaw +quadrotor_msgs/AuxCommand aux diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/package.xml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/package.xml new file mode 100755 index 00000000..42e9f76e --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/package.xml @@ -0,0 +1,20 @@ + + 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/decode_msgs.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/decode_msgs.cpp new file mode 100755 index 00000000..609ffb84 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/decode_msgs.cpp @@ -0,0 +1,103 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/encode_msgs.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/encode_msgs.cpp new file mode 100755 index 00000000..d0cccf2a --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/encode_msgs.cpp @@ -0,0 +1,73 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/__init__.py b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/__init__.py new file mode 100755 index 00000000..e69de29b diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_AuxCommand.py b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_AuxCommand.py new file mode 100755 index 00000000..652a2558 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_AuxCommand.py @@ -0,0 +1,143 @@ +"""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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Corrections.py b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Corrections.py new file mode 100755 index 00000000..d2b19c21 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Corrections.py @@ -0,0 +1,111 @@ +"""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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_OutputData.py b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_OutputData.py new file mode 100755 index 00000000..357c5f1e --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_OutputData.py @@ -0,0 +1,277 @@ +"""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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/package.xml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/package.xml new file mode 100755 index 00000000..e025d755 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/package.xml @@ -0,0 +1,30 @@ + + 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/plugin_description.xml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/plugin_description.xml new file mode 100755 index 00000000..e7cd4dc7 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/plugin_description.xml @@ -0,0 +1,30 @@ + + + + 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.cpp new file mode 100755 index 00000000..4d5ab223 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.cpp @@ -0,0 +1,538 @@ +/* + * 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.h new file mode 100755 index 00000000..72504f12 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.h @@ -0,0 +1,134 @@ +/* + * 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/goal_tool.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/goal_tool.cpp new file mode 100755 index 00000000..a5676585 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/goal_tool.cpp @@ -0,0 +1,81 @@ +/* + * 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/goal_tool.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/goal_tool.h new file mode 100755 index 00000000..dd69e07f --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/goal_tool.h @@ -0,0 +1,72 @@ +/* + * 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.cpp new file mode 100755 index 00000000..131b1183 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.cpp @@ -0,0 +1,365 @@ +/* + * 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.h new file mode 100755 index 00000000..ee64fc5c --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.h @@ -0,0 +1,114 @@ +/* + * 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/pose_tool.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/pose_tool.cpp new file mode 100755 index 00000000..8249d1e1 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/pose_tool.cpp @@ -0,0 +1,167 @@ +/* + * 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/pose_tool.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/pose_tool.h new file mode 100755 index 00000000..f2aa36cd --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/pose_tool.h @@ -0,0 +1,80 @@ +/* + * 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/probmap_display.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/probmap_display.cpp new file mode 100755 index 00000000..608e2e24 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/probmap_display.cpp @@ -0,0 +1,535 @@ +/* + * 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/probmap_display.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/probmap_display.h new file mode 100755 index 00000000..332def21 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/rviz_plugins/src/probmap_display.h @@ -0,0 +1,134 @@ +/* + * 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/CMakeLists.txt b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/CMakeLists.txt new file mode 100755 index 00000000..6a905522 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/CMakeLists.txt @@ -0,0 +1,100 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/converters.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/converters.h new file mode 100755 index 00000000..9932731d --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/converters.h @@ -0,0 +1,95 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/geometry_utils.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/geometry_utils.h new file mode 100755 index 00000000..c3babb1c --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/geometry_utils.h @@ -0,0 +1,248 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/utils.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/utils.h new file mode 100755 index 00000000..0ce4c281 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/include/uav_utils/utils.h @@ -0,0 +1,59 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/package.xml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/package.xml new file mode 100755 index 00000000..db864b75 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/package.xml @@ -0,0 +1,58 @@ + + + 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/odom_to_euler.py b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/odom_to_euler.py new file mode 100755 index 00000000..92ac8799 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/odom_to_euler.py @@ -0,0 +1,69 @@ +#!/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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/send_odom.py b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/send_odom.py new file mode 100755 index 00000000..a08b586c --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/send_odom.py @@ -0,0 +1,42 @@ +#!/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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/tf_assist.py b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/tf_assist.py new file mode 100755 index 00000000..259e8a18 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/tf_assist.py @@ -0,0 +1,162 @@ +#!/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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/topic_statistics.py b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/topic_statistics.py new file mode 100755 index 00000000..c69ffb64 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/scripts/topic_statistics.py @@ -0,0 +1,15 @@ +#!/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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/src/uav_utils_test.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/src/uav_utils_test.cpp new file mode 100755 index 00000000..60da5afb --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/uav_utils/src/uav_utils_test.cpp @@ -0,0 +1,122 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/CMakeLists.txt b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/CMakeLists.txt new file mode 100755 index 00000000..eb4e7524 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/CMakeLists.txt @@ -0,0 +1,35 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/package.xml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/package.xml new file mode 100755 index 00000000..2c6078d6 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/package.xml @@ -0,0 +1,25 @@ + + 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/src/sample_waypoints.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/src/sample_waypoints.h new file mode 100755 index 00000000..8daad3c9 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/src/sample_waypoints.h @@ -0,0 +1,213 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/src/waypoint_generator.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/src/waypoint_generator.cpp new file mode 100755 index 00000000..1eb2d043 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/Utils/waypoint_generator/src/waypoint_generator.cpp @@ -0,0 +1,258 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/CMakeLists.txt b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/CMakeLists.txt new file mode 100755 index 00000000..fcc912cd --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/CMakeLists.txt @@ -0,0 +1,228 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/include/my_visualization/plan_visual.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/include/my_visualization/plan_visual.h new file mode 100755 index 00000000..ede173a8 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/include/my_visualization/plan_visual.h @@ -0,0 +1,41 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/package.xml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/package.xml new file mode 100755 index 00000000..141b5394 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/package.xml @@ -0,0 +1,68 @@ + + + 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/min_snap_visual.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/min_snap_visual.cpp new file mode 100755 index 00000000..de217a9b --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/min_snap_visual.cpp @@ -0,0 +1,92 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/obvp_visual.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/obvp_visual.cpp new file mode 100755 index 00000000..1f2bf5d0 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/obvp_visual.cpp @@ -0,0 +1,90 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/plan_visual.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/plan_visual.cpp new file mode 100755 index 00000000..602fffa7 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/my_visualization/src/plan_visual.cpp @@ -0,0 +1,162 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/CMakeLists.txt b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/CMakeLists.txt new file mode 100755 index 00000000..bb6b0c7a --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/CMakeLists.txt @@ -0,0 +1,237 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/corrections_hummingbird.yaml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/corrections_hummingbird.yaml new file mode 100755 index 00000000..aeadaab3 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/corrections_hummingbird.yaml @@ -0,0 +1,4 @@ +corrections: + z: 0.0 + r: 0.0 + p: 0.0 diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/corrections_pelican.yaml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/corrections_pelican.yaml new file mode 100755 index 00000000..aeadaab3 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/corrections_pelican.yaml @@ -0,0 +1,4 @@ +corrections: + z: 0.0 + r: 0.0 + p: 0.0 diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains.yaml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains.yaml new file mode 100755 index 00000000..ffde85a8 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains.yaml @@ -0,0 +1,6 @@ +# 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains_hummingbird.yaml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains_hummingbird.yaml new file mode 100755 index 00000000..a1d016a6 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains_hummingbird.yaml @@ -0,0 +1,6 @@ +# 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains_pelican.yaml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains_pelican.yaml new file mode 100755 index 00000000..ffde85a8 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/config/gains_pelican.yaml @@ -0,0 +1,6 @@ +# 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/include/so3_control/SO3Control.h b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/include/so3_control/SO3Control.h new file mode 100755 index 00000000..afc2d959 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/include/so3_control/SO3Control.h @@ -0,0 +1,42 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/mainpage.dox b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/mainpage.dox new file mode 100755 index 00000000..101a5b1f --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/mainpage.dox @@ -0,0 +1,14 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b so3_control + + + +--> + + +*/ diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/nodelet_plugin.xml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/nodelet_plugin.xml new file mode 100755 index 00000000..af407f6c --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/nodelet_plugin.xml @@ -0,0 +1,7 @@ + + + + so3_control + + + diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/package.xml b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/package.xml new file mode 100755 index 00000000..c9273a42 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/package.xml @@ -0,0 +1,35 @@ + + 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/SO3Control.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/SO3Control.cpp new file mode 100755 index 00000000..9d512b87 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/SO3Control.cpp @@ -0,0 +1,124 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/control_example.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/control_example.cpp new file mode 100755 index 00000000..15a1738d --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/control_example.cpp @@ -0,0 +1,78 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/so3_control_nodelet.cpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/so3_control_nodelet.cpp new file mode 100755 index 00000000..8135ed2d --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_control/src/so3_control_nodelet.cpp @@ -0,0 +1,245 @@ +#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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/CMakeLists.txt b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/CMakeLists.txt new file mode 100755 index 00000000..6fd4e2fd --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/CMakeLists.txt @@ -0,0 +1,51 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/config/rviz.rviz b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/config/rviz.rviz new file mode 100755 index 00000000..0a8060b0 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/config/rviz.rviz @@ -0,0 +1,212 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/CHANGELOG b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/CHANGELOG new file mode 100755 index 00000000..b1537eb1 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/CHANGELOG @@ -0,0 +1,9 @@ +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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/Jamroot b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/Jamroot new file mode 100755 index 00000000..db8bfbce --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/Jamroot @@ -0,0 +1,44 @@ +# 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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/README b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/README new file mode 100755 index 00000000..f34601f5 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/README @@ -0,0 +1 @@ +odeint is a highly flexible library for solving ordinary differential equations. diff --git a/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint.hpp new file mode 100755 index 00000000..3200fbe1 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint.hpp @@ -0,0 +1,75 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/array_algebra.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/array_algebra.hpp new file mode 100755 index 00000000..096e427f --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/array_algebra.hpp @@ -0,0 +1,265 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/for_each.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/for_each.hpp new file mode 100755 index 00000000..8693e845 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/for_each.hpp @@ -0,0 +1,165 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/macros.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/macros.hpp new file mode 100755 index 00000000..ef97327c --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/macros.hpp @@ -0,0 +1,43 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/reduce.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/reduce.hpp new file mode 100755 index 00000000..247d03df --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/reduce.hpp @@ -0,0 +1,67 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/fusion_algebra.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/fusion_algebra.hpp new file mode 100755 index 00000000..81c4070f --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/fusion_algebra.hpp @@ -0,0 +1,186 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/range_algebra.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/range_algebra.hpp new file mode 100755 index 00000000..c54e09bf --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/range_algebra.hpp @@ -0,0 +1,159 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/vector_space_algebra.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/vector_space_algebra.hpp new file mode 100755 index 00000000..4673b9b2 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/vector_space_algebra.hpp @@ -0,0 +1,156 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/config.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/config.hpp new file mode 100755 index 00000000..0d08982b --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/config.hpp @@ -0,0 +1,47 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/gsl/gsl_wrapper.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/gsl/gsl_wrapper.hpp new file mode 100755 index 00000000..c961a894 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/gsl/gsl_wrapper.hpp @@ -0,0 +1,229 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mkl/mkl_operations.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mkl/mkl_operations.hpp new file mode 100755 index 00000000..c1e08914 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mkl/mkl_operations.hpp @@ -0,0 +1,181 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/implicit_euler_mtl4.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/implicit_euler_mtl4.hpp new file mode 100755 index 00000000..1e7afe6e --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/implicit_euler_mtl4.hpp @@ -0,0 +1,162 @@ +/* +[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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/mtl4_resize.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/mtl4_resize.hpp new file mode 100755 index 00000000..b7f94d1f --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/mtl4_resize.hpp @@ -0,0 +1,133 @@ +/* +[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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_algebra.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_algebra.hpp new file mode 100755 index 00000000..0f6b5939 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_algebra.hpp @@ -0,0 +1,198 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_operations.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_operations.hpp new file mode 100755 index 00000000..710657ae --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_operations.hpp @@ -0,0 +1,252 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_resize.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_resize.hpp new file mode 100755 index 00000000..843a1003 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_resize.hpp @@ -0,0 +1,119 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/vexcl/vexcl_resize.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/vexcl/vexcl_resize.hpp new file mode 100755 index 00000000..e9cbea31 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/vexcl/vexcl_resize.hpp @@ -0,0 +1,92 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_operations.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_operations.hpp new file mode 100755 index 00000000..d621b35e --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_operations.hpp @@ -0,0 +1,245 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_resize.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_resize.hpp new file mode 100755 index 00000000..677cfce8 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_resize.hpp @@ -0,0 +1,65 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp new file mode 100755 index 00000000..071159f7 --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp @@ -0,0 +1,154 @@ +/* + [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/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_const.hpp b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_const.hpp new file mode 100755 index 00000000..2eb7ae0a --- /dev/null +++ b/src/Prometheus/Modules/motion_planning/min_snap_trajectory/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_const.hpp @@ -0,0 +1,159 @@ +/* + [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