parent
e861d1e9b7
commit
ba53d6fbc2
@ -1,4 +1,5 @@
|
||||
|
||||
#
|
||||
Eazzy in the house
|
||||
geekChen will carry
|
||||
geekChen will carry
|
||||
# 2024.5.5 aircraft src submit
|
||||
@ -1,118 +0,0 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(prometheus_experiment)
|
||||
|
||||
## Compile as C++11, supported in ROS Kinetic and newer
|
||||
# add_compile_options(-std=c++11)
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
message_generation
|
||||
roscpp
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
sensor_msgs
|
||||
mavros
|
||||
std_msgs
|
||||
std_srvs
|
||||
tf2_ros
|
||||
tf2_eigen
|
||||
mavros_msgs
|
||||
prometheus_msgs
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
)
|
||||
|
||||
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
|
||||
###############################
|
||||
## 声明可执行cpp文件 ##
|
||||
###############################
|
||||
|
||||
###### Main File ##########
|
||||
|
||||
##px4_pos_controller.cpp
|
||||
#add_executable(px4_pos_controller src/px4_pos_controller.cpp)
|
||||
#add_dependencies(px4_pos_controller prometheus_control_gencpp)
|
||||
#target_link_libraries(px4_pos_controller ${catkin_LIBRARIES})
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# install(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
install(DIRECTORY launch/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
|
||||
)
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_prometheus_control.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
@ -1,260 +0,0 @@
|
||||
# Common configuration for PX4 autopilot
|
||||
#
|
||||
# node:
|
||||
startup_px4_usb_quirk: true
|
||||
|
||||
# --- system plugins ---
|
||||
|
||||
# sys_status & sys_time connection options
|
||||
conn:
|
||||
heartbeat_rate: 1.0 # send hertbeat rate in Hertz
|
||||
timeout: 10.0 # hertbeat timeout in seconds
|
||||
timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0)
|
||||
system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0)
|
||||
|
||||
# sys_status
|
||||
sys:
|
||||
min_voltage: 10.0 # diagnostics min voltage
|
||||
disable_diag: false # disable all sys_status diagnostics, except heartbeat
|
||||
|
||||
# sys_time
|
||||
time:
|
||||
time_ref_source: "fcu" # time_reference source
|
||||
timesync_mode: MAVLINK
|
||||
timesync_avg_alpha: 0.6 # timesync averaging factor
|
||||
|
||||
# --- mavros plugins (alphabetical order) ---
|
||||
|
||||
# 3dr_radio
|
||||
tdr_radio:
|
||||
low_rssi: 40 # raw rssi lower level for diagnostics
|
||||
|
||||
# actuator_control
|
||||
# None
|
||||
|
||||
# command
|
||||
cmd:
|
||||
use_comp_id_system_control: false # quirk for some old FCUs
|
||||
|
||||
# dummy
|
||||
# None
|
||||
|
||||
# ftp
|
||||
# None
|
||||
|
||||
# global_position
|
||||
global_position:
|
||||
frame_id: "map" # origin frame
|
||||
child_frame_id: "base_link" # body-fixed frame
|
||||
rot_covariance: 99999.0 # covariance for attitude?
|
||||
gps_uere: 1.0 # User Equivalent Range Error (UERE) of GPS sensor (m)
|
||||
use_relative_alt: true # use relative altitude for local coordinates
|
||||
tf:
|
||||
send: false # send TF?
|
||||
frame_id: "map" # TF frame_id
|
||||
global_frame_id: "earth" # TF earth frame_id
|
||||
child_frame_id: "base_link" # TF child_frame_id
|
||||
|
||||
# imu_pub
|
||||
imu:
|
||||
frame_id: "base_link"
|
||||
# need find actual values
|
||||
linear_acceleration_stdev: 0.0003
|
||||
angular_velocity_stdev: 0.0003490659 // 0.02 degrees
|
||||
orientation_stdev: 1.0
|
||||
magnetic_stdev: 0.0
|
||||
|
||||
# local_position
|
||||
local_position:
|
||||
frame_id: "world"
|
||||
tf:
|
||||
send: true
|
||||
frame_id: "world"
|
||||
child_frame_id: "base_link"
|
||||
send_fcu: false
|
||||
|
||||
# param
|
||||
# None, used for FCU params
|
||||
|
||||
# rc_io
|
||||
# None
|
||||
|
||||
# safety_area
|
||||
safety_area:
|
||||
p1: {x: 1.0, y: 1.0, z: 1.0}
|
||||
p2: {x: -1.0, y: -1.0, z: -1.0}
|
||||
|
||||
# setpoint_accel
|
||||
setpoint_accel:
|
||||
send_force: false
|
||||
|
||||
# setpoint_attitude
|
||||
setpoint_attitude:
|
||||
reverse_thrust: false # allow reversed thrust
|
||||
use_quaternion: false # enable PoseStamped topic subscriber
|
||||
tf:
|
||||
listen: false # enable tf listener (disable topic subscribers)
|
||||
frame_id: "map"
|
||||
child_frame_id: "target_attitude"
|
||||
rate_limit: 50.0
|
||||
|
||||
setpoint_raw:
|
||||
thrust_scaling: 1.0 # used in setpoint_raw attitude callback.
|
||||
# Note: PX4 expects normalized thrust values between 0 and 1, which means that
|
||||
# the scaling needs to be unitary and the inputs should be 0..1 as well.
|
||||
|
||||
# setpoint_position
|
||||
setpoint_position:
|
||||
tf:
|
||||
listen: false # enable tf listener (disable topic subscribers)
|
||||
frame_id: "map"
|
||||
child_frame_id: "target_position"
|
||||
rate_limit: 50.0
|
||||
mav_frame: LOCAL_NED
|
||||
|
||||
# setpoint_velocity
|
||||
setpoint_velocity:
|
||||
mav_frame: LOCAL_NED
|
||||
|
||||
# vfr_hud
|
||||
# None
|
||||
|
||||
# waypoint
|
||||
mission:
|
||||
pull_after_gcs: true # update mission if gcs updates
|
||||
|
||||
# --- mavros extras plugins (same order) ---
|
||||
|
||||
# adsb
|
||||
# None
|
||||
|
||||
# debug_value
|
||||
# None
|
||||
|
||||
# distance_sensor
|
||||
## Currently available orientations:
|
||||
# Check http://wiki.ros.org/mavros/Enumerations
|
||||
##
|
||||
distance_sensor:
|
||||
hrlv_ez4_pub:
|
||||
id: 0
|
||||
frame_id: "hrlv_ez4_sonar"
|
||||
orientation: PITCH_270 # RPY:{0.0, 270.0, 0.0} - downward-facing
|
||||
field_of_view: 0.0 # XXX TODO
|
||||
send_tf: true
|
||||
sensor_position: {x: 0.0, y: 0.0, z: -0.1}
|
||||
lidarlite_pub:
|
||||
id: 1
|
||||
frame_id: "lidarlite_laser"
|
||||
orientation: PITCH_270
|
||||
field_of_view: 0.0 # XXX TODO
|
||||
send_tf: true
|
||||
sensor_position: {x: 0.0, y: 0.0, z: -0.1}
|
||||
sonar_1_sub:
|
||||
subscriber: true
|
||||
id: 2
|
||||
orientation: PITCH_270
|
||||
laser_1_sub:
|
||||
subscriber: true
|
||||
id: 3
|
||||
orientation: PITCH_270
|
||||
|
||||
# image_pub
|
||||
image:
|
||||
frame_id: "px4flow"
|
||||
|
||||
# fake_gps
|
||||
fake_gps:
|
||||
# select data source
|
||||
use_mocap: true # ~mocap/pose
|
||||
mocap_transform: true # ~mocap/tf instead of pose
|
||||
use_vision: false # ~vision (pose)
|
||||
# origin (default: Zürich)
|
||||
geo_origin:
|
||||
lat: 47.3667 # latitude [degrees]
|
||||
lon: 8.5500 # longitude [degrees]
|
||||
alt: 408.0 # altitude (height over the WGS-84 ellipsoid) [meters]
|
||||
eph: 2.0
|
||||
epv: 2.0
|
||||
satellites_visible: 5 # virtual number of visible satellites
|
||||
fix_type: 3 # type of GPS fix (default: 3D)
|
||||
tf:
|
||||
listen: false
|
||||
send: false # send TF?
|
||||
frame_id: "map" # TF frame_id
|
||||
child_frame_id: "fix" # TF child_frame_id
|
||||
rate_limit: 10.0 # TF rate
|
||||
gps_rate: 5.0 # GPS data publishing rate
|
||||
|
||||
# landing_target
|
||||
landing_target:
|
||||
listen_lt: false
|
||||
mav_frame: "LOCAL_NED"
|
||||
land_target_type: "VISION_FIDUCIAL"
|
||||
image:
|
||||
width: 640 # [pixels]
|
||||
height: 480
|
||||
camera:
|
||||
fov_x: 2.0071286398 # default: 115 [degrees]
|
||||
fov_y: 2.0071286398
|
||||
tf:
|
||||
send: true
|
||||
listen: false
|
||||
frame_id: "landing_target"
|
||||
child_frame_id: "camera_center"
|
||||
rate_limit: 10.0
|
||||
target_size: {x: 0.3, y: 0.3}
|
||||
|
||||
# mocap_pose_estimate
|
||||
mocap:
|
||||
# select mocap source
|
||||
use_tf: false # ~mocap/tf
|
||||
use_pose: true # ~mocap/pose
|
||||
|
||||
# odom
|
||||
odometry:
|
||||
fcu:
|
||||
odom_parent_id_des: "map" # desired parent frame rotation of the FCU's odometry
|
||||
odom_child_id_des: "base_link" # desired child frame rotation of the FCU's odometry
|
||||
|
||||
# px4flow
|
||||
px4flow:
|
||||
frame_id: "px4flow"
|
||||
ranger_fov: 0.118682 # 6.8 degrees at 5 meters, 31 degrees at 1 meter
|
||||
ranger_min_range: 0.3 # meters
|
||||
ranger_max_range: 5.0 # meters
|
||||
|
||||
# vision_pose_estimate
|
||||
vision_pose:
|
||||
tf:
|
||||
listen: false # enable tf listener (disable topic subscribers)
|
||||
frame_id: "map"
|
||||
child_frame_id: "vision_estimate"
|
||||
rate_limit: 10.0
|
||||
|
||||
# vision_speed_estimate
|
||||
vision_speed:
|
||||
listen_twist: true # enable listen to twist topic, else listen to vec3d topic
|
||||
twist_cov: true # enable listen to twist with covariance topic
|
||||
|
||||
# vibration
|
||||
vibration:
|
||||
frame_id: "base_link"
|
||||
|
||||
# wheel_odometry
|
||||
wheel_odometry:
|
||||
count: 2 # number of wheels to compute odometry
|
||||
use_rpm: false # use wheel's RPM instead of cumulative distance to compute odometry
|
||||
wheel0: {x: 0.0, y: -0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m)
|
||||
wheel1: {x: 0.0, y: 0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m)
|
||||
send_raw: true # send wheel's RPM and cumulative distance (~/wheel_odometry/rpm, ~/wheel_odometry/distance)
|
||||
send_twist: false # send geometry_msgs/TwistWithCovarianceStamped instead of nav_msgs/Odometry
|
||||
frame_id: "map" # origin frame
|
||||
child_frame_id: "base_link" # body-fixed frame
|
||||
vel_error: 0.1 # wheel velocity measurement error 1-std (m/s)
|
||||
tf:
|
||||
send: false
|
||||
frame_id: "map"
|
||||
child_frame_id: "base_link"
|
||||
|
||||
# vim:set ts=2 sw=2 et:
|
||||
@ -1,33 +0,0 @@
|
||||
plugin_blacklist:
|
||||
- actuator_control
|
||||
- adsb
|
||||
- safety_area
|
||||
- 3dr_radio
|
||||
- cam_imu_sync
|
||||
- altitude
|
||||
- distance_sensor
|
||||
- fake_gps
|
||||
- gps_rtk
|
||||
- hil
|
||||
- home_position
|
||||
- landing_target
|
||||
- mocap_pose_estimate
|
||||
- mount_control
|
||||
- obstacle_distance
|
||||
- rc_io
|
||||
- vfr_hud
|
||||
- waypoint
|
||||
- wind_estimation
|
||||
- px4flow
|
||||
- global_position
|
||||
- companion_process_status
|
||||
- debug_value
|
||||
- wheel_odometry
|
||||
- vibration
|
||||
- odom
|
||||
- setpoint_attitude
|
||||
- setpoint_position
|
||||
- setpoint_accel
|
||||
- setpoint_velocity
|
||||
plugin_whitelist: []
|
||||
#- 'sys_*'
|
||||
@ -1,18 +0,0 @@
|
||||
## parameter for px4_sender.cpp
|
||||
|
||||
## 起飞高度,建议设置小一点
|
||||
## 2020.11.04测试发现飞机起飞有较大超调，未发现原因，但起飞后控制正常
|
||||
Takeoff_height : 0.5
|
||||
## 降落速度
|
||||
Land_speed : 0.2
|
||||
## 上锁高度
|
||||
Disarm_height : 0.25
|
||||
|
||||
## 地理围栏，根据实际情况设置
|
||||
geo_fence:
|
||||
x_min: -100
|
||||
x_max: 100
|
||||
y_min: -100
|
||||
y_max: 100
|
||||
z_min: -100.0
|
||||
z_max: 100.0
|
||||
@ -1,21 +0,0 @@
|
||||
Controller_Test:
|
||||
Circle:
|
||||
Center_x: 0.0
|
||||
Center_y: 0.0
|
||||
Center_z: 0.8
|
||||
circle_radius: 1.5 ## m 半径越小越难追踪
|
||||
linear_vel: 0.2 ## m/s 速度越大越难追踪
|
||||
direction: 1.0 ##direction = 1 for CCW 逆时针, direction = -1 for CW 顺时针
|
||||
Eight:
|
||||
Center_x: 0.0
|
||||
Center_y: 0.0
|
||||
Center_z: 1.0
|
||||
radial: 2.0 ## m 半径越小越难追踪
|
||||
omega: 0.3 ## rad/s 角速度越大越难追踪
|
||||
Step:
|
||||
step_length: 2.0 ## m 长度越大越难追踪
|
||||
step_interval: 5.0
|
||||
|
||||
|
||||
|
||||
|
||||
@ -1,397 +0,0 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /全局规划1/Reference_Path1
|
||||
Splitter Ratio: 0.5617647171020508
|
||||
Tree Height: 663
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /3D Nav Goal1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: PointCloud2
|
||||
Preferences:
|
||||
PromptSaveOnExit: true
|
||||
Toolbars:
|
||||
toolButtonStyle: 2
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 20
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Class: rviz/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: false
|
||||
base_link:
|
||||
Value: false
|
||||
base_link_frd:
|
||||
Value: false
|
||||
lidar_link:
|
||||
Value: false
|
||||
map:
|
||||
Value: false
|
||||
map_ned:
|
||||
Value: false
|
||||
odom:
|
||||
Value: false
|
||||
odom_ned:
|
||||
Value: false
|
||||
world:
|
||||
Value: true
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: true
|
||||
Tree:
|
||||
base_link:
|
||||
base_link_frd:
|
||||
{}
|
||||
lidar_link:
|
||||
{}
|
||||
world:
|
||||
map:
|
||||
map_ned:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.10000000149011612
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Name: Drone_Pose
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Shape: Axes
|
||||
Topic: /mavros/local_position/pose
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 30
|
||||
Class: rviz/Path
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Billboards
|
||||
Line Width: 0.029999999329447746
|
||||
Name: Drone_Path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /prometheus/drone_trajectory
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Axes Length: 0.5
|
||||
Axes Radius: 0.15000000596046448
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Name: Reference_Pose
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Shape: Axes
|
||||
Topic: /prometheus/control/ref_pose_rviz
|
||||
Unreliable: false
|
||||
Value: true
|
||||
Enabled: true
|
||||
Name: 无人机状态
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Class: octomap_rviz_plugin/OccupancyGrid
|
||||
Enabled: true
|
||||
Max. Height Display: 3.4028234663852886e+38
|
||||
Max. Octree Depth: 16
|
||||
Min. Height Display: -3.4028234663852886e+38
|
||||
Name: OccupancyGrid
|
||||
Octomap Topic: /octomap_full
|
||||
Queue Size: 5
|
||||
Value: true
|
||||
Voxel Alpha: 1
|
||||
Voxel Coloring: Z-Axis
|
||||
Voxel Rendering: Occupied Voxels
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: FlatColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 0
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: PointCloud2
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.019999999552965164
|
||||
Style: Flat Squares
|
||||
Topic: /octomap_point_cloud_centers
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
Enabled: true
|
||||
Name: Octomap建图
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 2.0094945430755615
|
||||
Min Value: -0.19618161022663116
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 238; 238; 236
|
||||
Color Transformer: FlatColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: Realsense_Global_PCL
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 5
|
||||
Size (m): 0.019999999552965164
|
||||
Style: Flat Squares
|
||||
Topic: /rtabmap/cloud_map
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 252; 233; 79
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Billboards
|
||||
Line Width: 0.07999999821186066
|
||||
Name: Reference_Path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /prometheus/global_planner/path_cmd
|
||||
Unreliable: false
|
||||
Value: true
|
||||
Enabled: true
|
||||
Name: 全局规划
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 245; 121; 0
|
||||
Color Transformer: FlatColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 0
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: PointCloud2
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Flat Squares
|
||||
Topic: /prometheus/sensors/pcl2
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/LaserScan
|
||||
Color: 239; 41; 41
|
||||
Color Transformer: FlatColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 0
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: LaserScan
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Flat Squares
|
||||
Topic: /prometheus/sensors/2Dlidar_scan
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: Intensity
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: PointCloud2
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Flat Squares
|
||||
Topic: /planning/global_inflate_cloud
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Default Light: true
|
||||
Fixed Frame: base_link
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/FocusCamera
|
||||
- Class: rviz_plugins/Goal3DTool
|
||||
Topic: /prometheus/planning/goal
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 18.588502883911133
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: -2.3272345066070557
|
||||
Y: -2.687499761581421
|
||||
Z: 4.139807224273682
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.42520231008529663
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 3.0588293075561523
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 960
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000018200000322fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000322000000c900fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000002500000012f0000005c00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000342fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000342000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005f80000032200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1920
|
||||
X: 0
|
||||
Y: 27
|
||||
@ -1,233 +0,0 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 455
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ""
|
||||
Preferences:
|
||||
PromptSaveOnExit: true
|
||||
Toolbars:
|
||||
toolButtonStyle: 2
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Class: rviz/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
base_link:
|
||||
Value: true
|
||||
base_link_frd:
|
||||
Value: true
|
||||
map:
|
||||
Value: true
|
||||
map_ned:
|
||||
Value: true
|
||||
odom:
|
||||
Value: true
|
||||
odom_ned:
|
||||
Value: true
|
||||
uav3:
|
||||
Value: true
|
||||
world:
|
||||
Value: true
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: true
|
||||
Tree:
|
||||
map:
|
||||
map_ned:
|
||||
{}
|
||||
world:
|
||||
base_link:
|
||||
base_link_frd:
|
||||
{}
|
||||
uav3:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.10000000149011612
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Name: Drone_Pose
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Shape: Axes
|
||||
Topic: /mavros/local_position/pose
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Axes Length: 0.5
|
||||
Axes Radius: 0.15000000596046448
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Name: Ref_Pose
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Shape: Axes
|
||||
Topic: /prometheus/control/ref_pose_rviz
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 30
|
||||
Class: rviz/Path
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Billboards
|
||||
Line Width: 0.029999999329447746
|
||||
Name: Drone_Path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /prometheus/drone_trajectory
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 25; 255; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Billboards
|
||||
Line Width: 0.05000000074505806
|
||||
Name: Ref_Path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /prometheus/reference_trajectory
|
||||
Unreliable: false
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Default Light: true
|
||||
Fixed Frame: world
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/FocusCamera
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Theta std deviation: 0.2617993950843811
|
||||
Topic: /initialpose
|
||||
X std deviation: 0.5
|
||||
Y std deviation: 0.5
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /move_base_simple/goal
|
||||
- Class: rviz/PublishPoint
|
||||
Single click: true
|
||||
Topic: /clicked_point
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 8.334606170654297
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: -1.1448005437850952
|
||||
Y: 1.7814745903015137
|
||||
Z: -0.4501960873603821
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.37539833784103394
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 0.84539794921875
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 752
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000019d00000252fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000252000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000252000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005a00000003efc0100000002fb0000000800540069006d00650100000000000005a0000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002e80000025200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1440
|
||||
X: 445
|
||||
Y: 186
|
||||
@ -1,148 +0,0 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
Splitter Ratio: 0.4355083405971527
|
||||
Tree Height: 465
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: Fisheye1_Image
|
||||
Preferences:
|
||||
PromptSaveOnExit: true
|
||||
Toolbars:
|
||||
toolButtonStyle: 2
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /camera_t265/fisheye1/image_raw
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Fisheye1_Image
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /camera_t265/fisheye2/image_raw
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Fisheye2_Image
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Default Light: true
|
||||
Fixed Frame: camera_t265_link
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/FocusCamera
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Theta std deviation: 0.2617993950843811
|
||||
Topic: /initialpose
|
||||
X std deviation: 0.5
|
||||
Y std deviation: 0.5
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /move_base_simple/goal
|
||||
- Class: rviz/PublishPoint
|
||||
Single click: true
|
||||
Topic: /clicked_point
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 11.76508617401123
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.3153980076313019
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 5.6985650062561035
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Fisheye1_Image:
|
||||
collapsed: false
|
||||
Fisheye2_Image:
|
||||
collapsed: false
|
||||
Height: 1306
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001970000047cfc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000025c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000269000000870000000000000000fb0000000c00430061006d0065007200610100000203000000ed0000000000000000fb0000000c00430061006d00650072006100000001a7000000870000000000000000fb0000001c00460069007300680065007900650031005f0049006d006100670065010000029f000001120000001600fffffffb0000001c00460069007300680065007900650032005f0049006d00610067006501000003b7000001020000001600ffffff000000010000010f0000047cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000047c000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000003efc0100000002fb0000000800540069006d0065010000000000000500000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000024e0000047c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1280
|
||||
X: 1280
|
||||
Y: 27
|
||||
@ -1,405 +0,0 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 546
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ""
|
||||
Preferences:
|
||||
PromptSaveOnExit: true
|
||||
Toolbars:
|
||||
toolButtonStyle: 2
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.20000000298023224
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Name: Pose
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Shape: Axes
|
||||
Topic: /uav1/mavros/local_position/pose
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 2
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 25; 255; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Billboards
|
||||
Line Width: 0.05000000074505806
|
||||
Name: Path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /uav1/prometheus/drone_trajectory
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Axes Length: 0.699999988079071
|
||||
Axes Radius: 0.25
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: false
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Name: Ref_Pose
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Shape: Axes
|
||||
Topic: /uav1/prometheus/control/ref_pose_rviz
|
||||
Unreliable: false
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: UAV1
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.20000000298023224
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Name: Pose
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Shape: Axes
|
||||
Topic: /uav2/mavros/local_position/pose
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 2
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 0; 0; 255
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Billboards
|
||||
Line Width: 0.05000000074505806
|
||||
Name: Path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /uav2/prometheus/drone_trajectory
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Axes Length: 0.699999988079071
|
||||
Axes Radius: 0.25
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: false
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Name: Ref_Pose
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Shape: Axes
|
||||
Topic: /uav2/prometheus/control/ref_pose_rviz
|
||||
Unreliable: false
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: UAV2
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.20000000298023224
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Name: Pose
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Shape: Axes
|
||||
Topic: /uav3/mavros/local_position/pose
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 2
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 0; 255; 255
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Billboards
|
||||
Line Width: 0.05000000074505806
|
||||
Name: Path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /uav3/prometheus/drone_trajectory
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Axes Length: 0.699999988079071
|
||||
Axes Radius: 0.25
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: false
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Name: Ref_Pose
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Shape: Axes
|
||||
Topic: /uav3/prometheus/control/ref_pose_rviz
|
||||
Unreliable: false
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: UAV3
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.20000000298023224
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Name: Pose
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Shape: Axes
|
||||
Topic: /uav4/mavros/local_position/pose
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 2
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 255; 255; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Billboards
|
||||
Line Width: 0.05000000074505806
|
||||
Name: Path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /uav4/prometheus/drone_trajectory
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Axes Length: 0.699999988079071
|
||||
Axes Radius: 0.25
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: false
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Name: Ref_Pose
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Shape: Axes
|
||||
Topic: /uav4/prometheus/control/ref_pose_rviz
|
||||
Unreliable: false
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: UAV4
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.20000000298023224
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Name: Pose
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Shape: Axes
|
||||
Topic: /uav5/mavros/local_position/pose
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 2
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 0; 85; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Billboards
|
||||
Line Width: 0.05000000074505806
|
||||
Name: Path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /uav5/prometheus/drone_trajectory
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Axes Length: 0.699999988079071
|
||||
Axes Radius: 0.25
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: false
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Name: Ref_Pose
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Shape: Axes
|
||||
Topic: /uav5/prometheus/control/ref_pose_rviz
|
||||
Unreliable: false
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: UAV5
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Default Light: true
|
||||
Fixed Frame: world
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/FocusCamera
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Theta std deviation: 0.2617993950843811
|
||||
Topic: /initialpose
|
||||
X std deviation: 0.5
|
||||
Y std deviation: 0.5
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /move_base_simple/goal
|
||||
- Class: rviz/PublishPoint
|
||||
Single click: true
|
||||
Topic: /clicked_point
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 34.19184112548828
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 1.4970207214355469
|
||||
Y: 0.133266419172287
|
||||
Z: -1.1527864933013916
|
||||
Focal Shape Fixed Size: false
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 1.5697963237762451
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 3.1504018306732178
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 846
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000156000002aefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000002ae000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002aefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003e000002ae000000a600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000026200fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002ae00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1200
|
||||
X: 60
|
||||
Y: 27
|
||||
@ -1,23 +0,0 @@
|
||||
<!-- 本launch为使用px4_sender进行机载控制时的地面端启动脚本 -->
|
||||
<!-- 硬件清单 -->
|
||||
<launch>
|
||||
<!-- run the ground_station.cpp -->
|
||||
<node pkg="prometheus_station" type="ground_station" name="ground_station" output="screen" launch-prefix="gnome-terminal --tab --">
|
||||
</node>
|
||||
|
||||
<!-- run the ground_station_msg.cpp -->
|
||||
<node pkg="prometheus_station" type="ground_station_msg" name="ground_station_msg" output="screen" launch-prefix="gnome-terminal --tab --">
|
||||
</node>
|
||||
|
||||
<!-- run the terminal_control.cpp -->
|
||||
<node pkg="prometheus_control" type="terminal_control" name="terminal_control" output="screen" launch-prefix="gnome-terminal --">
|
||||
<rosparam command="load" file="$(find prometheus_experiment)/config/prometheus_control_config/terminal_control.yaml" />
|
||||
</node>
|
||||
|
||||
<!-- run the rviz -->
|
||||
<arg name="visualization" default="true"/>
|
||||
<group if="$(arg visualization)">
|
||||
<node type="rviz" name="rviz" pkg="rviz" args="-d $(find prometheus_experiment)/config/rviz_config/rviz_controller_test.rviz" />
|
||||
</group>
|
||||
</launch>
|
||||
|
||||
@ -1,257 +0,0 @@
|
||||
# Common configuration for APM2 autopilot
|
||||
#
|
||||
# node:
|
||||
startup_px4_usb_quirk: false
|
||||
|
||||
# --- system plugins ---
|
||||
|
||||
# sys_status & sys_time connection options
|
||||
conn:
|
||||
heartbeat_rate: 1.0 # send hertbeat rate in Hertz
|
||||
heartbeat_mav_type: "ONBOARD_CONTROLLER"
|
||||
timeout: 10.0 # hertbeat timeout in seconds
|
||||
timesync_rate: 0.0 # TIMESYNC rate in Hertz (feature disabled if 0.0)
|
||||
system_time_rate: 0.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: "map"
|
||||
tf:
|
||||
send: false
|
||||
frame_id: "map"
|
||||
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
|
||||
setpoint_raw:
|
||||
thrust_scaling: 1.0 # specify thrust scaling (normalized, 0 to 1) for thrust (like PX4)
|
||||
|
||||
# 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
|
||||
use_mission_item_int: true # use the MISSION_ITEM_INT message instead of MISSION_ITEM
|
||||
# for uploading waypoints to FCU
|
||||
|
||||
# --- mavros extras plugins (same order) ---
|
||||
|
||||
# adsb
|
||||
# None
|
||||
|
||||
# debug_value
|
||||
# None
|
||||
|
||||
# distance_sensor
|
||||
## Currently available orientations:
|
||||
# Check http://wiki.ros.org/mavros/Enumerations
|
||||
##
|
||||
distance_sensor:
|
||||
rangefinder_pub:
|
||||
id: 0
|
||||
frame_id: "lidar"
|
||||
#orientation: PITCH_270 # sended by FCU
|
||||
field_of_view: 0.0 # XXX TODO
|
||||
send_tf: false
|
||||
sensor_position: {x: 0.0, y: 0.0, z: -0.1}
|
||||
rangefinder_sub:
|
||||
subscriber: true
|
||||
id: 1
|
||||
orientation: PITCH_270 # only that orientation are supported by APM 3.4+
|
||||
|
||||
# image_pub
|
||||
image:
|
||||
frame_id: "px4flow"
|
||||
|
||||
# fake_gps
|
||||
fake_gps:
|
||||
# select data source
|
||||
use_mocap: true # ~mocap/pose
|
||||
mocap_transform: false # ~mocap/tf instead of pose
|
||||
mocap_withcovariance: false # ~mocap/pose uses PoseWithCovarianceStamped Message
|
||||
use_vision: false # ~vision (pose)
|
||||
use_hil_gps: true
|
||||
gps_id: 4
|
||||
# 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
|
||||
horiz_accuracy: 0.5
|
||||
vert_accuracy: 0.5
|
||||
speed_accuracy: 0.0
|
||||
satellites_visible: 6 # 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:
|
||||
frame_tf:
|
||||
desired_frame: "ned"
|
||||
estimator_type: 3 # check enum MAV_ESTIMATOR_TYPE in <https://mavlink.io/en/messages/common.html>
|
||||
|
||||
# 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: true
|
||||
frame_id: "map"
|
||||
child_frame_id: "base_link"
|
||||
|
||||
# vim:set ts=2 sw=2 et:
|
||||
@ -1,18 +0,0 @@
|
||||
plugin_blacklist:
|
||||
# common
|
||||
- actuator_control
|
||||
- ftp
|
||||
- safety_area
|
||||
- hil
|
||||
# extras
|
||||
- altitude
|
||||
- debug_value
|
||||
- image_pub
|
||||
- px4flow
|
||||
- vibration
|
||||
- vision_speed_estimate
|
||||
- wheel_odometry
|
||||
|
||||
plugin_whitelist: []
|
||||
#- 'sys_*'
|
||||
|
||||
@ -1,9 +0,0 @@
|
||||
<launch>
|
||||
<arg name="ugv_id" default="1"/>
|
||||
|
||||
<!-- fake_vicon -->
|
||||
<node pkg="prometheus_ugv_control" type="fake_vicon" name="fake_vicon" output="screen">
|
||||
<param name="rigid_body_name" value="/ugv$(arg ugv_id)"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@ -1,12 +0,0 @@
|
||||
<launch>
|
||||
<arg name="swarm_num" default="1"/>
|
||||
<arg name="ugv_id" default="1"/>
|
||||
<arg name="sim_mode" default="false"/>
|
||||
|
||||
<!-- 启动地面站 -->
|
||||
<node pkg="prometheus_ugv_control" type="ugv_ground_station" name="ugv_ground_station" output="screen">
|
||||
<param name="swarm_num" value="$(arg swarm_num)"/>
|
||||
<param name="ugv1_id" value="$(arg ugv_id)" />
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
@ -1,50 +0,0 @@
|
||||
<launch>
|
||||
<arg name="swarm_num" default="1"/>
|
||||
<arg name="ugv_id" default="1"/>
|
||||
|
||||
<arg name="x_min" default="-3.0"/>
|
||||
<arg name="x_max" default="3.0"/>
|
||||
<arg name="y_min" default="-3.0"/>
|
||||
<arg name="y_max" default="3.0"/>
|
||||
<arg name="sim_mode" default="false"/>
|
||||
|
||||
<!-- n号小车 -->
|
||||
<group ns="/ugv$(arg ugv_id)">
|
||||
<!-- 启动MAVROS -->
|
||||
<node pkg="mavros" type="mavros_node" name="mavros" output="screen">
|
||||
<param name="fcu_url" value="/dev/ttyUSB0:921600" />
|
||||
<param name="gcs_url" value="" />
|
||||
<param name="target_system_id" value="1" />
|
||||
<param name="target_component_id" value="1" />
|
||||
<rosparam command="load" file="$(find prometheus_experiment)/launch_ugv/apm_pluginlists.yaml" />
|
||||
<rosparam command="load" file="$(find prometheus_experiment)/launch_ugv/apm_config.yaml" />
|
||||
</node>
|
||||
</group>
|
||||
|
||||
<!-- vrpn -->
|
||||
<!-- 注意修改vicon电脑ip地址 -->
|
||||
<!-- 改为T265 -->
|
||||
<include file="$(find vrpn_client_ros)/launch/sample.launch">
|
||||
<arg name="server" value="192.168.1.2"/>
|
||||
</include>
|
||||
|
||||
<!-- 启动ugv_controller -->
|
||||
<node pkg="prometheus_ugv_control" type="ugv_controller" name="ugv_controller_$(arg ugv_id)" output="screen">
|
||||
<param name="ugv_id" value="$(arg ugv_id)"/>
|
||||
<param name="sim_mode" value="$(arg sim_mode)" />
|
||||
<param name="flag_printf" value="false" />
|
||||
<param name="k_p" value="0.3" />
|
||||
<param name="k_yaw" value="1.0" />
|
||||
<param name="geo_fence/x_min" value="$(arg x_min)"/>
|
||||
<param name="geo_fence/x_max" value="$(arg x_max)"/>
|
||||
<param name="geo_fence/y_min" value="$(arg y_min)"/>
|
||||
<param name="geo_fence/y_max" value="$(arg y_max)"/>
|
||||
</node>
|
||||
|
||||
<!-- 启动ugv_estimator -->
|
||||
<node pkg="prometheus_ugv_control" type="ugv_estimator" name="ugv_estimator_$(arg ugv_id)" output="screen">
|
||||
<param name="ugv_id" value="$(arg ugv_id)"/>
|
||||
<param name="sim_mode" value="$(arg sim_mode)" />
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@ -1,12 +0,0 @@
|
||||
<launch>
|
||||
<arg name="swarm_num" default="1"/>
|
||||
<arg name="ugv_id" default="1"/>
|
||||
<arg name="sim_mode" default="false"/>
|
||||
|
||||
<!-- 启动控制终端 -->
|
||||
<node pkg="prometheus_ugv_control" type="ugv_terminal_control" name="ugv_terminal_control" output="screen">
|
||||
<param name="ugv_name" value="/ugv1"/>
|
||||
<param name="sim_mode" value="$(arg sim_mode)" />
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
@ -1,28 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>prometheus_experiment</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The prometheus_experiment package</description>
|
||||
|
||||
<maintainer email="fatmoonqyp@126.com">Yuhua Qi</maintainer>
|
||||
<author email="fatmoonqyp@126.com">Yuhua Qi</author>
|
||||
<license>TODO</license>
|
||||
|
||||
<url type="website">http://www.amovauto.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>
|
||||
|
||||
</export>
|
||||
</package>
|
||||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
@ -0,0 +1,11 @@
|
||||
#ifndef CONTROL_COMMON_H
|
||||
#define CONTROL_COMMON_H
|
||||
|
||||
#include <string>
|
||||
using namespace std;
|
||||
|
||||
#define PX4_SENDER 0
|
||||
#define PX4_POS_CONTROLLER 1
|
||||
#define PX4_GEO_CONTROLLER 2
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,208 @@
|
||||
#ifndef CONTROL_UTILS_H
|
||||
#define CONTROL_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;
|
||||
}
|
||||
|
||||
Eigen::Vector4d rot2Quaternion(const Eigen::Matrix3d &R)
|
||||
{
|
||||
Eigen::Vector4d quat;
|
||||
double tr = R.trace();
|
||||
if (tr > 0.0) {
|
||||
double S = sqrt(tr + 1.0) * 2.0; // S=4*qw
|
||||
quat(0) = 0.25 * S;
|
||||
quat(1) = (R(2, 1) - R(1, 2)) / S;
|
||||
quat(2) = (R(0, 2) - R(2, 0)) / S;
|
||||
quat(3) = (R(1, 0) - R(0, 1)) / S;
|
||||
} else if ((R(0, 0) > R(1, 1)) & (R(0, 0) > R(2, 2))) {
|
||||
double S = sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2)) * 2.0; // S=4*qx
|
||||
quat(0) = (R(2, 1) - R(1, 2)) / S;
|
||||
quat(1) = 0.25 * S;
|
||||
quat(2) = (R(0, 1) + R(1, 0)) / S;
|
||||
quat(3) = (R(0, 2) + R(2, 0)) / S;
|
||||
} else if (R(1, 1) > R(2, 2)) {
|
||||
double S = sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2)) * 2.0; // S=4*qy
|
||||
quat(0) = (R(0, 2) - R(2, 0)) / S;
|
||||
quat(1) = (R(0, 1) + R(1, 0)) / S;
|
||||
quat(2) = 0.25 * S;
|
||||
quat(3) = (R(1, 2) + R(2, 1)) / S;
|
||||
} else {
|
||||
double S = sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1)) * 2.0; // S=4*qz
|
||||
quat(0) = (R(1, 0) - R(0, 1)) / S;
|
||||
quat(1) = (R(0, 2) + R(2, 0)) / S;
|
||||
quat(2) = (R(1, 2) + R(2, 1)) / S;
|
||||
quat(3) = 0.25 * S;
|
||||
}
|
||||
return quat;
|
||||
}
|
||||
|
||||
static Eigen::Vector3d matrix_hat_inv(const Eigen::Matrix3d &m)
|
||||
{
|
||||
Eigen::Vector3d v;
|
||||
// TODO: Sanity checks if m is skew symmetric
|
||||
v << m(7), m(2), m(3);
|
||||
return v;
|
||||
}
|
||||
|
||||
Eigen::Vector4d quatMultiplication(const Eigen::Vector4d &q, const Eigen::Vector4d &p)
|
||||
{
|
||||
Eigen::Vector4d quat;
|
||||
quat << p(0) * q(0) - p(1) * q(1) - p(2) * q(2) - p(3) * q(3), p(0) * q(1) + p(1) * q(0) - p(2) * q(3) + p(3) * q(2),
|
||||
p(0) * q(2) + p(1) * q(3) + p(2) * q(0) - p(3) * q(1), p(0) * q(3) - p(1) * q(2) + p(2) * q(1) + p(3) * q(0);
|
||||
return quat;
|
||||
}
|
||||
|
||||
Eigen::Matrix3d quat2RotMatrix(const Eigen::Vector4d &q)
|
||||
{
|
||||
Eigen::Matrix3d rotmat;
|
||||
rotmat << q(0) * q(0) + q(1) * q(1) - q(2) * q(2) - q(3) * q(3), 2 * q(1) * q(2) - 2 * q(0) * q(3),
|
||||
2 * q(0) * q(2) + 2 * q(1) * q(3),
|
||||
|
||||
2 * q(0) * q(3) + 2 * q(1) * q(2), q(0) * q(0) - q(1) * q(1) + q(2) * q(2) - q(3) * q(3),
|
||||
2 * q(2) * q(3) - 2 * q(0) * q(1),
|
||||
|
||||
2 * q(1) * q(3) - 2 * q(0) * q(2), 2 * q(0) * q(1) + 2 * q(2) * q(3),
|
||||
q(0) * q(0) - q(1) * q(1) - q(2) * q(2) + q(3) * q(3);
|
||||
return rotmat;
|
||||
}
|
||||
|
||||
Eigen::Vector4d acc2quaternion(const Eigen::Vector3d &vector_acc, const double &yaw)
|
||||
{
|
||||
Eigen::Vector4d quat;
|
||||
Eigen::Vector3d zb_des, yb_des, xb_des, proj_xb_des;
|
||||
Eigen::Matrix3d rotmat;
|
||||
|
||||
proj_xb_des << std::cos(yaw), std::sin(yaw), 0.0;
|
||||
|
||||
zb_des = vector_acc / vector_acc.norm();
|
||||
yb_des = zb_des.cross(proj_xb_des) / (zb_des.cross(proj_xb_des)).norm();
|
||||
xb_des = yb_des.cross(zb_des) / (yb_des.cross(zb_des)).norm();
|
||||
|
||||
rotmat << xb_des(0), yb_des(0), zb_des(0), xb_des(1), yb_des(1), zb_des(1), xb_des(2), yb_des(2), zb_des(2);
|
||||
quat = rot2Quaternion(rotmat);
|
||||
return quat;
|
||||
}
|
||||
|
||||
//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,44 @@
|
||||
/***************************************************************************************************************************
|
||||
* message_utils.h
|
||||
*
|
||||
* Author: Jario
|
||||
*
|
||||
* Update Time: 2020.4.29
|
||||
***************************************************************************************************************************/
|
||||
#ifndef PROMETHEUS_MESSAGE_UTILS_H
|
||||
#define PROMETHEUS_MESSAGE_UTILS_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <prometheus_msgs/Message.h>
|
||||
using namespace std;
|
||||
|
||||
// 消息发布函数
|
||||
inline void pub_message(ros::Publisher& puber, int msg_type, std::string source_node, std::string msg_content)
|
||||
{
|
||||
prometheus_msgs::Message exect_msg;
|
||||
exect_msg.header.stamp = ros::Time::now();
|
||||
exect_msg.message_type = msg_type;
|
||||
exect_msg.source_node = source_node;
|
||||
exect_msg.content = msg_content;
|
||||
puber.publish(exect_msg);
|
||||
}
|
||||
|
||||
// 消息打印函数
|
||||
inline void printf_message(const prometheus_msgs::Message& message)
|
||||
{
|
||||
//cout <<">>>>>>>>>>>>>>>>>>>>>>>> Message <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
|
||||
if(message.message_type == prometheus_msgs::Message::NORMAL)
|
||||
{
|
||||
cout << "[NORMAL]" << "["<< message.source_node << "]:" << message.content <<endl;
|
||||
}else if(message.message_type == prometheus_msgs::Message::WARN)
|
||||
{
|
||||
cout << "[WARN]" << "["<< message.source_node << "]:" <<message.content <<endl;
|
||||
}else if(message.message_type == prometheus_msgs::Message::ERROR)
|
||||
{
|
||||
cout << "[ERROR]" << "["<< message.source_node << "]:" << message.content <<endl;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,88 @@
|
||||
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
|
||||
serial
|
||||
)
|
||||
|
||||
find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
add_message_files(
|
||||
DIRECTORY msg
|
||||
FILES
|
||||
DetectionInfo.msg
|
||||
MultiDetectionInfo.msg
|
||||
BoundingBox.msg
|
||||
BoundingBoxes.msg
|
||||
ControlCommand.msg
|
||||
PositionReference.msg
|
||||
AttitudeReference.msg
|
||||
DroneState.msg
|
||||
ControlOutput.msg
|
||||
Message.msg
|
||||
LogMessage.msg
|
||||
LogMessageControl.msg
|
||||
LogMessagePlanning.msg
|
||||
LogMessageDetection.msg
|
||||
SwarmCommand.msg
|
||||
Formation.msg
|
||||
ArucoInfo.msg
|
||||
MultiArucoInfo.msg
|
||||
IndoorSearch.msg
|
||||
UgvState.msg
|
||||
UgvCommand.msg
|
||||
GimbalTrackError.msg
|
||||
Cloud_platform.msg
|
||||
gimbal.msg
|
||||
GimbalCtl.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,7 @@
|
||||
std_msgs/Header header
|
||||
|
||||
## 位置控制器输出： 期望油门 + 期望角度
|
||||
float32[3] throttle_sp ## [0-1] 惯性系下的油门量
|
||||
float32 desired_throttle ## [0-1] 机体系z轴
|
||||
float32[3] desired_attitude ## [rad]
|
||||
geometry_msgs/Quaternion desired_att_q ## 四元数
|
||||
@ -0,0 +1,7 @@
|
||||
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,4 @@
|
||||
std_msgs/Header header
|
||||
uint8[59] received_data
|
||||
uint8[5] get_data_com
|
||||
uint8[20] gimbal_control # 控制
|
||||
@ -0,0 +1,23 @@
|
||||
std_msgs/Header header
|
||||
|
||||
## 控制命令的编号 防止接收到错误命令， 编号应该逐次递加
|
||||
uint32 Command_ID
|
||||
|
||||
## 消息来源
|
||||
string source
|
||||
## 控制命令的模式
|
||||
uint8 Mode
|
||||
# enum Mode 控制模式枚举
|
||||
uint8 Idle=0
|
||||
uint8 Takeoff=1
|
||||
uint8 Hold=2
|
||||
uint8 Land=3
|
||||
uint8 Move=4
|
||||
uint8 Disarm=5
|
||||
uint8 User_Mode1=6
|
||||
uint8 User_Mode2=7
|
||||
|
||||
## 控制参考量
|
||||
## 位置参考量：位置、速度、加速度、加加速度、加加加速度
|
||||
## 角度参考量：偏航角、偏航角速度、偏航角加速度
|
||||
PositionReference Reference_State
|
||||
@ -0,0 +1,11 @@
|
||||
std_msgs/Header header
|
||||
|
||||
## 位置控制器输出： 期望推力及期望油门
|
||||
float32[3] Thrust ## [rad]
|
||||
float32[3] Throttle
|
||||
|
||||
## NE控制器的中间变量
|
||||
float32[3] u_l ## [0-1] 惯性系下的油门量
|
||||
float32[3] u_d ## [rad]
|
||||
float32[3] NE ## [rad]
|
||||
|
||||
@ -0,0 +1,6 @@
|
||||
uint8 HORIZONTAL=0
|
||||
uint8 TRIANGEL=1
|
||||
uint8 DIAMOND=2
|
||||
uint8 DIAMOND_STAGE_1=3
|
||||
|
||||
uint8 type
|
||||
@ -0,0 +1,9 @@
|
||||
float32 pitch
|
||||
float32 yaw
|
||||
float32 zoom
|
||||
float32 focus
|
||||
uint16 home
|
||||
uint16 TakePicture
|
||||
uint16 cameraModeChange
|
||||
uint16 yawfollow
|
||||
uint16 quadhome
|
||||
@ -0,0 +1,12 @@
|
||||
## 是否检测到目标
|
||||
bool detected
|
||||
|
||||
# 坐标
|
||||
float64 x
|
||||
float64 y
|
||||
# 速度
|
||||
float64 velx
|
||||
float64 vely
|
||||
# 积分
|
||||
float64 Ix
|
||||
float64 Iy
|
||||
@ -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,12 @@
|
||||
std_msgs/Header header
|
||||
|
||||
## 节点回传地面站的消息
|
||||
uint8 message_type
|
||||
# enum message_type
|
||||
uint8 NORMAL = 0
|
||||
uint8 WARN = 1
|
||||
uint8 ERROR = 2
|
||||
|
||||
string source_node
|
||||
|
||||
string content
|
||||
@ -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,24 @@
|
||||
std_msgs/Header header
|
||||
|
||||
## 控制命令的编号 防止接收到错误命令， 编号应该逐次递加
|
||||
uint32 Command_ID
|
||||
|
||||
## 控制命令的模式
|
||||
uint8 Mode
|
||||
# enum Mode 控制模式枚举
|
||||
uint8 Start=0
|
||||
uint8 Hold=1
|
||||
uint8 Disarm=2
|
||||
uint8 Point_Control=3
|
||||
uint8 Direct_Control=4
|
||||
uint8 ENU_Vel_Control=5
|
||||
uint8 ENU_Pos_Control=6
|
||||
uint8 Path_Control=7
|
||||
|
||||
float32[2] position_ref ## [m]
|
||||
float32 yaw_ref ## [rad]
|
||||
|
||||
float32[2] linear_vel ## [m/s]
|
||||
float32 angular_vel ## [rad/s]
|
||||
|
||||
|
||||
@ -0,0 +1,15 @@
|
||||
float64 imu0
|
||||
float64 imu1
|
||||
float64 imu2
|
||||
float64 rc0
|
||||
float64 rc1
|
||||
float64 rc2
|
||||
float64 rel0 # r
|
||||
float64 rel1 # p
|
||||
float64 rel2 # y
|
||||
float64 imuvel0
|
||||
float64 imuvel1
|
||||
float64 imuvel2
|
||||
float64 relvel0
|
||||
float64 relvel1
|
||||
float64 relvel2
|
||||
@ -0,0 +1,208 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(prometheus_control)
|
||||
|
||||
## 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
|
||||
tf
|
||||
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
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
CATKIN_DEPENDS message_runtime
|
||||
)
|
||||
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
../common/include
|
||||
)
|
||||
|
||||
|
||||
###############################
|
||||
## 声明可执行cpp文件 ##
|
||||
###############################
|
||||
|
||||
add_library(OptiTrackFeedbackRigidBody src/lib/OptiTrackFeedBackRigidBody.cpp)
|
||||
add_library(KeyboardEvent src/lib/KeyboardEvent.cpp)
|
||||
|
||||
###### Main File ##########
|
||||
##rover_pos_controller.cpp
|
||||
add_executable(rover_pos_controller src/rover_control/rover_pos_controller.cpp)
|
||||
add_dependencies(rover_pos_controller prometheus_control_gencpp)
|
||||
target_link_libraries(rover_pos_controller ${catkin_LIBRARIES})
|
||||
|
||||
##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})
|
||||
|
||||
##px4_geometry_controller.cpp
|
||||
add_executable(px4_geometry_controller src/px4_geometry_controller.cpp)
|
||||
add_dependencies(px4_geometry_controller prometheus_control_gencpp)
|
||||
target_link_libraries(px4_geometry_controller ${catkin_LIBRARIES})
|
||||
|
||||
##px4_pos_estimator.cpp
|
||||
add_executable(px4_pos_estimator src/px4_pos_estimator.cpp)
|
||||
add_dependencies(px4_pos_estimator prometheus_control_gencpp)
|
||||
target_link_libraries(px4_pos_estimator ${catkin_LIBRARIES})
|
||||
target_link_libraries(px4_pos_estimator OptiTrackFeedbackRigidBody)
|
||||
|
||||
##ros_info_color_test.cpp
|
||||
add_executable(ros_info_color_test src/Utilities/ros_info_color_test.cpp)
|
||||
add_dependencies(ros_info_color_test prometheus_control_gencpp)
|
||||
target_link_libraries(ros_info_color_test ${catkin_LIBRARIES})
|
||||
|
||||
# add_executable(planner_server src/Test/planner_server.cpp)
|
||||
# add_dependencies(planner_server prometheus_control_gencpp)
|
||||
# target_link_libraries(planner_server ${catkin_LIBRARIES})
|
||||
|
||||
|
||||
###### Test File ##########
|
||||
#add_executable(px4_fw_controller src/Test/px4_fw_controller.cpp)
|
||||
#add_dependencies(px4_fw_controller prometheus_control_gencpp)
|
||||
#target_link_libraries(px4_fw_controller ${catkin_LIBRARIES})
|
||||
|
||||
add_executable(px4_sender src/px4_sender.cpp)
|
||||
add_dependencies(px4_sender prometheus_control_gencpp)
|
||||
target_link_libraries(px4_sender ${catkin_LIBRARIES})
|
||||
|
||||
|
||||
###### Utilities File ##########
|
||||
#add_executable(keyboard_control src/Utilities/keyboard_control.cpp)
|
||||
#add_dependencies(keyboard_control prometheus_control_gencpp)
|
||||
#target_link_libraries(keyboard_control ${catkin_LIBRARIES})
|
||||
#target_link_libraries(keyboard_control KeyboardEvent)
|
||||
|
||||
##px4_pos_estimator_with_noise.cpp
|
||||
add_executable(px4_pos_estimator_with_noise src//Utilities/px4_pos_estimator_with_noise.cpp)
|
||||
add_dependencies(px4_pos_estimator_with_noise prometheus_control_gencpp)
|
||||
target_link_libraries(px4_pos_estimator_with_noise ${catkin_LIBRARIES})
|
||||
target_link_libraries(px4_pos_estimator_with_noise OptiTrackFeedbackRigidBody)
|
||||
|
||||
add_executable(filter_tester src/Utilities/filter_tester.cpp)
|
||||
add_dependencies(filter_tester prometheus_control_gencpp)
|
||||
target_link_libraries(filter_tester ${catkin_LIBRARIES})
|
||||
|
||||
add_executable(TFmini src/Utilities/TFmini.cpp)
|
||||
add_dependencies(TFmini prometheus_control_gencpp)
|
||||
target_link_libraries(TFmini ${catkin_LIBRARIES})
|
||||
|
||||
add_executable(fake_vicon src/Utilities/fake_vicon.cpp)
|
||||
add_dependencies(fake_vicon prometheus_control_gencpp)
|
||||
target_link_libraries(fake_vicon ${catkin_LIBRARIES})
|
||||
|
||||
add_executable(terminal_control src/Utilities/terminal_control.cpp)
|
||||
add_dependencies(terminal_control prometheus_control_gencpp)
|
||||
target_link_libraries(terminal_control ${catkin_LIBRARIES})
|
||||
target_link_libraries(terminal_control KeyboardEvent)
|
||||
|
||||
add_executable(terminal_control_rover src/Utilities/terminal_control_rover.cpp)
|
||||
add_dependencies(terminal_control_rover prometheus_control_gencpp)
|
||||
target_link_libraries(terminal_control_rover ${catkin_LIBRARIES})
|
||||
|
||||
add_executable(OptiTrackTest src/Utilities/OptiTrackTest.cpp)
|
||||
add_dependencies(OptiTrackTest prometheus_control_gencpp)
|
||||
target_link_libraries(OptiTrackTest ${catkin_LIBRARIES})
|
||||
target_link_libraries(OptiTrackTest OptiTrackFeedbackRigidBody)
|
||||
target_link_libraries(OptiTrackTest KeyboardEvent)
|
||||
|
||||
add_executable(set_mode src/Utilities/set_mode.cpp)
|
||||
add_dependencies(set_mode prometheus_control_gencpp)
|
||||
target_link_libraries(set_mode ${catkin_LIBRARIES})
|
||||
|
||||
add_executable(eigen_test src/Utilities/eigen_test.cpp)
|
||||
add_dependencies(eigen_test prometheus_control_gencpp)
|
||||
target_link_libraries(eigen_test ${catkin_LIBRARIES})
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# 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,57 @@
|
||||
#ifndef HIGHPASSFILTER_H
|
||||
#define HIGHPASSFILTER_H
|
||||
|
||||
#include <math.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
||||
class HighPassFilter
|
||||
{
|
||||
public:
|
||||
|
||||
//构造函数
|
||||
HighPassFilter(void)
|
||||
{
|
||||
T = 0.0f;
|
||||
Output_prev = 0.0f;
|
||||
Input_prev = 0.0f;
|
||||
}
|
||||
|
||||
void set_Time_constant(float Time_constant);
|
||||
|
||||
float apply(float input, float dt);
|
||||
|
||||
// Return the cutoff frequency
|
||||
float get_Time_constant() const { return T; }
|
||||
|
||||
private:
|
||||
|
||||
//Previous output
|
||||
float Output_prev;
|
||||
|
||||
float Input_prev;
|
||||
|
||||
//Time constant
|
||||
float T;
|
||||
};
|
||||
|
||||
float HighPassFilter::apply(float input, float dt)
|
||||
{
|
||||
// do the filtering
|
||||
float output = 1/(T+dt)*(T*Output_prev + input - Input_prev);
|
||||
|
||||
Output_prev = output;
|
||||
|
||||
Input_prev = input;
|
||||
|
||||
// return the value.
|
||||
return output;
|
||||
}
|
||||
|
||||
void HighPassFilter::set_Time_constant(float Time_constant)
|
||||
{
|
||||
T = Time_constant;
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,63 @@
|
||||
#ifndef LEADLAGFILTER_H
|
||||
#define LEADLAGFILTER_H
|
||||
|
||||
#include <math.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
||||
class LeadLagFilter
|
||||
{
|
||||
public:
|
||||
|
||||
//构造函数
|
||||
LeadLagFilter()
|
||||
{
|
||||
T = 0.0f;
|
||||
Kd = 0.0f;
|
||||
Output_prev = 0.0f;
|
||||
Input_prev = 0.0f;
|
||||
}
|
||||
|
||||
void set_Time_constant(float Time_constant,float _Kd);
|
||||
|
||||
float apply(float input, float dt);
|
||||
|
||||
// Return the cutoff frequency
|
||||
float get_Time_constant() const { return T; }
|
||||
|
||||
float get_Kd() const { return Kd; }
|
||||
|
||||
private:
|
||||
|
||||
//Previous output
|
||||
float Output_prev;
|
||||
|
||||
float Input_prev;
|
||||
|
||||
float Kd;
|
||||
|
||||
//Time constant
|
||||
float T;
|
||||
};
|
||||
|
||||
float LeadLagFilter::apply(float input, float dt)
|
||||
{
|
||||
// do the filtering
|
||||
float output = 1/(T+dt)*(T*Output_prev + input - Input_prev + dt*Kd*input);
|
||||
|
||||
Output_prev = output;
|
||||
|
||||
Input_prev = input;
|
||||
|
||||
// return the value.
|
||||
return output;
|
||||
}
|
||||
|
||||
void LeadLagFilter::set_Time_constant(float Time_constant,float _Kd)
|
||||
{
|
||||
T = Time_constant;
|
||||
Kd = _Kd;
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,52 @@
|
||||
#ifndef LOWPASSFILTER_H
|
||||
#define LOWPASSFILTER_H
|
||||
|
||||
#include <math.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
||||
class LowPassFilter
|
||||
{
|
||||
public:
|
||||
//构造函数
|
||||
LowPassFilter(void)
|
||||
{
|
||||
T = 0.0f;
|
||||
Output_prev = 0.0f;
|
||||
}
|
||||
|
||||
void set_Time_constant(float Time_constant);
|
||||
|
||||
float apply(float input, float dt);
|
||||
|
||||
// Return the cutoff frequency
|
||||
float get_Time_constant() const { return T; }
|
||||
|
||||
private:
|
||||
|
||||
//Previous output
|
||||
float Output_prev;
|
||||
|
||||
//Time constant
|
||||
float T;
|
||||
};
|
||||
|
||||
float LowPassFilter::apply(float input, float dt)
|
||||
{
|
||||
// do the filtering
|
||||
float output = T/(T+dt)*Output_prev + dt/(T+dt)*input;
|
||||
|
||||
Output_prev = output;
|
||||
|
||||
// return the value.
|
||||
return output;
|
||||
}
|
||||
|
||||
void LowPassFilter::set_Time_constant(float Time_constant)
|
||||
{
|
||||
T = Time_constant;
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,56 @@
|
||||
#pragma once
|
||||
#include <termios.h>
|
||||
#include <sstream>
|
||||
#include <vector>
|
||||
#include "ros/ros.h"
|
||||
|
||||
# define U_KEY_NONE -1
|
||||
# define U_KEY_SPACE 32
|
||||
# define U_KEY_LEFT 37
|
||||
# define U_KEY_UP 38
|
||||
# define U_KEY_RIGHT 39
|
||||
# define U_KEY_DOWN 40
|
||||
|
||||
# define U_KEY_0 48
|
||||
# define U_KEY_1 49
|
||||
# define U_KEY_2 50
|
||||
# define U_KEY_3 51
|
||||
# define U_KEY_4 52
|
||||
# define U_KEY_5 53
|
||||
# define U_KEY_6 54
|
||||
# define U_KEY_7 55
|
||||
# define U_KEY_8 56
|
||||
# define U_KEY_9 57
|
||||
|
||||
# define U_KEY_A 97
|
||||
# define U_KEY_D 100
|
||||
# define U_KEY_E 101
|
||||
# define U_KEY_F 102
|
||||
# define U_KEY_H 104
|
||||
# define U_KEY_K 107
|
||||
# define U_KEY_L 108
|
||||
# define U_KEY_M 109
|
||||
# define U_KEY_O 111
|
||||
# define U_KEY_P 112
|
||||
# define U_KEY_Q 113
|
||||
# define U_KEY_R 114
|
||||
# define U_KEY_S 115
|
||||
# define U_KEY_T 116
|
||||
# define U_KEY_U 117
|
||||
# define U_KEY_V 118
|
||||
# define U_KEY_W 119
|
||||
# define U_KEY_X 120
|
||||
# define U_KEY_Y 121
|
||||
# define U_KEY_Z 122
|
||||
|
||||
using namespace std;
|
||||
|
||||
class KeyboardEvent{
|
||||
static int input_flag; //keyboard flag
|
||||
static char buff; // keyboard buff
|
||||
static void getch();// get keyboard input. We need all instance to use the same function to catch the keyboard
|
||||
public:
|
||||
KeyboardEvent();
|
||||
void RosWhileLoopRun(); // this should be placed inside the ros while loop
|
||||
char GetPressedKey();
|
||||
};
|
||||
@ -0,0 +1,78 @@
|
||||
#pragma once
|
||||
#include "ros/ros.h"
|
||||
#include "std_msgs/Empty.h"
|
||||
#include "geometry_msgs/Twist.h"
|
||||
#include "geometry_msgs/PoseStamped.h"
|
||||
//#include "UtilityFunctions.h"
|
||||
#include <Eigen/Eigen>
|
||||
//maximum window size
|
||||
#ifndef max_windowsize
|
||||
#define max_windowsize 10
|
||||
#endif
|
||||
using namespace Eigen;
|
||||
|
||||
struct optitrack_pose{
|
||||
Vector3d Position;
|
||||
double q0;
|
||||
double q1;
|
||||
double q2;
|
||||
double q3;
|
||||
double t;
|
||||
Matrix<double, 3, 4> L;// Quaternion auxiliary matirx
|
||||
Matrix<double, 3, 4> R;// Quaternion auxiliary matirx
|
||||
Matrix3d R_IB; //
|
||||
Matrix3d R_BI; //
|
||||
};
|
||||
|
||||
struct rigidbody_state{
|
||||
Vector4d quaterion;
|
||||
Vector3d Position;// inertial position
|
||||
Vector3d V_I; // inertial velocity
|
||||
Matrix3d Omega_Cross; // angular velocity skew
|
||||
Vector3d Omega_BI;// Frame B to Frame I expressed in Frame B
|
||||
Matrix3d R_IB; // rotation matrix
|
||||
Matrix3d R_BI; //
|
||||
Vector3d Euler;// euler angle
|
||||
double time_stamp;
|
||||
};
|
||||
|
||||
class OptiTrackFeedBackRigidBody{
|
||||
|
||||
//-------Optitrack Related-----///
|
||||
geometry_msgs::PoseStamped OptiTrackdata;
|
||||
unsigned int OptiTrackFlag; // OptiTrackState 0: no data feed,: 1 data feed present
|
||||
void OptiTrackCallback(const geometry_msgs::PoseStamped& msg);
|
||||
unsigned int FeedbackState;// 0 no feedback, 1 has feedback
|
||||
ros::Subscriber subOptiTrack;// OptiTrack Data
|
||||
//--------Filter Parameters-------//
|
||||
unsigned int linear_velocity_window; // window size
|
||||
unsigned int angular_velocity_window; // window size
|
||||
//--------Filter Buffer-----------//
|
||||
// raw velocity buffer from numerical differentiation
|
||||
Vector3d velocity_raw[max_windowsize];
|
||||
Vector3d angular_velocity_raw[max_windowsize];
|
||||
Vector3d velocity_filtered; // filtered velocity
|
||||
Vector3d angular_velocity_filtered;// filtered angular velocity
|
||||
optitrack_pose pose[2];/*pose info from optitrack pose[1] should be the latest mesaured value,
|
||||
pose[0] is value of the last measurment (in world frame by default, if other frames
|
||||
are used , please changle the frame selectioin in the launch file */
|
||||
//--------Filter Methods-----------//
|
||||
void CalculateVelocityFromPose();// calculate velocity info from pose update measurements
|
||||
void MovingWindowAveraging();// a filter using moving window
|
||||
void PushRawVelocity(Vector3d& new_linear_velocity, Vector3d& new_angular_velocity);// push newly measured velocity into raw velocity buffer
|
||||
void PushPose();//push newly measured pose into dronepose buffer
|
||||
void SetZeroVelocity();
|
||||
//--------Update Rigid-body State ------//
|
||||
rigidbody_state state;
|
||||
public:
|
||||
OptiTrackFeedBackRigidBody(const char* name,ros::NodeHandle& n, unsigned int linear_window, unsigned int angular_window);
|
||||
~OptiTrackFeedBackRigidBody();
|
||||
int GetOptiTrackState();
|
||||
void GetState(rigidbody_state& state);
|
||||
void GetRaWVelocity(Vector3d& linear_velocity,Vector3d& angular_velocity);
|
||||
void RosWhileLoopRun();// This function should be put into ros while loop
|
||||
void GetEulerAngleFromQuaterion_NormalConvention(double (&eulerangle)[3]);
|
||||
void GetEulerAngleFromQuaterion_OptiTrackYUpConvention(double (&eulerangle)[3]);
|
||||
void Veemap(Matrix3d& cross_matrix, Vector3d& vector);
|
||||
void Hatmap(Vector3d& vector, Matrix3d& cross_matrix);
|
||||
};
|
||||
@ -0,0 +1,177 @@
|
||||
#include <ros/ros.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <termios.h>
|
||||
#include <errno.h>
|
||||
#include <string>
|
||||
#include <sensor_msgs/Range.h>
|
||||
|
||||
namespace benewake
|
||||
{
|
||||
|
||||
class TFmini
|
||||
{
|
||||
public:
|
||||
TFmini(std::string _name, int _baudRate);
|
||||
~TFmini(){};
|
||||
float getDist();
|
||||
void closePort();
|
||||
|
||||
unsigned char dataBuf[7];
|
||||
|
||||
private:
|
||||
std::string portName_;
|
||||
int baudRate_;
|
||||
int serial_;
|
||||
|
||||
bool readData(unsigned char *_buf, int _nRead);
|
||||
};
|
||||
|
||||
TFmini::TFmini(std::string _name, int _baudRate) :
|
||||
portName_(_name), baudRate_(_baudRate)
|
||||
{
|
||||
serial_ = open(_name.c_str(), O_RDWR);
|
||||
if(serial_ == -1)
|
||||
{
|
||||
ROS_ERROR_STREAM("Failed to open serial port!");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
struct termios options;
|
||||
if(tcgetattr(serial_, &options) != 0){
|
||||
ROS_ERROR_STREAM("Can't get serial port sets!");
|
||||
exit(0);
|
||||
}
|
||||
tcflush(serial_, TCIFLUSH);
|
||||
switch(_baudRate)
|
||||
{
|
||||
case 921600:
|
||||
cfsetispeed(&options, B921600);
|
||||
cfsetospeed(&options, B921600);
|
||||
break;
|
||||
case 576000:
|
||||
cfsetispeed(&options, B576000);
|
||||
cfsetospeed(&options, B576000);
|
||||
break;
|
||||
case 500000:
|
||||
cfsetispeed(&options, B500000);
|
||||
cfsetospeed(&options, B500000);
|
||||
break;
|
||||
case 460800:
|
||||
cfsetispeed(&options, B460800);
|
||||
cfsetospeed(&options, B460800);
|
||||
break;
|
||||
case 230400:
|
||||
cfsetispeed(&options, B230400);
|
||||
cfsetospeed(&options, B230400);
|
||||
break;
|
||||
case 115200:
|
||||
cfsetispeed(&options, B115200);
|
||||
cfsetospeed(&options, B115200);
|
||||
break;
|
||||
case 57600:
|
||||
cfsetispeed(&options, B57600);
|
||||
cfsetospeed(&options, B57600);
|
||||
break;
|
||||
case 38400:
|
||||
cfsetispeed(&options, B38400);
|
||||
cfsetospeed(&options, B38400);
|
||||
break;
|
||||
case 19200:
|
||||
cfsetispeed(&options, B19200);
|
||||
cfsetospeed(&options, B19200);
|
||||
break;
|
||||
case 9600:
|
||||
cfsetispeed(&options, B9600);
|
||||
cfsetospeed(&options, B9600);
|
||||
break;
|
||||
case 4800:
|
||||
cfsetispeed(&options, B4800);
|
||||
cfsetospeed(&options, B4800);
|
||||
break;
|
||||
default:
|
||||
ROS_ERROR_STREAM("Unsupported baud rate!");
|
||||
exit(0);
|
||||
}
|
||||
options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
|
||||
options.c_oflag &= ~OPOST;
|
||||
options.c_iflag &= ~(IXON | IXOFF | IXANY);
|
||||
options.c_iflag &= ~(INLCR | ICRNL | IGNCR);
|
||||
options.c_oflag &= ~(ONLCR | OCRNL);
|
||||
if(tcsetattr(serial_, TCSANOW, &options) != 0){
|
||||
ROS_ERROR_STREAM("Can't set serial port options!");
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
|
||||
bool TFmini::readData(unsigned char* _buf, int _nRead)
|
||||
{
|
||||
int total = 0, ret = 0;
|
||||
|
||||
while(total != _nRead)
|
||||
{
|
||||
ret = read(serial_, _buf + total, (_nRead - total));
|
||||
if(ret < 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
total += ret;
|
||||
}
|
||||
#ifdef DEBUG
|
||||
for(int i = 0; i < _nRead; i++)
|
||||
{
|
||||
printf("%02x ", _buf[i]);
|
||||
}
|
||||
printf("\n");
|
||||
#endif // DEBUG
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
float TFmini::getDist()
|
||||
{
|
||||
while(true)
|
||||
{
|
||||
if(readData(dataBuf, 2))
|
||||
{
|
||||
if(dataBuf[0] == 0x59 && dataBuf[1] == 0x59)
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1.0;
|
||||
}
|
||||
}
|
||||
|
||||
if(readData(dataBuf, 7))
|
||||
{
|
||||
int sumCheck = (0x59 + 0x59) % 256;
|
||||
for(int i = 0; i < 6; i++)
|
||||
{
|
||||
sumCheck = (sumCheck + dataBuf[i]) % 256;
|
||||
}
|
||||
|
||||
if(sumCheck == dataBuf[6])
|
||||
{
|
||||
return ((float)(dataBuf[1] << 8 | dataBuf[0]) / 100.0);
|
||||
}
|
||||
else
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1.0;
|
||||
}
|
||||
}
|
||||
|
||||
void TFmini::closePort()
|
||||
{
|
||||
close(serial_);
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,202 @@
|
||||
/***************************************************************************************************************************
|
||||
* math_utils.h
|
||||
*
|
||||
* Author: Qyp
|
||||
*
|
||||
* Update Time: 2019.3.16
|
||||
*
|
||||
* Introduction: math utils functions 数学工具函数
|
||||
*
|
||||
* 1、转换 ref to https://github.com/PX4/Matrix/blob/56b069956da141da244926ed7000e89b2ba6c731/matrix/Euler.hpp
|
||||
***************************************************************************************************************************/
|
||||
#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;
|
||||
}
|
||||
|
||||
Eigen::Vector4d rot2Quaternion(const Eigen::Matrix3d &R)
|
||||
{
|
||||
Eigen::Vector4d quat;
|
||||
double tr = R.trace();
|
||||
if (tr > 0.0) {
|
||||
double S = sqrt(tr + 1.0) * 2.0; // S=4*qw
|
||||
quat(0) = 0.25 * S;
|
||||
quat(1) = (R(2, 1) - R(1, 2)) / S;
|
||||
quat(2) = (R(0, 2) - R(2, 0)) / S;
|
||||
quat(3) = (R(1, 0) - R(0, 1)) / S;
|
||||
} else if ((R(0, 0) > R(1, 1)) & (R(0, 0) > R(2, 2))) {
|
||||
double S = sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2)) * 2.0; // S=4*qx
|
||||
quat(0) = (R(2, 1) - R(1, 2)) / S;
|
||||
quat(1) = 0.25 * S;
|
||||
quat(2) = (R(0, 1) + R(1, 0)) / S;
|
||||
quat(3) = (R(0, 2) + R(2, 0)) / S;
|
||||
} else if (R(1, 1) > R(2, 2)) {
|
||||
double S = sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2)) * 2.0; // S=4*qy
|
||||
quat(0) = (R(0, 2) - R(2, 0)) / S;
|
||||
quat(1) = (R(0, 1) + R(1, 0)) / S;
|
||||
quat(2) = 0.25 * S;
|
||||
quat(3) = (R(1, 2) + R(2, 1)) / S;
|
||||
} else {
|
||||
double S = sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1)) * 2.0; // S=4*qz
|
||||
quat(0) = (R(1, 0) - R(0, 1)) / S;
|
||||
quat(1) = (R(0, 2) + R(2, 0)) / S;
|
||||
quat(2) = (R(1, 2) + R(2, 1)) / S;
|
||||
quat(3) = 0.25 * S;
|
||||
}
|
||||
return quat;
|
||||
}
|
||||
|
||||
static Eigen::Vector3d matrix_hat_inv(const Eigen::Matrix3d &m)
|
||||
{
|
||||
Eigen::Vector3d v;
|
||||
// TODO: Sanity checks if m is skew symmetric
|
||||
v << m(7), m(2), m(3);
|
||||
return v;
|
||||
}
|
||||
|
||||
Eigen::Vector4d quatMultiplication(const Eigen::Vector4d &q, const Eigen::Vector4d &p)
|
||||
{
|
||||
Eigen::Vector4d quat;
|
||||
quat << p(0) * q(0) - p(1) * q(1) - p(2) * q(2) - p(3) * q(3), p(0) * q(1) + p(1) * q(0) - p(2) * q(3) + p(3) * q(2),
|
||||
p(0) * q(2) + p(1) * q(3) + p(2) * q(0) - p(3) * q(1), p(0) * q(3) - p(1) * q(2) + p(2) * q(1) + p(3) * q(0);
|
||||
return quat;
|
||||
}
|
||||
|
||||
Eigen::Matrix3d quat2RotMatrix(const Eigen::Vector4d &q)
|
||||
{
|
||||
Eigen::Matrix3d rotmat;
|
||||
rotmat << q(0) * q(0) + q(1) * q(1) - q(2) * q(2) - q(3) * q(3), 2 * q(1) * q(2) - 2 * q(0) * q(3),
|
||||
2 * q(0) * q(2) + 2 * q(1) * q(3),
|
||||
|
||||
2 * q(0) * q(3) + 2 * q(1) * q(2), q(0) * q(0) - q(1) * q(1) + q(2) * q(2) - q(3) * q(3),
|
||||
2 * q(2) * q(3) - 2 * q(0) * q(1),
|
||||
|
||||
2 * q(1) * q(3) - 2 * q(0) * q(2), 2 * q(0) * q(1) + 2 * q(2) * q(3),
|
||||
q(0) * q(0) - q(1) * q(1) - q(2) * q(2) + q(3) * q(3);
|
||||
return rotmat;
|
||||
}
|
||||
|
||||
//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,285 @@
|
||||
/***************************************************************************************************************************
|
||||
* prometheus_station_utils.h
|
||||
*
|
||||
* Author: Qyp
|
||||
*
|
||||
* Update Time: 2019.7.6
|
||||
***************************************************************************************************************************/
|
||||
#ifndef PROMETHEUS_STATION_UTILS_H
|
||||
#define PROMETHEUS_STATION_UTILS_H
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <math.h>
|
||||
#include <prometheus_msgs/Message.h>
|
||||
#include <prometheus_msgs/ControlCommand.h>
|
||||
#include <prometheus_msgs/SwarmCommand.h>
|
||||
#include <prometheus_msgs/DroneState.h>
|
||||
#include <prometheus_msgs/PositionReference.h>
|
||||
#include <prometheus_msgs/AttitudeReference.h>
|
||||
#include <prometheus_msgs/ControlOutput.h>
|
||||
#include <prometheus_msgs/LogMessageControl.h>
|
||||
#include <prometheus_msgs/LogMessageDetection.h>
|
||||
#include <prometheus_msgs/LogMessagePlanning.h>
|
||||
#include <prometheus_msgs/DetectionInfo.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <nav_msgs/Path.h>
|
||||
#include "printf_utils.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
#define NUM_POINT 2
|
||||
|
||||
|
||||
|
||||
|
||||
namespace prometheus_station_utils
|
||||
{
|
||||
|
||||
// 五种状态机
|
||||
enum MISSION_TYPE
|
||||
{
|
||||
CONTROL,
|
||||
LAND,
|
||||
PLAN,
|
||||
TRACK,
|
||||
};
|
||||
|
||||
// 打印上层控制指令
|
||||
void printf_command_control(const prometheus_msgs::ControlCommand& _ControlCommand)
|
||||
{
|
||||
cout << GREEN <<">>>>>>>>>>>>>>>>>>>>>>>> Control Command <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" << TAIL <<endl;
|
||||
|
||||
//固定的浮点显示
|
||||
cout.setf(ios::fixed);
|
||||
//setprecision(n) 设显示小数精度为n位
|
||||
cout<<setprecision(NUM_POINT);
|
||||
//左对齐
|
||||
cout.setf(ios::left);
|
||||
// 强制显示小数点
|
||||
cout.setf(ios::showpoint);
|
||||
// 强制显示符号
|
||||
cout.setf(ios::showpos);
|
||||
|
||||
cout << GREEN << "Source: [ "<< _ControlCommand.source << " ] Command_ID: "<< _ControlCommand.Command_ID << TAIL <<endl;
|
||||
|
||||
switch(_ControlCommand.Mode)
|
||||
{
|
||||
case prometheus_msgs::ControlCommand::Idle:
|
||||
|
||||
if(_ControlCommand.Reference_State.yaw_ref == 999)
|
||||
{
|
||||
cout << GREEN << "Command: [ Idle + Arming + Switching to OFFBOARD mode ] " << TAIL <<endl;
|
||||
}else
|
||||
{
|
||||
cout << GREEN << "Command: [ Idle ] " << TAIL <<endl;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case prometheus_msgs::ControlCommand::Takeoff:
|
||||
cout << GREEN << "Command: [ Takeoff ] " << TAIL <<endl;
|
||||
break;
|
||||
|
||||
case prometheus_msgs::ControlCommand::Hold:
|
||||
cout << GREEN << "Command: [ Hold ] " << TAIL <<endl;
|
||||
break;
|
||||
|
||||
case prometheus_msgs::ControlCommand::Land:
|
||||
cout << GREEN << "Command: [ Land ] " << TAIL <<endl;
|
||||
break;
|
||||
|
||||
case prometheus_msgs::ControlCommand::Move:
|
||||
if(_ControlCommand.Reference_State.Move_mode == prometheus_msgs::PositionReference::XYZ_POS)
|
||||
{
|
||||
cout << GREEN << "Command: [ Move ] " << "Move_mode: [ XYZ_POS ] " << TAIL <<endl;
|
||||
}else if(_ControlCommand.Reference_State.Move_mode == prometheus_msgs::PositionReference::XY_POS_Z_VEL)
|
||||
{
|
||||
cout << GREEN << "Command: [ Move ] " << "Move_mode: [ XY_POS_Z_VEL ] " << TAIL <<endl;
|
||||
}else if(_ControlCommand.Reference_State.Move_mode == prometheus_msgs::PositionReference::XY_VEL_Z_POS)
|
||||
{
|
||||
cout << GREEN << "Command: [ Move ] " << "Move_mode: [ XY_VEL_Z_POS ] " << TAIL <<endl;
|
||||
}else if(_ControlCommand.Reference_State.Move_mode == prometheus_msgs::PositionReference::XYZ_VEL)
|
||||
{
|
||||
cout << GREEN << "Command: [ Move ] " << "Move_mode: [ XYZ_VEL ] " << TAIL <<endl;
|
||||
}else if(_ControlCommand.Reference_State.Move_mode == prometheus_msgs::PositionReference::TRAJECTORY)
|
||||
{
|
||||
cout << GREEN << "Command: [ Move ] " << "Move_mode: [ TRAJECTORY ] " << TAIL <<endl;
|
||||
}
|
||||
|
||||
if(_ControlCommand.Reference_State.Move_frame == prometheus_msgs::PositionReference::ENU_FRAME)
|
||||
{
|
||||
cout << GREEN << "Move_frame: [ ENU_FRAME ] " << TAIL <<endl;
|
||||
}else if(_ControlCommand.Reference_State.Move_frame == prometheus_msgs::PositionReference::BODY_FRAME)
|
||||
{
|
||||
cout << GREEN << "Move_frame: [ BODY_FRAME ] " << TAIL <<endl;
|
||||
}
|
||||
|
||||
cout << GREEN << "Position [X Y Z] : " << _ControlCommand.Reference_State.position_ref[0] << " [ m ] "<< _ControlCommand.Reference_State.position_ref[1]<<" [ m ] "<< _ControlCommand.Reference_State.position_ref[2]<<" [ m ] "<< TAIL <<endl;
|
||||
cout << GREEN << "Velocity [X Y Z] : " << _ControlCommand.Reference_State.velocity_ref[0] << " [m/s] "<< _ControlCommand.Reference_State.velocity_ref[1]<<" [m/s] "<< _ControlCommand.Reference_State.velocity_ref[2]<<" [m/s] "<< TAIL <<endl;
|
||||
cout << GREEN << "Acceleration [X Y Z] : " << _ControlCommand.Reference_State.acceleration_ref[0] << " [m/s^2] "<< _ControlCommand.Reference_State.acceleration_ref[1]<<" [m/s^2] "<< _ControlCommand.Reference_State.acceleration_ref[2]<<" [m/s^2] "<< TAIL <<endl;
|
||||
|
||||
cout << GREEN << "Yaw : " << _ControlCommand.Reference_State.yaw_ref* 180/M_PI << " [deg] " << TAIL <<endl;
|
||||
|
||||
break;
|
||||
|
||||
case prometheus_msgs::ControlCommand::Disarm:
|
||||
cout << GREEN << "Command: [ Disarm ] " << TAIL <<endl;
|
||||
break;
|
||||
|
||||
case prometheus_msgs::ControlCommand::User_Mode1:
|
||||
cout << GREEN << "Command: [ User_Mode1 ] " << TAIL <<endl;
|
||||
break;
|
||||
|
||||
case prometheus_msgs::ControlCommand::User_Mode2:
|
||||
cout << GREEN << "Command: [ User_Mode2 ] " << TAIL <<endl;
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 打印无人机状态
|
||||
void prinft_drone_state(const prometheus_msgs::DroneState& _Drone_state)
|
||||
{
|
||||
cout << GREEN <<">>>>>>>>>>>>>>>>>>>>>>>> Drone State <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" << TAIL <<endl;
|
||||
|
||||
//固定的浮点显示
|
||||
cout.setf(ios::fixed);
|
||||
//setprecision(n) 设显示小数精度为n位
|
||||
cout<<setprecision(NUM_POINT);
|
||||
//左对齐
|
||||
cout.setf(ios::left);
|
||||
// 强制显示小数点
|
||||
cout.setf(ios::showpoint);
|
||||
// 强制显示符号
|
||||
cout.setf(ios::showpos);
|
||||
|
||||
cout << GREEN << "Time: " << _Drone_state.time_from_start <<" [s] ";
|
||||
|
||||
//是否和飞控建立起连接
|
||||
if (_Drone_state.connected == true)
|
||||
{
|
||||
cout << GREEN << " [ Connected ]";
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << GREEN << " [ Unconnected ]";
|
||||
}
|
||||
|
||||
//是否上锁
|
||||
if (_Drone_state.armed == true)
|
||||
{
|
||||
cout << GREEN << " [ Armed ]";
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << GREEN << " [ DisArmed ]";
|
||||
}
|
||||
|
||||
//是否在地面
|
||||
if (_Drone_state.landed == true)
|
||||
{
|
||||
cout << GREEN << " [ Ground ] ";
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << GREEN << " [ Air ] ";
|
||||
}
|
||||
|
||||
cout << GREEN << "[ " << _Drone_state.mode<<" ] " << TAIL <<endl;
|
||||
|
||||
cout << GREEN << "Position [X Y Z] : " << _Drone_state.position[0] << " [ m ] "<< _Drone_state.position[1]<<" [ m ] "<<_Drone_state.position[2]<<" [ m ] "<< TAIL <<endl;
|
||||
cout << GREEN << "Velocity [X Y Z] : " << _Drone_state.velocity[0] << " [m/s] "<< _Drone_state.velocity[1]<<" [m/s] "<<_Drone_state.velocity[2]<<" [m/s] "<< TAIL <<endl;
|
||||
cout << GREEN << "Attitude [R P Y] : " << _Drone_state.attitude[0] * 180/M_PI <<" [deg] "<<_Drone_state.attitude[1] * 180/M_PI << " [deg] "<< _Drone_state.attitude[2] * 180/M_PI<<" [deg] "<< TAIL <<endl;
|
||||
cout << GREEN << "Att_rate [R P Y] : " << _Drone_state.attitude_rate[0] * 180/M_PI <<" [deg/s] "<<_Drone_state.attitude_rate[1] * 180/M_PI << " [deg/s] "<< _Drone_state.attitude_rate[2] * 180/M_PI<<" [deg/s] "<< TAIL <<endl;
|
||||
}
|
||||
|
||||
// 打印位置控制器输出结果
|
||||
void prinft_attitude_reference(const prometheus_msgs::AttitudeReference& _AttitudeReference)
|
||||
{
|
||||
//固定的浮点显示
|
||||
cout.setf(ios::fixed);
|
||||
//setprecision(n) 设显示小数精度为n位
|
||||
cout<<setprecision(NUM_POINT);
|
||||
//左对齐
|
||||
cout.setf(ios::left);
|
||||
// 强制显示小数点
|
||||
cout.setf(ios::showpoint);
|
||||
// 强制显示符号
|
||||
cout.setf(ios::showpos);
|
||||
|
||||
cout << GREEN << "Attitude_sp [R P Y] : " << _AttitudeReference.desired_attitude[0] * 180/M_PI <<" [deg] "<<_AttitudeReference.desired_attitude[1] * 180/M_PI << " [deg] "<< _AttitudeReference.desired_attitude[2] * 180/M_PI<<" [deg] "<< TAIL <<endl;
|
||||
cout << GREEN << "Throttle_sp [ 0-1 ] : " << _AttitudeReference.desired_throttle << TAIL <<endl;
|
||||
}
|
||||
|
||||
// tracking error
|
||||
Eigen::Vector3d tracking_error(const prometheus_msgs::DroneState& _Drone_state, const prometheus_msgs::ControlCommand& _ControlCommand)
|
||||
{
|
||||
Eigen::Vector3d error;
|
||||
|
||||
error[0] = sqrt(pow(_Drone_state.position[0] - _ControlCommand.Reference_State.position_ref[0],2)+
|
||||
pow(_Drone_state.position[1] - _ControlCommand.Reference_State.position_ref[1],2)+
|
||||
pow(_Drone_state.position[2] - _ControlCommand.Reference_State.position_ref[2],2));
|
||||
error[1] = sqrt(pow(_Drone_state.velocity[0] - _ControlCommand.Reference_State.velocity_ref[0],2)+
|
||||
pow(_Drone_state.velocity[1] - _ControlCommand.Reference_State.velocity_ref[1],2)+
|
||||
pow(_Drone_state.velocity[2] - _ControlCommand.Reference_State.velocity_ref[2],2));
|
||||
error[2] = 0;
|
||||
|
||||
return error;
|
||||
}
|
||||
|
||||
|
||||
void prinft_ref_pose(const geometry_msgs::PoseStamped& ref_pose)
|
||||
{
|
||||
cout << GREEN <<">>>>>>>>>>>>>>>>>>>>>>> Ref Pose <<<<<<<<<<<<<<<<<<<<<<<<<<<<" << TAIL <<endl;
|
||||
|
||||
//固定的浮点显示
|
||||
cout.setf(ios::fixed);
|
||||
//setprecision(n) 设显示小数精度为n位
|
||||
cout<<setprecision(NUM_POINT);
|
||||
//左对齐
|
||||
cout.setf(ios::left);
|
||||
// 强制显示小数点
|
||||
cout.setf(ios::showpoint);
|
||||
// 强制显示符号
|
||||
cout.setf(ios::showpos);
|
||||
|
||||
cout << GREEN << "Ref_position [X Y Z] : " << ref_pose.pose.position.x <<" [m] "<< ref_pose.pose.position.y <<" [m] " << ref_pose.pose.position.z <<" [m] "<< TAIL <<endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> 其 他 函 数 <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
|
||||
|
||||
// 【获取当前时间函数】 单位：秒
|
||||
float get_time_in_sec(const ros::Time& begin_time)
|
||||
{
|
||||
ros::Time time_now = ros::Time::now();
|
||||
float currTimeSec = time_now.sec - begin_time.sec;
|
||||
float currTimenSec = time_now.nsec / 1e9 - begin_time.nsec / 1e9;
|
||||
return (currTimeSec + currTimenSec);
|
||||
}
|
||||
// 将四元数转换至(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;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
#endif
|
||||
@ -0,0 +1,161 @@
|
||||
//串口读取库函数（用于读取超声波数据）
|
||||
//主要功能：读取串口数据
|
||||
//输入：需要先设置波特率、端口号等
|
||||
//输出：串口数据
|
||||
|
||||
#ifndef _SERIAL_H
|
||||
#define _SERIAL_H
|
||||
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <termios.h>
|
||||
#include <stdlib.h>
|
||||
#include <errno.h>
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
|
||||
class Serial{
|
||||
public:
|
||||
Serial();
|
||||
~Serial();
|
||||
public:
|
||||
static int set_opt(int fd, int nSpeed, int nBits, char nEvent, int nStop);
|
||||
static int open_portUSB(int comport);
|
||||
static int open_port_ultrasonic();
|
||||
static int nwrite(int serialfd, unsigned char *data, int datalength);
|
||||
static int nread(int fd, unsigned char *data, int datalength);
|
||||
|
||||
};
|
||||
|
||||
|
||||
Serial::Serial( ) {
|
||||
}
|
||||
|
||||
Serial::~Serial() {
|
||||
}
|
||||
|
||||
|
||||
int Serial::set_opt(int fd,int nSpeed, int nBits, char nEvent, int nStop)
|
||||
{
|
||||
struct termios newtio,oldtio;
|
||||
if ( tcgetattr( fd,&oldtio) != 0) {
|
||||
perror("SetupSerial 1");
|
||||
return -1;
|
||||
}
|
||||
bzero(&newtio, sizeof(newtio));
|
||||
newtio.c_cflag |= CLOCAL | CREAD;
|
||||
newtio.c_cflag &= ~CSIZE;
|
||||
|
||||
switch( nBits )
|
||||
{
|
||||
case 7:
|
||||
newtio.c_cflag |= CS7;
|
||||
break;
|
||||
case 8:
|
||||
newtio.c_cflag |= CS8;
|
||||
break;
|
||||
}
|
||||
|
||||
switch( nEvent )
|
||||
{
|
||||
case 'O':
|
||||
newtio.c_cflag |= PARENB;
|
||||
newtio.c_cflag |= PARODD;
|
||||
newtio.c_iflag |= (INPCK | ISTRIP);
|
||||
break;
|
||||
case 'E':
|
||||
newtio.c_iflag |= (INPCK | ISTRIP);
|
||||
newtio.c_cflag |= PARENB;
|
||||
newtio.c_cflag &= ~PARODD;
|
||||
break;
|
||||
case 'N':
|
||||
newtio.c_cflag &= ~PARENB;
|
||||
break;
|
||||
}
|
||||
|
||||
switch( nSpeed )
|
||||
{
|
||||
case 2400:
|
||||
cfsetispeed(&newtio, B2400);
|
||||
cfsetospeed(&newtio, B2400);
|
||||
break;
|
||||
case 4800:
|
||||
cfsetispeed(&newtio, B4800);
|
||||
cfsetospeed(&newtio, B4800);
|
||||
break;
|
||||
case 9600:
|
||||
cfsetispeed(&newtio, B9600);
|
||||
cfsetospeed(&newtio, B9600);
|
||||
break;
|
||||
case 115200:
|
||||
cfsetispeed(&newtio, B115200);
|
||||
cfsetospeed(&newtio, B115200);
|
||||
break;
|
||||
default:
|
||||
cfsetispeed(&newtio, B9600);
|
||||
cfsetospeed(&newtio, B9600);
|
||||
break;
|
||||
}
|
||||
if( nStop == 1 )
|
||||
newtio.c_cflag &= ~CSTOPB;
|
||||
else if ( nStop == 2 )
|
||||
newtio.c_cflag |= CSTOPB;
|
||||
newtio.c_cc[VTIME] = 0;
|
||||
newtio.c_cc[VMIN] = 0;
|
||||
tcflush(fd,TCIFLUSH);
|
||||
if((tcsetattr(fd,TCSANOW,&newtio))!=0)
|
||||
{
|
||||
perror("com set error");
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int Serial::open_portUSB(int comport) //通过参数，打开相应的串口
|
||||
{
|
||||
string serial_port_adr("/dev/ttyUSB"+ to_string(comport));
|
||||
cout << serial_port_adr << endl;
|
||||
int fd = open( serial_port_adr.c_str() , O_RDWR|O_NOCTTY);
|
||||
if (-1 == fd)
|
||||
{
|
||||
perror("Can't Open Serial Port!!");
|
||||
}
|
||||
return fd;
|
||||
}
|
||||
|
||||
|
||||
int Serial::open_port_ultrasonic() //通过参数，打开相应的串口
|
||||
{
|
||||
string serial_port_adr("/dev/ultrasonic");
|
||||
cout << serial_port_adr << endl;
|
||||
int fd = open( serial_port_adr.c_str() , O_RDWR|O_NOCTTY);
|
||||
if (-1 == fd)
|
||||
{
|
||||
perror("Can't Open Serial Port!!");
|
||||
}
|
||||
return fd;
|
||||
}
|
||||
|
||||
|
||||
int Serial::nwrite (int serialfd,unsigned char *data, int datalength )
|
||||
{
|
||||
return write(serialfd, data, datalength);
|
||||
}
|
||||
|
||||
int Serial::nread(int fd, unsigned char *data,int datalength) {
|
||||
int readlen = read(fd,data,datalength);
|
||||
cout << "readlen: " << readlen<<endl;
|
||||
if( readlen <= 0 )
|
||||
{
|
||||
printf("read error!!");
|
||||
}
|
||||
return readlen;
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,72 @@
|
||||
#include "ros/ros.h"
|
||||
#include "OptiTrackFeedBackRigidBody.h"
|
||||
#include "KeyboardEvent.h"
|
||||
#include <Eigen/Eigen>
|
||||
using namespace std;
|
||||
using namespace Eigen;
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
double Control_Rate = 40;// Hz the rate
|
||||
|
||||
// Initialize ros node
|
||||
ros::init(argc, argv, "OptiTrackTest");
|
||||
ros::NodeHandle n;
|
||||
// Initialize Data Recorder
|
||||
//DataRecorder Rigidbody1_recorder("OptiTestRigidbody1.txt",31,Control_Rate);
|
||||
double Rigidbody1state[16];
|
||||
/* 0 OptTrack Flag
|
||||
1 - 3 Rigidbody1 positition from OpTiFeedback (hx,hy,Altitude)
|
||||
4 - 6 Rigidbody1 filtered velocity from OpTiFeedback
|
||||
7 - 9 Rigidbody1 raw velocity from Optifeedback
|
||||
10 - 12 Rigidbody1 raw angular velocity from Optifeedback
|
||||
13 - 15 Rigidbody1 filtered angular velocity from Optifeedback
|
||||
16 - 18 Rigidbody1 euler angle from OpTiFeedback
|
||||
19 - 22 Rigidbody1 quaterion from OpTiFeedback
|
||||
23 - 31 Rigidbody1 Rotation matrix R_IB from OptiFeedback
|
||||
*/
|
||||
// Initialize OptiTrack System
|
||||
OptiTrackFeedBackRigidBody Opti_RigidBody1("/vrpn_client_node/UAV/pose",n,3,3);
|
||||
OptiTrackFeedBackRigidBody Opti_RigidBody2("/vrpn_client_node/RigidBody2/pose",n,3,3);
|
||||
KeyboardEvent keyboardcontrol;
|
||||
rigidbody_state RigidBody1state;
|
||||
ros::Rate loop_rate(Control_Rate);
|
||||
while (ros::ok())
|
||||
{
|
||||
Opti_RigidBody1.RosWhileLoopRun();
|
||||
Opti_RigidBody2.RosWhileLoopRun();
|
||||
keyboardcontrol.RosWhileLoopRun();
|
||||
switch (keyboardcontrol.GetPressedKey())
|
||||
{
|
||||
|
||||
case U_KEY_E:
|
||||
{
|
||||
//Rigidbody1_recorder.StartRecording();
|
||||
}
|
||||
case U_KEY_Q:
|
||||
{
|
||||
Opti_RigidBody1.GetOptiTrackState();
|
||||
Opti_RigidBody2.GetOptiTrackState();
|
||||
Opti_RigidBody1.GetState(RigidBody1state);
|
||||
cout<< "Rigid 1 Position: \n" << RigidBody1state.Position << endl;
|
||||
cout<< "Rigid 1 Velocity: \n" << RigidBody1state.V_I << endl;
|
||||
cout<< "Rigid 1 AngularVelocity: \n" << RigidBody1state.Omega_BI << endl;
|
||||
cout<< "Rigid 1 R_IB: \n" << RigidBody1state.R_IB << endl;
|
||||
cout<< "Rigid 1 R_BI: \n" << RigidBody1state.R_BI << endl;
|
||||
cout<< "Rigid 1 Rotation Test: \n" << RigidBody1state.R_IB * RigidBody1state.R_BI<< endl;
|
||||
cout<< "Rigid 1 Euler: \n" <<RigidBody1state.Euler*57.3<<endl;
|
||||
cout<< "Rigid 1 quaterion: \n" <<RigidBody1state.quaterion<<endl;
|
||||
//Rigidbody1_recorder.StopRecording();
|
||||
break;
|
||||
}
|
||||
case U_KEY_NONE:
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
//Rigidbody1_recorder.RecorderUpdate(Rigidbody1state);
|
||||
ros::spinOnce();// do the loop once
|
||||
loop_rate.sleep();
|
||||
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,52 @@
|
||||
#include <TFmini.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "TFmini");
|
||||
ros::NodeHandle nh("~");
|
||||
std::string id = "TFmini";
|
||||
std::string portName;
|
||||
int baud_rate;
|
||||
benewake::TFmini *tfmini_obj;
|
||||
|
||||
nh.param("serial_port", portName, std::string("/dev/ttyUSB0"));
|
||||
nh.param("baud_rate", baud_rate, 115200);
|
||||
|
||||
tfmini_obj = new benewake::TFmini(portName, baud_rate);
|
||||
ros::Publisher pub_range = nh.advertise<sensor_msgs::Range>("/prometheus/tfmini_range", 1000, true);
|
||||
sensor_msgs::Range TFmini_range;
|
||||
TFmini_range.radiation_type = sensor_msgs::Range::INFRARED;
|
||||
TFmini_range.field_of_view = 0.04;
|
||||
TFmini_range.min_range = 0.3;
|
||||
TFmini_range.max_range = 12;
|
||||
TFmini_range.header.frame_id = id;
|
||||
float dist = 0;
|
||||
ROS_INFO_STREAM("Start processing ...");
|
||||
|
||||
while(ros::master::check() && ros::ok())
|
||||
{
|
||||
ros::spinOnce();
|
||||
dist = tfmini_obj->getDist();
|
||||
if(dist > 0 && dist < TFmini_range.max_range)
|
||||
{
|
||||
TFmini_range.range = dist;
|
||||
TFmini_range.header.stamp = ros::Time::now();
|
||||
pub_range.publish(TFmini_range);
|
||||
|
||||
cout << "get" << dist<<endl;
|
||||
}
|
||||
else if(dist == -1.0)
|
||||
{
|
||||
ROS_ERROR_STREAM("Failed to read data. TFmini ros node stopped!");
|
||||
break;
|
||||
}
|
||||
else if(dist == 0.0)
|
||||
{
|
||||
ROS_ERROR_STREAM("Data validation error!");
|
||||
}
|
||||
}
|
||||
|
||||
tfmini_obj->closePort();
|
||||
}
|
||||
@ -0,0 +1,59 @@
|
||||
|
||||
//头文件
|
||||
#include <ros/ros.h>
|
||||
|
||||
|
||||
#include <iostream>
|
||||
|
||||
//话题头文件
|
||||
|
||||
#include <mavros_msgs/SetMode.h>
|
||||
#include <mavros_msgs/State.h>
|
||||
#include <mavros_msgs/State.h>
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <math.h>
|
||||
#include <math_utils.h>
|
||||
|
||||
|
||||
using namespace std;
|
||||
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "test");
|
||||
ros::NodeHandle nh("~");
|
||||
|
||||
ros::Rate rate(20.0);
|
||||
Eigen::Vector3d vector;
|
||||
|
||||
vector = Eigen::Vector3d(0.0,0.0,0.0);
|
||||
|
||||
|
||||
//float a;
|
||||
|
||||
Eigen::Vector3d thr_sp;
|
||||
float yaw_sp;
|
||||
|
||||
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 循 环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
|
||||
while(ros::ok())
|
||||
{
|
||||
|
||||
cout << "Please input the thrust_x: "<<endl;
|
||||
cin >> thr_sp(0);
|
||||
cout << "Please input the thrust_y: "<<endl;
|
||||
cin >> thr_sp(1);
|
||||
cout << "Please input the thrust_z: "<<endl;
|
||||
cin >> thr_sp(2);
|
||||
cout << "Please input the yaw_sp: "<<endl;
|
||||
cin >> yaw_sp;
|
||||
|
||||
|
||||
//Eigen::Quaterniond q_sp = ThrottleToAttitude(thr_sp, yaw_sp);
|
||||
|
||||
//周期休眠
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
@ -0,0 +1,87 @@
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <Filter/LowPassFilter.h>
|
||||
#include <prometheus_control_utils.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "filter_tester");
|
||||
ros::NodeHandle nh;
|
||||
|
||||
geometry_msgs::Point random;
|
||||
|
||||
ros::Publisher log_pub = nh.advertise<geometry_msgs::Point>("/prometheus/test", 10);
|
||||
|
||||
ros::Rate rate(50.0);
|
||||
|
||||
float T1 = 0.1;
|
||||
float T2 = 1;
|
||||
|
||||
LowPassFilter LPF1;
|
||||
LowPassFilter LPF2;
|
||||
LowPassFilter LPF3;
|
||||
|
||||
LPF1.set_Time_constant(T2);
|
||||
LPF2.set_Time_constant(T2);
|
||||
LPF3.set_Time_constant(T2);
|
||||
|
||||
|
||||
float dt = 1;
|
||||
|
||||
float input1,input2;
|
||||
float output1,output2;
|
||||
|
||||
while(ros::ok())
|
||||
{
|
||||
|
||||
// cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>--------<<<<<<<<<<<<<<<<<<<<<<<<<<< "<< endl;
|
||||
// cout << "Input the input of the LPF1"<<endl;
|
||||
// cin >> input1;
|
||||
|
||||
// cout << "Input the input of the LPF2"<<endl;
|
||||
// cin >> input2;
|
||||
|
||||
// output1 = LPF1.apply(input1,dt);
|
||||
|
||||
// output2 = LPF2.apply(input2,dt);
|
||||
|
||||
// float _T1 = LPF1.get_Time_constant();
|
||||
// float _T2 = LPF2.get_Time_constant();
|
||||
|
||||
|
||||
// cout << "T for LPF1: "<< _T1 <<endl;
|
||||
// cout << "T for LPF2: "<< _T2 <<endl;
|
||||
|
||||
// cout << "ouput for LPF1: "<< output1 <<endl;
|
||||
// cout << "ouput for LPF2: "<< output2 <<endl;
|
||||
|
||||
|
||||
// 先生成随机数
|
||||
random.x = prometheus_control_utils::random_num(0.2, 0.1);
|
||||
random.y = prometheus_control_utils::random_num(2, 0.0);
|
||||
random.z = prometheus_control_utils::random_num(0.1, 0.05);
|
||||
|
||||
// 低通滤波
|
||||
random.x = LPF1.apply(random.x , 0.02);
|
||||
random.y = LPF2.apply(random.y , 0.02);
|
||||
random.z = LPF3.apply(random.z , 0.02);
|
||||
|
||||
ROS_INFO("random.x: [%f]", random.x);
|
||||
ROS_INFO("random.y: [%f]", random.y);
|
||||
ROS_INFO("random.z: [%f]", random.z);
|
||||
|
||||
|
||||
log_pub.publish(random);
|
||||
|
||||
|
||||
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,180 @@
|
||||
/***************************************************************************************************************************
|
||||
* set_mode.cpp
|
||||
*
|
||||
* Author: Qyp
|
||||
*
|
||||
* Update Time: 2019.3.16
|
||||
*
|
||||
* Introduction: Change mode
|
||||
***************************************************************************************************************************/
|
||||
|
||||
//头文件
|
||||
#include <ros/ros.h>
|
||||
|
||||
|
||||
#include <iostream>
|
||||
|
||||
//话题头文件
|
||||
#include <mavros_msgs/CommandBool.h>
|
||||
#include <mavros_msgs/SetMode.h>
|
||||
#include <mavros_msgs/State.h>
|
||||
|
||||
|
||||
using namespace std;
|
||||
mavros_msgs::State current_state; //无人机当前状态[包含上锁状态 模式] (从飞控中读取)
|
||||
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
|
||||
void state_cb(const mavros_msgs::State::ConstPtr& msg)
|
||||
{
|
||||
current_state = *msg;
|
||||
}
|
||||
|
||||
|
||||
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "set_mode");
|
||||
ros::NodeHandle nh("~");
|
||||
|
||||
// 【订阅】无人机当前状态 - 来自飞控
|
||||
// 本话题来自飞控(通过/plugins/sys_status.cpp)
|
||||
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("/mavros/state", 10, state_cb);
|
||||
|
||||
// 【服务】修改系统模式
|
||||
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
|
||||
|
||||
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
|
||||
|
||||
mavros_msgs::SetMode mode_cmd;
|
||||
|
||||
mavros_msgs::CommandBool arm_cmd;
|
||||
arm_cmd.request.value = true;
|
||||
|
||||
ros::Rate rate(10.0);
|
||||
|
||||
int Num_StateMachine = 0;
|
||||
int flag_1;
|
||||
|
||||
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 循 环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
|
||||
while(ros::ok())
|
||||
{
|
||||
switch (Num_StateMachine)
|
||||
{
|
||||
// input
|
||||
case 0:
|
||||
cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>--------<<<<<<<<<<<<<<<<<<<<<<<<<<< "<< endl;
|
||||
cout << "Input the mode: 0 for OFFBOARD,1 for STABILIZED, 2 for POSCTL,3 for ALTCTL, 4 for arm "<<endl;
|
||||
cin >> flag_1;
|
||||
|
||||
//1000 降落 也可以指向其他任务
|
||||
if (flag_1 == 0)
|
||||
{
|
||||
Num_StateMachine = 1;
|
||||
break;
|
||||
}
|
||||
else if (flag_1 == 1)
|
||||
{
|
||||
Num_StateMachine = 2;
|
||||
break;
|
||||
}
|
||||
else if(flag_1 == 2)
|
||||
{
|
||||
Num_StateMachine = 3;
|
||||
}//惯性系移动
|
||||
else if(flag_1 == 3)
|
||||
{
|
||||
Num_StateMachine = 4;
|
||||
}
|
||||
else if(flag_1 == 4)
|
||||
{
|
||||
Num_StateMachine = 5;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
//OFFBOARD
|
||||
case 1:
|
||||
if(current_state.mode != "OFFBOARD")
|
||||
{
|
||||
mode_cmd.request.custom_mode = "OFFBOARD";
|
||||
set_mode_client.call(mode_cmd);
|
||||
cout << "Setting to OFFBOARD Mode..." <<endl;
|
||||
|
||||
}else
|
||||
{
|
||||
Num_StateMachine = 0;
|
||||
cout << "Set to OFFBOARD Mode Susscess!!!" <<endl;
|
||||
}
|
||||
break;
|
||||
|
||||
//STABILIZED
|
||||
case 2:
|
||||
if(current_state.mode != "STABILIZED")
|
||||
{
|
||||
mode_cmd.request.custom_mode = "STABILIZED";
|
||||
set_mode_client.call(mode_cmd);
|
||||
cout << "Setting to STABILIZED Mode..." <<endl;
|
||||
|
||||
}else
|
||||
{
|
||||
Num_StateMachine = 0;
|
||||
cout << "Set to STABILIZED Mode Susscess!!!" <<endl;
|
||||
}
|
||||
break;
|
||||
|
||||
//POSCTL
|
||||
case 3:
|
||||
if(current_state.mode != "POSCTL")
|
||||
{
|
||||
mode_cmd.request.custom_mode = "POSCTL";
|
||||
set_mode_client.call(mode_cmd);
|
||||
cout << "Setting to POSCTL Mode..." <<endl;
|
||||
|
||||
}else
|
||||
{
|
||||
Num_StateMachine = 0;
|
||||
cout << "Set to POSCTL Mode Susscess!!!" <<endl;
|
||||
}
|
||||
break;
|
||||
|
||||
//ALTCTL
|
||||
case 4:
|
||||
if(current_state.mode != "ALTCTL")
|
||||
{
|
||||
mode_cmd.request.custom_mode = "ALTCTL";
|
||||
set_mode_client.call(mode_cmd);
|
||||
cout << "Setting to ALTCTL Mode..." <<endl;
|
||||
|
||||
}else
|
||||
{
|
||||
Num_StateMachine = 0;
|
||||
cout << "Set to ALTCTL Mode Susscess!!!" <<endl;
|
||||
}
|
||||
break;
|
||||
|
||||
//arm
|
||||
case 5:
|
||||
if(!current_state.armed)
|
||||
{
|
||||
arm_cmd.request.value = true;
|
||||
arming_client.call(arm_cmd);
|
||||
|
||||
cout << "Arming..." <<endl;
|
||||
|
||||
}else
|
||||
{
|
||||
Num_StateMachine = 0;
|
||||
cout << "Arm Susscess!!!" <<endl;
|
||||
}
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
//执行回调函数
|
||||
ros::spinOnce();
|
||||
//周期休眠
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
@ -0,0 +1,68 @@
|
||||
#include "KeyboardEvent.h"
|
||||
|
||||
KeyboardEvent::KeyboardEvent()
|
||||
{
|
||||
input_flag = -1;
|
||||
buff = 0;
|
||||
}
|
||||
|
||||
void KeyboardEvent::getch()
|
||||
{
|
||||
input_flag = -1; //if not key is pressed, then return -1 for the keyboard_input. reset this flag every time
|
||||
fd_set set;
|
||||
struct timeval timeout;
|
||||
int rv;
|
||||
buff = 0;
|
||||
int len = 1;
|
||||
int filedesc = 0;
|
||||
FD_ZERO(&set);
|
||||
FD_SET(filedesc, &set);
|
||||
|
||||
timeout.tv_sec = 0;
|
||||
//timeout.tv_usec = 1000;//
|
||||
timeout.tv_usec = 1000;
|
||||
rv = select(filedesc + 1, &set, NULL, NULL, &timeout);
|
||||
|
||||
struct termios old = {0};
|
||||
if (tcgetattr(filedesc, &old) < 0)
|
||||
ROS_ERROR("tcsetattr()");
|
||||
old.c_lflag &= ~ICANON;
|
||||
old.c_lflag &= ~ECHO;
|
||||
old.c_cc[VMIN] = 1;
|
||||
old.c_cc[VTIME] = 0;
|
||||
if (tcsetattr(filedesc, TCSANOW, &old) < 0)
|
||||
ROS_ERROR("tcsetattr ICANON");
|
||||
|
||||
if(rv == -1){
|
||||
ROS_ERROR("select");
|
||||
}
|
||||
else if(rv == 0)
|
||||
{
|
||||
//{ROS_INFO("-----");
|
||||
}
|
||||
else
|
||||
{read(filedesc, &buff, len);
|
||||
input_flag = 1;}
|
||||
|
||||
old.c_lflag |= ICANON;
|
||||
old.c_lflag |= ECHO;
|
||||
if (tcsetattr(filedesc, TCSADRAIN, &old) < 0)
|
||||
ROS_ERROR ("tcsetattr ~ICANON");
|
||||
}
|
||||
char KeyboardEvent::GetPressedKey()
|
||||
{
|
||||
char key;
|
||||
if(input_flag>-1)//if keyboard is pressed, then perform the callbacks
|
||||
{
|
||||
key = buff;
|
||||
}else{
|
||||
key = U_KEY_NONE;
|
||||
}
|
||||
return key;
|
||||
}
|
||||
void KeyboardEvent::RosWhileLoopRun()
|
||||
{
|
||||
getch();
|
||||
}
|
||||
int KeyboardEvent::input_flag;
|
||||
char KeyboardEvent::buff;
|
||||
@ -0,0 +1,385 @@
|
||||
#include "OptiTrackFeedBackRigidBody.h"
|
||||
|
||||
OptiTrackFeedBackRigidBody::OptiTrackFeedBackRigidBody(const char* name,ros::NodeHandle& n,unsigned int linear_window, unsigned int angular_window)
|
||||
{
|
||||
// load filter window size
|
||||
linear_velocity_window = linear_window;
|
||||
angular_velocity_window = angular_window;
|
||||
if(linear_velocity_window>max_windowsize)
|
||||
{
|
||||
ROS_INFO("Linear Velocity Window Size Overlimit, Max Value is [%d]",max_windowsize);
|
||||
ROS_INFO("Input Valude is [%d]",linear_velocity_window);
|
||||
linear_velocity_window = max_windowsize;
|
||||
}
|
||||
if(angular_velocity_window>max_windowsize)
|
||||
{
|
||||
ROS_INFO("Angular Velocity Window Size Overlimit, Max Value is [%d]",max_windowsize);
|
||||
ROS_INFO("Input Valude is [%d]",angular_velocity_window);
|
||||
angular_velocity_window = max_windowsize;
|
||||
}
|
||||
// set up subscriber to vrpn optitrack beedback
|
||||
subOptiTrack = n.subscribe(name, 1, &OptiTrackFeedBackRigidBody::OptiTrackCallback,this);
|
||||
//Initialize all velocity
|
||||
for(int i =0;i<max_windowsize;i++)
|
||||
{
|
||||
velocity_raw[i](0)=0;
|
||||
velocity_raw[i](1)=0;
|
||||
velocity_raw[i](2)=0;
|
||||
angular_velocity_raw[i](0)=0;
|
||||
angular_velocity_raw[i](1)=0;
|
||||
angular_velocity_raw[i](2)=0;
|
||||
}
|
||||
velocity_filtered(0)=0;
|
||||
velocity_filtered(1)=0;
|
||||
velocity_filtered(2)=0;
|
||||
angular_velocity_filtered(0)=0;
|
||||
angular_velocity_filtered(1)=0;
|
||||
angular_velocity_filtered(2)=0;
|
||||
//Initialize all pose
|
||||
for(int i = 0;i<2;i++)
|
||||
{
|
||||
pose[i].q0 = 1;
|
||||
pose[i].q1 = 0;
|
||||
pose[i].q2 = 0;
|
||||
pose[i].q3 = 0;
|
||||
pose[i].t = 0;
|
||||
pose[i].Position(0) = 0;
|
||||
pose[i].Position(1) = 0;
|
||||
pose[i].Position(2) = 0;
|
||||
pose[i].L<< 0,1,0,0,
|
||||
0,0,1,0,
|
||||
0,0,0,1;
|
||||
pose[i].R<< 0,1,0,0,
|
||||
0,0,1,0,
|
||||
0,0,0,1;
|
||||
pose[1].R_IB<< 1,0,0,
|
||||
0,1,0,
|
||||
0,0,1;
|
||||
pose[1].R_BI<< 1,0,0,
|
||||
0,1,0,
|
||||
0,0,1;
|
||||
}
|
||||
// Initialize flag
|
||||
OptiTrackFlag = 0;
|
||||
FeedbackState = 0;
|
||||
}
|
||||
|
||||
void OptiTrackFeedBackRigidBody::CalculateVelocityFromPose()
|
||||
{
|
||||
|
||||
/* Logic:
|
||||
* 1) push the current pose into buffer
|
||||
* 2) determine whether the buffer has a valid time value (pose[0].t >0); if so calculate velocity
|
||||
* 3) if not just set the velocity_onestep as zero
|
||||
* 4) push current time, and velocity_onestep into the velocity buffer
|
||||
* 5) update the filtered velocity
|
||||
*/
|
||||
// perform the Logic:
|
||||
// step (1): push the current pose into buffer
|
||||
PushPose();
|
||||
// step (2): determine whether the buffer has a valid time value (pose[0].t >0); if so calculate velocity
|
||||
double dt = 0.0;
|
||||
Vector3d velocity_onestep;
|
||||
Vector3d angular_velocity_onestep;
|
||||
if (pose[0].t >0)// calculate only when last time stamp has been recorded.
|
||||
{
|
||||
// step (2)
|
||||
dt = pose[1].t - pose[0].t;// time step
|
||||
// calculate linear velocity
|
||||
velocity_onestep = (pose[1].Position- pose[0].Position)/dt;
|
||||
// calculate angular velocity
|
||||
Matrix3d RotationDifference = - pose[1].R_BI*pose[0].R_IB/dt;
|
||||
Veemap(RotationDifference,angular_velocity_onestep);
|
||||
}else// step (3): if not set velocity to zero
|
||||
{
|
||||
velocity_onestep(0) = 0.0;
|
||||
velocity_onestep(1) = 0.0;
|
||||
velocity_onestep(2) = 0.0;
|
||||
angular_velocity_onestep(0) = 0.0;
|
||||
angular_velocity_onestep(1) = 0.0;
|
||||
angular_velocity_onestep(2) = 0.0;
|
||||
}
|
||||
// step (4): push current time, and velocity_onestep into the velocity buffer
|
||||
PushRawVelocity(velocity_onestep,angular_velocity_onestep);
|
||||
// step (5): update filtered velocity
|
||||
MovingWindowAveraging();
|
||||
}
|
||||
void OptiTrackFeedBackRigidBody::PushPose()
|
||||
{
|
||||
pose[0] = pose[1];// straightforward push the pose into buffer
|
||||
// update the latest pose
|
||||
double t_current = (double)OptiTrackdata.header.stamp.sec + (double)OptiTrackdata.header.stamp.nsec*0.000000001;
|
||||
pose[1].t = t_current;
|
||||
// take a special note at the order of the quaterion
|
||||
pose[1].q0 = OptiTrackdata.pose.orientation.w;
|
||||
pose[1].q1 = OptiTrackdata.pose.orientation.x;
|
||||
pose[1].q2 = OptiTrackdata.pose.orientation.y;
|
||||
pose[1].q3 = OptiTrackdata.pose.orientation.z;
|
||||
// update the auxiliary matrix
|
||||
/*
|
||||
L = [-q1 q0 q3 -q2;
|
||||
-q2 -q3 q0 q1;
|
||||
-q3 q2 -q1 q0]
|
||||
R = [-q1 q0 -q3 q2;
|
||||
-q2 q3 q0 -q1;
|
||||
-q3 -q2 q1 q0]
|
||||
R_IB = RL^T
|
||||
*/
|
||||
pose[1].L(0,0) = - pose[1].q1;
|
||||
pose[1].L(1,0) = - pose[1].q2;
|
||||
pose[1].L(2,0) = - pose[1].q3;
|
||||
|
||||
pose[1].L(0,1) = pose[1].q0;
|
||||
pose[1].L(1,2) = pose[1].q0;
|
||||
pose[1].L(2,3) = pose[1].q0;
|
||||
|
||||
pose[1].L(0,2) = pose[1].q3;
|
||||
pose[1].L(0,3) = - pose[1].q2;
|
||||
pose[1].L(1,1) = - pose[1].q3;
|
||||
pose[1].L(1,3) = pose[1].q1;
|
||||
pose[1].L(2,1) = pose[1].q2;
|
||||
pose[1].L(2,2) = - pose[1].q1;
|
||||
|
||||
pose[1].R(0,0) = - pose[1].q1;
|
||||
pose[1].R(1,0) = - pose[1].q2;
|
||||
pose[1].R(2,0) = - pose[1].q3;
|
||||
|
||||
pose[1].R(0,1) = pose[1].q0;
|
||||
pose[1].R(1,2) = pose[1].q0;
|
||||
pose[1].R(2,3) = pose[1].q0;
|
||||
|
||||
pose[1].R(0,2) = -pose[1].q3;
|
||||
pose[1].R(0,3) = pose[1].q2;
|
||||
pose[1].R(1,1) = pose[1].q3;
|
||||
pose[1].R(1,3) = -pose[1].q1;
|
||||
pose[1].R(2,1) = -pose[1].q2;
|
||||
pose[1].R(2,2) = pose[1].q1;
|
||||
|
||||
pose[1].R_IB = pose[1].R * pose[1].L.transpose();
|
||||
pose[1].R_BI = pose[1].R_IB.transpose();
|
||||
// position is straight forward
|
||||
pose[1].Position(0) = OptiTrackdata.pose.position.x;
|
||||
pose[1].Position(1) = OptiTrackdata.pose.position.y;
|
||||
pose[1].Position(2) = OptiTrackdata.pose.position.z;
|
||||
}
|
||||
|
||||
void OptiTrackFeedBackRigidBody::PushRawVelocity(Vector3d& new_linear_velocity, Vector3d& new_angular_velocity)
|
||||
{
|
||||
/* Logic:
|
||||
* a(i-1) = a(i), i = 2...windowsize
|
||||
* should fristly start from i = 2. a(1) = a(2); a(2) = a(3);....; a(N-1) = a(N)
|
||||
* secondly a(N) = a_new
|
||||
*/
|
||||
// linear velocity
|
||||
for(int i = 1;i<linear_velocity_window;i++)//first step
|
||||
{
|
||||
velocity_raw[i-1] = velocity_raw[i];
|
||||
}
|
||||
velocity_raw[linear_velocity_window-1] = new_linear_velocity;// second step update the last variable in the velocity buffer
|
||||
// angular velocity
|
||||
for(int i = 1;i<angular_velocity_window;i++)//first step
|
||||
{
|
||||
angular_velocity_raw[i-1] = angular_velocity_raw[i];
|
||||
}
|
||||
angular_velocity_raw[angular_velocity_window-1] = new_angular_velocity;// second step update the last variable in the velocity buffer
|
||||
}
|
||||
|
||||
void OptiTrackFeedBackRigidBody::MovingWindowAveraging()
|
||||
{
|
||||
|
||||
/* Logic: Average the raw velocity measurement in the
|
||||
*/
|
||||
double weight_linear = (double)1/linear_velocity_window;// the weight on each velocity to be summed up.
|
||||
double weight_angular = (double)1/angular_velocity_window;// the weight on each velocity to be summed up.
|
||||
// create a temporary variable to store the summed velocity and initialize it witht the 1st buffer value
|
||||
Vector3d velocitytemp;
|
||||
Vector3d angular_velocitytemp;
|
||||
velocitytemp = weight_linear*velocity_raw[0];
|
||||
angular_velocitytemp = weight_angular*angular_velocity_raw[0];
|
||||
|
||||
for(int i = 1;i<linear_velocity_window;i++)// sum starts from the second buffer value
|
||||
{
|
||||
velocitytemp += weight_linear*velocity_raw[i];
|
||||
}
|
||||
for(int i = 1;i<angular_velocity_window;i++)// sum starts from the second buffer value
|
||||
{
|
||||
angular_velocitytemp += weight_angular*angular_velocity_raw[i];
|
||||
}
|
||||
// the filtered vlocity is just the weighted summed result
|
||||
velocity_filtered = velocitytemp;
|
||||
angular_velocity_filtered = angular_velocitytemp;
|
||||
}
|
||||
|
||||
void OptiTrackFeedBackRigidBody::GetState(rigidbody_state& state)
|
||||
{
|
||||
state.time_stamp = pose[1].t;
|
||||
state.Position = pose[1].Position;
|
||||
state.V_I = velocity_filtered;
|
||||
state.Omega_BI = angular_velocity_filtered;
|
||||
Hatmap(state.Omega_BI,state.Omega_Cross);
|
||||
state.R_IB = pose[1].R_IB;
|
||||
state.R_BI = pose[1].R_BI;
|
||||
double euler_temp[3];
|
||||
GetEulerAngleFromQuaterion_NormalConvention(euler_temp);
|
||||
state.Euler(0) = euler_temp[0];// euler angle
|
||||
state.Euler(1) = euler_temp[1];// euler angle
|
||||
state.Euler(2) = euler_temp[2];// euler angle
|
||||
state.quaterion(0) = pose[1].q0;
|
||||
state.quaterion(1) = pose[1].q1;
|
||||
state.quaterion(2) = pose[1].q2;
|
||||
state.quaterion(3) = pose[1].q3;
|
||||
}
|
||||
void OptiTrackFeedBackRigidBody::GetRaWVelocity(Vector3d& linear_velocity,Vector3d& angular_velocity)
|
||||
{
|
||||
linear_velocity = velocity_raw[linear_velocity_window-1];// return the filtered velocity
|
||||
angular_velocity = angular_velocity_raw[angular_velocity_window-1];// return the filtered velocity
|
||||
}
|
||||
void OptiTrackFeedBackRigidBody::SetZeroVelocity()
|
||||
{
|
||||
for(int i =0;i<linear_velocity_window;i++)
|
||||
{
|
||||
velocity_raw[i](0)=0;
|
||||
velocity_raw[i](1)=0;
|
||||
velocity_raw[i](2)=0;
|
||||
}
|
||||
for(int i =0;i<angular_velocity_window;i++)
|
||||
{
|
||||
angular_velocity_raw[i](0)=0;
|
||||
angular_velocity_raw[i](1)=0;
|
||||
angular_velocity_raw[i](2)=0;
|
||||
}
|
||||
velocity_filtered(0)=0;
|
||||
velocity_filtered(1)=0;
|
||||
velocity_filtered(2)=0;
|
||||
angular_velocity_filtered(0) =0;
|
||||
angular_velocity_filtered(1) =0;
|
||||
angular_velocity_filtered(2) =0;
|
||||
}
|
||||
|
||||
void OptiTrackFeedBackRigidBody::RosWhileLoopRun()
|
||||
{
|
||||
if(OptiTrackFlag==1)
|
||||
{// update the velocity only when there is OptiTrack feedback
|
||||
CalculateVelocityFromPose();
|
||||
FeedbackState=1;
|
||||
}else{
|
||||
// if the optitrack measurements no longer feedback, when the pose update will stop and we only return 0 velocity
|
||||
SetZeroVelocity();
|
||||
FeedbackState=0;
|
||||
}
|
||||
|
||||
OptiTrackFlag = 0;// reset the feedback flag to 0
|
||||
}
|
||||
int OptiTrackFeedBackRigidBody::GetOptiTrackState()
|
||||
{
|
||||
if (FeedbackState==1) {
|
||||
ROS_INFO("OptiTrack:Normal");
|
||||
}else{
|
||||
ROS_INFO("OptiTrack:No FeedBack");
|
||||
}
|
||||
ROS_INFO("Linear Velocity Filter Window Size is [%d]",linear_velocity_window);
|
||||
ROS_INFO("Angular Velocity Filter Window Size is [%d]",angular_velocity_window);
|
||||
return FeedbackState;
|
||||
}
|
||||
void OptiTrackFeedBackRigidBody::GetEulerAngleFromQuaterion_NormalConvention(double (&eulerangle)[3])
|
||||
{
|
||||
|
||||
|
||||
/* Normal means the following https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
|
||||
*/
|
||||
// eulerangle[0] = atan2(2.0 * (quat[3] * quat[2] + quat[0] * quat[1]), 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2]));
|
||||
// eulerangle[1] = asin(2.0 * (quat[2] * quat[0] - quat[3] * quat[1]));
|
||||
// eulerangle[2] = atan2(2.0 * (quat[3] * quat[0] + quat[1] * quat[2]), 1.0 - 2.0 * (quat[2] * quat[2] + quat[3] * quat[3]));
|
||||
|
||||
|
||||
// roll (x-axis rotation)
|
||||
double sinr_cosp = +2.0 * (pose[1].q0 * pose[1].q1 + pose[1].q2 * pose[1].q3);
|
||||
double cosr_cosp = +1.0 - 2.0 * (pose[1].q1 * pose[1].q1 +pose[1].q2 * pose[1].q2);
|
||||
double roll = atan2(sinr_cosp, cosr_cosp);
|
||||
|
||||
// pitch (y-axis rotation)
|
||||
double sinp = +2.0 * (pose[1].q0 * pose[1].q2 - pose[1].q3 * pose[1].q1);
|
||||
double pitch;
|
||||
if (fabs(sinp) >= 1)
|
||||
pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
|
||||
else
|
||||
pitch = asin(sinp);
|
||||
|
||||
// yaw (z-axis rotation)
|
||||
double siny_cosp = +2.0 * (pose[1].q0 * pose[1].q3 + pose[1].q1 * pose[1].q2);
|
||||
double cosy_cosp = +1.0 - 2.0 * (pose[1].q2 * pose[1].q2 + pose[1].q3 * pose[1].q3);
|
||||
double yaw = atan2(siny_cosp, cosy_cosp);
|
||||
//double yaw = atan2(2.0 * (dronepose[1].q3 * dronepose[1].q0 + dronepose[1].q1 * dronepose[1].q2), -1.0 + 2.0 * (dronepose[1].q0 * dronepose[1].q0 + dronepose[1].q1 * dronepose[1].q1));
|
||||
eulerangle[0] = roll;
|
||||
eulerangle[1] = pitch;
|
||||
eulerangle[2] = yaw;
|
||||
|
||||
}
|
||||
|
||||
void OptiTrackFeedBackRigidBody::GetEulerAngleFromQuaterion_OptiTrackYUpConvention(double (&eulerangle)[3])
|
||||
{
|
||||
|
||||
// OptiTrack gives a quaternion with q2 and q3 flipped. (and sign flipped for q3)
|
||||
// roll (x-axis rotation)
|
||||
double sinr_cosp = +2.0 * (pose[1].q0 * pose[1].q1 + pose[1].q3 * pose[1].q2);
|
||||
double cosr_cosp = +1.0 - 2.0 * (pose[1].q1 * pose[1].q1 +pose[1].q3 * pose[1].q3);
|
||||
double roll = atan2(sinr_cosp, cosr_cosp);
|
||||
|
||||
// pitch (y-axis rotation)
|
||||
double sinp = +2.0 * (pose[1].q0 * pose[1].q3 - pose[1].q2 * pose[1].q1);
|
||||
double pitch;
|
||||
if (fabs(sinp) >= 1)
|
||||
pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
|
||||
else
|
||||
pitch = asin(sinp);
|
||||
|
||||
// yaw (z-axis rotation)
|
||||
double siny_cosp = +2.0 * (pose[1].q0 * pose[1].q2 + pose[1].q1 * pose[1].q3);
|
||||
double cosy_cosp = +1.0 - 2.0 * (pose[1].q2 * pose[1].q2 + pose[1].q3 * pose[1].q3);
|
||||
double yaw = atan2(siny_cosp, cosy_cosp);
|
||||
eulerangle[0] = roll;
|
||||
eulerangle[1] = pitch;
|
||||
eulerangle[2] = yaw;
|
||||
|
||||
}
|
||||
|
||||
void OptiTrackFeedBackRigidBody::OptiTrackCallback(const geometry_msgs::PoseStamped& msg)
|
||||
{
|
||||
// must use head information to distiguish the correct
|
||||
OptiTrackdata = msg; // update optitrack data
|
||||
OptiTrackFlag = 1;// signal a new measurement feed has been revcieved.
|
||||
}
|
||||
|
||||
OptiTrackFeedBackRigidBody::~OptiTrackFeedBackRigidBody()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void OptiTrackFeedBackRigidBody::Veemap(Matrix3d& cross_matrix, Vector3d& vector)
|
||||
{
|
||||
vector(0) = -cross_matrix(1,2);
|
||||
vector(1) = cross_matrix(0,2);
|
||||
vector(2) = -cross_matrix(0,1);
|
||||
}
|
||||
void OptiTrackFeedBackRigidBody::Hatmap(Vector3d& vector, Matrix3d& cross_matrix)
|
||||
{
|
||||
/*
|
||||
|
||||
r^x = [0 -r3 r2;
|
||||
r3 0 -r1;
|
||||
-r2 r1 0]
|
||||
*/
|
||||
|
||||
cross_matrix(0,0) = 0.0;
|
||||
cross_matrix(0,1) = - vector(2);
|
||||
cross_matrix(0,2) = vector(1);
|
||||
|
||||
cross_matrix(1,0) = vector(2);
|
||||
cross_matrix(1,1) = 0.0;
|
||||
cross_matrix(1,2) = - vector(0);
|
||||
|
||||
cross_matrix(2,0) = - vector(1);
|
||||
cross_matrix(2,1) = vector(0);
|
||||
cross_matrix(2,2) = 0.0;
|
||||
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue