Compare commits
9 Commits
master
...
liuyunhua_
| Author | SHA1 | Date |
|---|---|---|
 Amov
|
0673fba5e1 | 2 years ago |
|
liuyunhua
|
1f10821146 | 2 years ago |
|
liuyunhua
|
1ddbf039c5 | 2 years ago |
|
liuyunhua
|
0dfc44d64a | 2 years ago |
|
liuyunhua
|
7f1a599a07 | 2 years ago |
|
liuyunhua
|
3e1ca0c072 | 2 years ago |
|
liuyunhua
|
fbca701f9c | 2 years ago |
|
liuyunhua
|
e48e145170 | 2 years ago |
 Dingzijian
|
0eb2473759 | 2 years ago |
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -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
|
||||
|
||||
@ -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
|
||||
@ -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)
|
||||
@ -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:
|
||||
@ -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_*'
|
||||
@ -0,0 +1,29 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>prometheus_experiment</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The prometheus_experiment package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<maintainer email="qiyh8@mail.sysu.edu.cn">Yuhua Qi</maintainer>
|
||||
<author email="fatmoonqyp@126.com">Yuhua Qi</author>
|
||||
<license>TODO</license>
|
||||
|
||||
<url type="website">https://www.amovlab.com</url>
|
||||
<url type="repository">https://github.com/potato77/prometheus_experiment.git</url>
|
||||
|
||||
<build_depend>message_generation</build_depend>
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
|
||||
|
||||
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
</export>
|
||||
</package>
|
||||
@ -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.
|
||||
@ -0,0 +1 @@
|
||||
/opt/ros/melodic/share/catkin/cmake/toplevel.cmake
|
||||
@ -0,0 +1,248 @@
|
||||
#ifndef __GEOMETRY_UTILS_H
|
||||
#define __GEOMETRY_UTILS_H
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
/* clang-format off */
|
||||
namespace geometry_utils {
|
||||
|
||||
template <typename Scalar_t>
|
||||
Scalar_t toRad(const Scalar_t& x) {
|
||||
return x / 180.0 * M_PI;
|
||||
}
|
||||
|
||||
template <typename Scalar_t>
|
||||
Scalar_t toDeg(const Scalar_t& x) {
|
||||
return x * 180.0 / M_PI;
|
||||
}
|
||||
|
||||
template <typename Scalar_t>
|
||||
Eigen::Matrix<Scalar_t, 3, 3> rotx(Scalar_t t) {
|
||||
Eigen::Matrix<Scalar_t, 3, 3> 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 <typename Scalar_t>
|
||||
Eigen::Matrix<Scalar_t, 3, 3> roty(Scalar_t t) {
|
||||
Eigen::Matrix<Scalar_t, 3, 3> 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 <typename Scalar_t>
|
||||
Eigen::Matrix<Scalar_t, 3, 3> rotz(Scalar_t t) {
|
||||
Eigen::Matrix<Scalar_t, 3, 3> 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 <typename Derived>
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 3> ypr_to_R(const Eigen::DenseBase<Derived>& 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<typename Derived::Scalar, 3, 3> Rz = Eigen::Matrix<typename Derived::Scalar, 3, 3>::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<typename Derived::Scalar, 3, 3> Ry = Eigen::Matrix<typename Derived::Scalar, 3, 3>::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<typename Derived::Scalar, 3, 3> Rx = Eigen::Matrix<typename Derived::Scalar, 3, 3>::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<typename Derived::Scalar, 3, 3> R = Rz * Ry * Rx;
|
||||
return R;
|
||||
}
|
||||
|
||||
template <typename Derived>
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 1> R_to_ypr(const Eigen::DenseBase<Derived>& 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<typename Derived::Scalar, 3, 1> n = R.col(0);
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 1> o = R.col(1);
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 1> a = R.col(2);
|
||||
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 1> 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 <typename Derived>
|
||||
Eigen::Quaternion<typename Derived::Scalar> ypr_to_quaternion(const Eigen::DenseBase<Derived>& 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<typename Derived::Scalar> 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 <typename Scalar_t>
|
||||
Eigen::Matrix<Scalar_t, 3, 1> quaternion_to_ypr(const Eigen::Quaternion<Scalar_t>& q_) {
|
||||
Eigen::Quaternion<Scalar_t> q = q_.normalized();
|
||||
|
||||
Eigen::Matrix<Scalar_t, 3, 1> 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 <typename Scalar_t>
|
||||
Scalar_t get_yaw_from_quaternion(const Eigen::Quaternion<Scalar_t>& q) {
|
||||
return quaternion_to_ypr(q)(0);
|
||||
}
|
||||
|
||||
template <typename Scalar_t>
|
||||
Eigen::Quaternion<Scalar_t> yaw_to_quaternion(Scalar_t yaw) {
|
||||
return Eigen::Quaternion<Scalar_t>(rotz(yaw));
|
||||
}
|
||||
|
||||
template <typename Scalar_t>
|
||||
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 <typename Scalar_t>
|
||||
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 <typename Scalar_t>
|
||||
Scalar_t yaw_add(Scalar_t a, Scalar_t b) {
|
||||
return angle_add(a, b);
|
||||
}
|
||||
|
||||
template <typename Derived>
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 3> get_skew_symmetric(const Eigen::DenseBase<Derived>& 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<typename Derived::Scalar, 3, 3> 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 <typename Derived>
|
||||
Eigen::Matrix<typename Derived::Scalar, 3, 1> from_skew_symmetric(const Eigen::DenseBase<Derived>& 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<typename Derived::Scalar, 3, 1> v;
|
||||
v(0) = M(2, 1);
|
||||
v(1) = -M(2, 0);
|
||||
v(2) = M(1, 0);
|
||||
|
||||
assert(v.isApprox(Eigen::Matrix<typename Derived::Scalar, 3, 1>(-M(1, 2), M(0, 2), -M(0, 1))));
|
||||
|
||||
return v;
|
||||
}
|
||||
|
||||
|
||||
} // end of namespace geometry_utils
|
||||
/* clang-format on */
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,127 @@
|
||||
#ifndef MATH_UTILS_H
|
||||
#define MATH_UTILS_H
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <math.h>
|
||||
|
||||
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
|
||||
@ -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
|
||||
)
|
||||
@ -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
|
||||
@ -0,0 +1,8 @@
|
||||
# 目标框相关信息
|
||||
string Class # 类别
|
||||
float64 probability # 置信度
|
||||
int64 xmin # 右上角
|
||||
int64 ymin
|
||||
int64 xmax # 坐下角
|
||||
int64 ymax
|
||||
|
||||
@ -0,0 +1,3 @@
|
||||
Header header
|
||||
Header image_header
|
||||
BoundingBox[] bounding_boxes
|
||||
@ -0,0 +1,9 @@
|
||||
#目标框的位置（主要斜对角两个点）
|
||||
float32 left
|
||||
float32 top
|
||||
float32 bot
|
||||
float32 right
|
||||
|
||||
|
||||
## TRACK TARGET(目标框ID)
|
||||
int32 trackIds
|
||||
@ -0,0 +1,5 @@
|
||||
#队形位置
|
||||
geometry_msgs/Point[] formation_poses
|
||||
|
||||
#位置点是否选取
|
||||
bool[] assigned
|
||||
@ -0,0 +1,3 @@
|
||||
float64 latitude
|
||||
float64 longitude
|
||||
float64 altitude
|
||||
@ -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
|
||||
@ -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
|
||||
@ -0,0 +1,7 @@
|
||||
std_msgs/Header header
|
||||
|
||||
## 检测到的aruco码数量
|
||||
int32 num_arucos
|
||||
|
||||
## 每个aruco码的检测结果
|
||||
ArucoInfo[] aruco_infos
|
||||
@ -0,0 +1,10 @@
|
||||
Header header
|
||||
|
||||
## 检测到的目标数量
|
||||
int32 num_objs
|
||||
|
||||
## Detecting or Tracking (0:detect, 1:track)
|
||||
int32 detect_or_track
|
||||
|
||||
## 每个目标的检测结果
|
||||
DetectionInfo[] detection_infos
|
||||
@ -0,0 +1,7 @@
|
||||
std_msgs/Header header
|
||||
|
||||
##
|
||||
int32 uav_num
|
||||
|
||||
## 从1开始
|
||||
UAVState[] uav_state_all
|
||||
@ -0,0 +1,3 @@
|
||||
uint8 uav_id
|
||||
float32 x
|
||||
float32 y
|
||||
@ -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
|
||||
@ -0,0 +1,3 @@
|
||||
float64 latitude
|
||||
float64 longitude
|
||||
float64 altitude
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -0,0 +1,30 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>prometheus_msgs</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The prometheus_msgs package</description>
|
||||
|
||||
<maintainer email="qiyh8@mail.sysu.edu.cn">Yuhua Qi</maintainer>
|
||||
|
||||
<license>TODO</license>
|
||||
|
||||
<build_depend>message_generation</build_depend>
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>actionlib_msgs</build_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<build_export_depend>actionlib_msgs</build_export_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>actionlib_msgs</exec_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_export_depend>geometry_msgs</build_export_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_export_depend>sensor_msgs</build_export_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
</export>
|
||||
</package>
|
||||
@ -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})
|
||||
|
||||
|
||||
@ -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
|
||||
@ -0,0 +1,2 @@
|
||||
float64 kf_correction
|
||||
float64[2] angle_corrections
|
||||
@ -0,0 +1,4 @@
|
||||
float64 Kp
|
||||
float64 Kd
|
||||
float64 Kp_yaw
|
||||
float64 Kd_yaw
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
|
||||
@ -0,0 +1,6 @@
|
||||
Header header
|
||||
geometry_msgs/Vector3 force
|
||||
geometry_msgs/Quaternion orientation
|
||||
float64[3] kR
|
||||
float64[3] kOm
|
||||
quadrotor_msgs/AuxCommand aux
|
||||
@ -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
|
||||
@ -0,0 +1,4 @@
|
||||
Header header
|
||||
uint16 loop_rate
|
||||
float64 voltage
|
||||
uint8 seq
|
||||
@ -0,0 +1,6 @@
|
||||
Header header
|
||||
float32 thrust
|
||||
float32 roll
|
||||
float32 pitch
|
||||
float32 yaw
|
||||
quadrotor_msgs/AuxCommand aux
|
||||
@ -0,0 +1,20 @@
|
||||
<package>
|
||||
<name>quadrotor_msgs</name>
|
||||
<version>0.0.0</version>
|
||||
<description>quadrotor_msgs</description>
|
||||
<maintainer email="todo@todo.todo">Kartik Mohta</maintainer>
|
||||
<url>http://ros.org/wiki/quadrotor_msgs</url>
|
||||
<license>BSD</license>
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
<run_depend>nav_msgs</run_depend>
|
||||
<run_depend>message_runtime</run_depend>
|
||||
<export>
|
||||
<cpp cflags="-I${prefix}/include"
|
||||
lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib"/>
|
||||
</export>
|
||||
|
||||
</package>
|
||||
@ -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)
|
||||
|
||||
@ -0,0 +1,50 @@
|
||||
#ifndef AUTONOMOUS_LANDING_TOPIC_HPP
|
||||
#define AUTONOMOUS_LANDING_TOPIC_HPP
|
||||
|
||||
#include <ros/ros.h>
|
||||
#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
|
||||
@ -0,0 +1,39 @@
|
||||
#ifndef GimbalBasic_HPP
|
||||
#define GimbalBasic_HPP
|
||||
|
||||
#include <ros/ros.h>
|
||||
#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
|
||||
@ -0,0 +1,26 @@
|
||||
#ifndef OBJECT_TRACKING_TOPIC_HPP
|
||||
#define OBJECT_TRACKING_TOPIC_HPP
|
||||
|
||||
#include <ros/ros.h>
|
||||
#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
|
||||
@ -0,0 +1,90 @@
|
||||
#ifndef SWARM_CONTROL_TOPIC_HPP
|
||||
#define SWARM_CONTROL_TOPIC_HPP
|
||||
|
||||
#include <ros/ros.h>
|
||||
#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 <vector>
|
||||
|
||||
using namespace std;
|
||||
|
||||
//订阅: /prometheus/formation_assign/result
|
||||
//发布: /Prometheus/formation_assign/info
|
||||
|
||||
struct MultiUAVState
|
||||
{
|
||||
int uav_num;
|
||||
std::vector<UAVState> 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<ros::Publisher> simulation_communication_state_pub;
|
||||
|
||||
bool is_simulation_;
|
||||
|
||||
std::string udp_ip, multicast_udp_ip;
|
||||
|
||||
std::string user_type_ = "";
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,75 @@
|
||||
#ifndef UAV_BASIC_TOPIC_HPP
|
||||
#define UAV_BASIC_TOPIC_HPP
|
||||
|
||||
#include <ros/ros.h>
|
||||
#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
|
||||
@ -0,0 +1,63 @@
|
||||
#ifndef UGV_BASIC_TOPIC_HPP
|
||||
#define UGV_BASIC_TOPIC_HPP
|
||||
|
||||
#include <ros/ros.h>
|
||||
#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
|
||||
@ -0,0 +1,34 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>prometheus_communication_bridge</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The ground_station_bridge module</description>
|
||||
|
||||
<maintainer email="Amov@gmail.com">Amov</maintainer>
|
||||
<license>Aomv</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>std_srvs</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<build_export_depend>std_srvs</build_export_depend>
|
||||
<build_export_depend>geometry_msgs</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>std_srvs</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_export_depend>sensor_msgs</build_export_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<build_depend>mavros_msgs</build_depend>
|
||||
<build_export_depend>mavros_msgs</build_export_depend>
|
||||
<exec_depend>mavros_msgs</exec_depend>
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
</export>
|
||||
</package>
|
||||
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,98 @@
|
||||
#ifndef COMUNICATION_HPP
|
||||
#define COMUNICATION_HPP
|
||||
|
||||
#include <unistd.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
|
||||
#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 <typename T>
|
||||
int encodeMsg(int8_t send_mode, T msg);
|
||||
|
||||
//解码
|
||||
int decodeMsg(char *buff);
|
||||
|
||||
//根据传入的struct返回对应的MSG_ID
|
||||
template <typename T>
|
||||
uint8_t getMsgId(T msg);
|
||||
|
||||
template <typename T>
|
||||
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
|
||||
Binary file not shown.
@ -0,0 +1,76 @@
|
||||
#include "autonomous_landing_topic.hpp"
|
||||
|
||||
AutonomousLanding::AutonomousLanding(ros::NodeHandle &nh,Communication *communication)
|
||||
{
|
||||
nh.param<int>("ROBOT_ID", robot_id, 0);
|
||||
nh.param<std::string>("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<std_srvs::SetBool>("/gimbal/search");
|
||||
//【服务】是否开启视频录制
|
||||
this->gimbal_record_video_client_ = nh.serviceClient<std_srvs::SetBool>("/gimbal/record_video");
|
||||
//【服务】是否自动反馈(真实IMU速度)
|
||||
this->gimbal_track_mode_client_ = nh.serviceClient<std_srvs::SetBool>("/gimbal/track_mode");
|
||||
//【服务】自主降落参数配置
|
||||
this->param_set_client_ = nh.serviceClient<mavros_msgs::ParamSet>("/autonomous_landing/ParamSet");
|
||||
//【发布】无人车数据
|
||||
this->ugv_state_pub_ = nh.advertise<prometheus_msgs::RheaState>("/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_);
|
||||
}
|
||||
@ -0,0 +1,93 @@
|
||||
#include "gimbal_basic_topic.hpp"
|
||||
|
||||
GimbalBasic::GimbalBasic(ros::NodeHandle &nh,Communication *communication)
|
||||
{
|
||||
nh.param<std::string>("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<prometheus_msgs::WindowPosition>("/detection/bbox_draw", 1000);
|
||||
//【发布】吊舱控制
|
||||
this->gimbal_control_pub_ = nh.advertise<prometheus_msgs::GimbalControl>("/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_);
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
@ -0,0 +1,30 @@
|
||||
#include "object_tracking_topic.hpp"
|
||||
|
||||
ObjectTracking::ObjectTracking(ros::NodeHandle &nh,Communication *communication)
|
||||
{
|
||||
nh.param<std::string>("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);
|
||||
}
|
||||
@ -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_msgs::SwarmCommand>("/prometheus/swarm_command", 1000);
|
||||
//【发布】连接是否失效
|
||||
this->communication_state_pub_ = nh.advertise<std_msgs::Bool>("/uav" + std::to_string(id) + "/prometheus/communication_state", 10);
|
||||
//【发布】所有无人机状态
|
||||
this->all_uav_state_pub_ = nh.advertise<prometheus_msgs::MultiUAVState>("/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<std_msgs::Bool>("/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_msgs::MultiUAVState>("/prometheus/all_uav_state", 1000);
|
||||
//【发布】集群控制指令
|
||||
this->swarm_command_pub_ = nh.advertise<prometheus_msgs::SwarmCommand>("/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<std::string>("ground_stationt_ip", udp_ip, "127.0.0.1");
|
||||
nh.param<std::string>("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():"<<simulation_communication_state_pub.;
|
||||
// for(auto it = ; i < simulation_communication_state_pub.size();i++)
|
||||
// {
|
||||
// std::cout << " i 1: " << i << std::endl;
|
||||
// simulation_communication_state_pub[i].shutdown();
|
||||
// std::cout << " i 2: " << i << std::endl;
|
||||
// }
|
||||
// std::cout << "close 2" << std::endl;
|
||||
// }else
|
||||
// {
|
||||
// this->communication_state_pub_.shutdown();
|
||||
// }
|
||||
|
||||
// this->all_uav_state_pub_.shutdown();
|
||||
// this->swarm_command_pub_.shutdown();
|
||||
}
|
||||
@ -0,0 +1,147 @@
|
||||
#include "uav_basic_topic.hpp"
|
||||
|
||||
UAVBasic::UAVBasic()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
UAVBasic::UAVBasic(ros::NodeHandle &nh,int id,Communication *communication)
|
||||
{
|
||||
nh.param<std::string>("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<prometheus_msgs::UAVCommand>("/uav" + std::to_string(id) + "/prometheus/command", 1);
|
||||
//【发布】mavros接口调用指令(-> uav_control.cpp)
|
||||
this->uav_setup_pub_ = nh.advertise<prometheus_msgs::UAVSetup>("/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_;
|
||||
}
|
||||
@ -0,0 +1,118 @@
|
||||
#include "ugv_basic_topic.hpp"
|
||||
|
||||
UGVBasic::UGVBasic(ros::NodeHandle &nh,Communication *communication)
|
||||
{
|
||||
this->communication_ = communication;
|
||||
nh.param<std::string>("ground_stationt_ip", udp_ip, "127.0.0.1");
|
||||
nh.param<std::string>("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<prometheus_msgs::RheaCommunication>("/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_;
|
||||
}
|
||||
@ -0,0 +1 @@
|
||||
/opt/ros/melodic/share/catkin/cmake/toplevel.cmake
|
||||
@ -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}
|
||||
)
|
||||
@ -0,0 +1,213 @@
|
||||
#ifndef _BSPLINE_OPTIMIZER_H_
|
||||
#define _BSPLINE_OPTIMIZER_H_
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <path_searching/dyn_a_star.h>
|
||||
#include <bspline_opt/uniform_bspline.h>
|
||||
#include <plan_env/grid_map.h>
|
||||
#include <plan_env/obj_predictor.h>
|
||||
#include <ros/ros.h>
|
||||
#include "bspline_opt/lbfgs.hpp"
|
||||
#include <traj_utils/plan_container.hpp>
|
||||
|
||||
// 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<std::vector<Eigen::Vector3d>> base_point; // The point at the statrt of the direction vector (collision point)
|
||||
std::vector<std::vector<Eigen::Vector3d>> direction; // Direction vector, must be normalized.
|
||||
std::vector<bool> flag_temp; // A flag that used in many places. Initialize it everytime before using it.
|
||||
// std::vector<bool> 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<buf.size; k++ )
|
||||
// if ( buf.base_point[k].size() > 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<Eigen::Vector3d> &guide_pt);
|
||||
void setWaypoints(const vector<Eigen::Vector3d> &waypts,
|
||||
const vector<int> &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<Eigen::Vector3d> ref_pts_;
|
||||
|
||||
std::vector<ControlPoints> distinctiveTrajs(vector<std::pair<int, int>> segments);
|
||||
std::vector<std::pair<int, int>> 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<Eigen::Vector3d> guide_pts_; // geometric guiding path points, N-6
|
||||
vector<Eigen::Vector3d> waypoints_; // waypts constraints
|
||||
vector<int> 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<double> &x, std::vector<double> &grad, void *func_data);
|
||||
void combineCost(const std::vector<double> &x, vector<double> &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<BsplineOptimizer> Ptr;
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
} // namespace ego_planner
|
||||
#endif
|
||||
@ -0,0 +1,52 @@
|
||||
#ifndef _GRADIENT_DESCENT_OPT_H_
|
||||
#define _GRADIENT_DESCENT_OPT_H_
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <Eigen/Eigen>
|
||||
|
||||
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
|
||||
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,80 @@
|
||||
#ifndef _UNIFORM_BSPLINE_H_
|
||||
#define _UNIFORM_BSPLINE_H_
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
|
||||
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<Eigen::Vector3d> &point_set,
|
||||
const vector<Eigen::Vector3d> &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
|
||||
@ -0,0 +1,77 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>bspline_opt</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The bspline_opt package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="iszhouxin@zju.edu.cn">iszhouxin</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/bspline_opt</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>plan_env</build_depend>
|
||||
<build_depend>path_searching</build_depend>
|
||||
<build_depend>traj_utils</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<build_export_depend>plan_env</build_export_depend>
|
||||
<build_export_depend>path_searching</build_export_depend>
|
||||
<build_export_depend>traj_utils</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>plan_env</exec_depend>
|
||||
<exec_depend>path_searching</exec_depend>
|
||||
<exec_depend>traj_utils</exec_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,94 @@
|
||||
#include <bspline_opt/gradient_descent_optimizer.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
@ -0,0 +1,377 @@
|
||||
#include "bspline_opt/uniform_bspline.h"
|
||||
#include <ros/ros.h>
|
||||
|
||||
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<Eigen::VectorXd> 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<Eigen::Vector3d> &point_set,
|
||||
const vector<Eigen::Vector3d> &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
|
||||
@ -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()
|
||||
@ -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.
|
||||
@ -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.
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
## 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
|
||||
<node pkg="drone_detect" type="drone_detect" name="drone_$(arg drone_id)_drone_detect" output="screen">
|
||||
<rosparam command="load" file="$(find local_sensing_node)/params/camera.yaml" />
|
||||
<rosparam command="load" file="$(find drone_detect)/config/default.yaml"/>
|
||||
<param name="my_id" value="$(arg drone_id)" />
|
||||
|
||||
<remap from="~odometry" to="/drone_$(arg drone_id)_$(arg odom_topic)"/>
|
||||
<remap from="~depth" to="/drone_$(arg drone_id)_pcl_render_node/depth"/>
|
||||
<remap from="~colordepth" to="/drone_$(arg drone_id)_pcl_render_node/colordepth"/>
|
||||
<remap from="~camera_pose" to="/drone_$(arg drone_id)_pcl_render_node/camera_pose"/>
|
||||
|
||||
<remap from="~drone0" to="/drone_0_$(arg odom_topic)"/>
|
||||
<remap from="~drone1" to="/drone_1_$(arg odom_topic)"/>
|
||||
<remap from="~drone2" to="/drone_2_$(arg odom_topic)"/>
|
||||
</node>
|
||||
```
|
||||
|
||||
|
||||
|
||||
## 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.
|
||||
@ -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
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue